diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0799441..6ccd7f2 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -7,12 +7,14 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; @@ -23,12 +25,14 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.AdvancedSubsystem; import frc.lib.CTREUtil; import frc.lib.FaultLogger; import frc.robot.Constants; import frc.robot.Constants.IntakeConstants; import frc.robot.Robot; +import frc.robot.utils.SysId; public class Intake extends AdvancedSubsystem { private final Mechanism2d _mech = new Mechanism2d(1.85, 1); @@ -44,9 +48,24 @@ public class Intake extends AdvancedSubsystem { private final MotionMagicVoltage _actuatorPositionSetter = new MotionMagicVoltage(0); private final VelocityVoltage _feedVelocitySetter = new VelocityVoltage(0); + private final VoltageOut _actuatorVoltageSetter = new VoltageOut(0); + private final VoltageOut _feedVoltageSetter = new VoltageOut(0); + private final StatusSignal _actuatorPositionGetter = _actuatorMotor.getPosition(); private final StatusSignal _feedVelocityGetter = _feedMotor.getVelocity(); + private final SysIdRoutine _actuatorRoutine = + new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Voltage volts) -> setActuatorVoltage(volts.in(Volts)), null, this)); + + private final SysIdRoutine _feedRoutine = + new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Voltage volts) -> setFeedVoltage(volts.in(Volts)), null, this)); + private SingleJointedArmSim _actuatorSim; private double _lastSimTime; @@ -80,6 +99,9 @@ public Intake() { FaultLogger.register(_feedMotor); FaultLogger.register(_actuatorMotor); + SysId.displayRoutine("Actuator", _actuatorRoutine); + SysId.displayRoutine("Intake Feed", _feedRoutine); + if (Robot.isSimulation()) { _actuatorSim = new SingleJointedArmSim( @@ -173,6 +195,14 @@ public Command outtake() { .withName("Outtake"); } + private void setActuatorVoltage(double volts) { + _actuatorMotor.setControl(_actuatorVoltageSetter.withOutput(volts)); + } + + private void setFeedVoltage(double volts) { + _feedMotor.setControl(_feedVoltageSetter.withOutput(volts)); + } + @Override public void periodic() { super.periodic(); diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java index b49592e..36098d4 100644 --- a/src/main/java/frc/robot/subsystems/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/Manipulator.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.BooleanEntry; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; @@ -26,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.AdvancedSubsystem; import frc.lib.CTREUtil; import frc.lib.FaultLogger; @@ -33,6 +35,7 @@ import frc.robot.Constants; import frc.robot.Constants.ManipulatorConstants; import frc.robot.Robot; +import frc.robot.utils.SysId; import java.util.function.Consumer; public class Manipulator extends AdvancedSubsystem { @@ -62,6 +65,12 @@ public class Manipulator extends AdvancedSubsystem { private final StatusSignal _feedVelocityGetter = _leftMotor.getVelocity(); + private final SysIdRoutine _feedRoutine = + new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Voltage volts) -> setFeedVoltage(volts.in(Volts)), null, this)); + private DIOSim _coralBeamSim; private DIOSim _algaeBeamSim; @@ -89,6 +98,8 @@ public Manipulator(Consumer currentPieceSetter) { FaultLogger.register(_leftMotor); FaultLogger.register(_rightMotor); + SysId.displayRoutine("Manipulator Feed", _feedRoutine); + if (Robot.isSimulation()) { _coralBeamSim = new DIOSim(_coralBeam); _algaeBeamSim = new DIOSim(_algaeBeam); @@ -244,6 +255,10 @@ public Command pulse() { .until(() -> _coralEvent.rising().getAsBoolean()); } + private void setFeedVoltage(double volts) { + _leftMotor.setControl(_feedVoltageSetter.withOutput(volts)); + } + @Override public void periodic() { super.periodic(); diff --git a/src/main/java/frc/robot/subsystems/Serializer.java b/src/main/java/frc/robot/subsystems/Serializer.java index e25570d..7027182 100644 --- a/src/main/java/frc/robot/subsystems/Serializer.java +++ b/src/main/java/frc/robot/subsystems/Serializer.java @@ -1,19 +1,23 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.BooleanEntry; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.simulation.DIOSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.AdvancedSubsystem; import frc.lib.CTREUtil; import frc.lib.FaultLogger; @@ -21,6 +25,7 @@ import frc.robot.Constants.SerializerConstants; import frc.robot.Robot; import frc.robot.subsystems.Manipulator.Piece; +import frc.robot.utils.SysId; import java.util.function.Consumer; public class Serializer extends AdvancedSubsystem { @@ -38,6 +43,14 @@ public class Serializer extends AdvancedSubsystem { private final VelocityVoltage _feedVelocitySetter = new VelocityVoltage(0); private final StatusSignal _feedVelocityGetter = _feedMotor.getVelocity(); + private final VoltageOut _feedVoltageSetter = new VoltageOut(0); + + private final SysIdRoutine _feedRoutine = + new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Voltage volts) -> setFeedVoltage(volts.in(Volts)), null, this)); + private final Consumer _currentPieceSetter; public Serializer(Consumer currentPieceSetter) { @@ -48,6 +61,8 @@ public Serializer(Consumer currentPieceSetter) { _frontBeam = new DigitalInput(SerializerConstants.frontBeamPort); _backBeam = new DigitalInput(SerializerConstants.backBeamPort); + SysId.displayRoutine("Serializer Feed", _feedRoutine); + if (Robot.isSimulation()) { _frontBeamSim = new DIOSim(_frontBeam); _backBeamSim = new DIOSim(_backBeam); @@ -122,6 +137,10 @@ public Command inversePassoff() { .withName("Inverse Passoff"); } + private void setFeedVoltage(double volts) { + _feedMotor.setControl(_feedVoltageSetter.withOutput(volts)); + } + @Override public void periodic() { super.periodic(); diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java index 0c79f25..8a87145 100644 --- a/src/main/java/frc/robot/subsystems/Wristevator.java +++ b/src/main/java/frc/robot/subsystems/Wristevator.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotionMagicIsRunningValue; import dev.doglog.DogLog; @@ -24,6 +25,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; @@ -39,6 +41,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.AdvancedSubsystem; import frc.lib.CTREUtil; import frc.lib.FaultLogger; @@ -48,6 +51,7 @@ import frc.robot.Constants.WristevatorConstants.Preset; import frc.robot.Constants.WristevatorConstants.Setpoint; import frc.robot.Robot; +import frc.robot.utils.SysId; import java.util.function.DoubleSupplier; public class Wristevator extends AdvancedSubsystem { @@ -68,14 +72,17 @@ public class Wristevator extends AdvancedSubsystem { private final TalonFX _wristMotor = new TalonFX(WristevatorConstants.wristMotorId, Constants.canivore); - private final StatusSignal _heightGetter = _leftMotor.getPosition(); - private final StatusSignal _angleGetter = _wristMotor.getPosition(); - private final DynamicMotionMagicVoltage _heightSetter = new DynamicMotionMagicVoltage(HOME.getHeight().in(Rotations), 0, 0, 0); private final DynamicMotionMagicVoltage _angleSetter = new DynamicMotionMagicVoltage(HOME.getAngle().in(Rotations), 0, 0, 0); + private final VoltageOut _elevatorVoltageSetter = new VoltageOut(0); + private final VoltageOut _wristVoltageSetter = new VoltageOut(0); + + private final StatusSignal _heightGetter = _leftMotor.getPosition(); + private final StatusSignal _angleGetter = _wristMotor.getPosition(); + private final StatusSignal _elevatorReference = _leftMotor.getClosedLoopReference(); private final StatusSignal _elevatorReferenceSlope = _leftMotor.getClosedLoopReferenceSlope(); @@ -84,6 +91,18 @@ public class Wristevator extends AdvancedSubsystem { private final StatusSignal _wristReferenceSlope = _wristMotor.getClosedLoopReferenceSlope(); + private final SysIdRoutine _elevatorRoutine = + new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Voltage volts) -> setElevatorVoltage(volts.in(Volts)), null, this)); + + private final SysIdRoutine _wristRoutine = + new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Voltage volts) -> setWristVoltage(volts.in(Volts)), null, this)); + @Logged(name = "Motion Magic Timestamp Threshold") private double _motionMagicTimestampThreshold = 0; @@ -202,6 +221,9 @@ public Wristevator() { DogLog.log("Wristevator/Presets", presets); DogLog.log("Wristevator/Intermediates", intermediates); + SysId.displayRoutine("Elevator", _elevatorRoutine); + SysId.displayRoutine("Wrist", _wristRoutine); + if (Robot.isSimulation()) { _homeSwitchSim = new DIOSim(_homeSwitch); @@ -544,6 +566,14 @@ public Command setSpeeds(DoubleSupplier elevatorSpeed, DoubleSupplier wristSpeed .withName("Set Speeds"); } + private void setElevatorVoltage(double volts) { + _leftMotor.setControl(_elevatorVoltageSetter.withOutput(volts)); + } + + private void setWristVoltage(double volts) { + _wristMotor.setControl(_wristVoltageSetter.withOutput(volts)); + } + @Override public void periodic() { super.periodic();