From b96a0a926eb57f8b6d57f0b00621ee93bf9314fe Mon Sep 17 00:00:00 2001 From: Jeremy Nicholl Date: Mon, 18 Nov 2024 14:23:58 -0500 Subject: [PATCH 1/2] Create a stub for sys id work --- src/main/java/frc/robot/RobotContainer.java | 5 ++ .../robot/subsystems/DrivebaseSubsystem.java | 32 ++++++++++- .../java/frc/robot/sysid/SysIdRunner.java | 55 +++++++++++++++++++ 3 files changed, 90 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/sysid/SysIdRunner.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c83c01e..3d5f9ba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.Constants.Gamepads; import frc.robot.commands.DriveCommand; import frc.robot.commands.DriverPullupIntake; @@ -46,6 +47,7 @@ import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.LedSubsystem; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.sysid.SysIdRunner; enum AutoOption { driveForward, @@ -65,6 +67,7 @@ enum AutoOption { sideLongAmpBlue, sideLongAmpRed, pathPlannerExample, + sysIdSequence } /** @@ -140,6 +143,7 @@ public RobotContainer() { m_autoChooser.addOption("Side Long Source Blue", AutoOption.sideLongSourceBlue); m_autoChooser.addOption("Side Long Amp Blue", AutoOption.sideLongAmpBlue); m_autoChooser.addOption("Side Long Amp Red", AutoOption.sideLongAmpRed); + m_autoChooser.addOption("SysId", AutoOption.sysIdSequence); dashboard.add("Auto Routine", m_autoChooser).withSize(2, 1).withPosition(8, 0); @@ -320,6 +324,7 @@ public Command getAutonomousCommand() { case sideLongAmpBlue -> new sideLongAmpBlue(m_drivebase, m_shooter, m_intake, m_led, waittime.getDouble(0)); case sideLongAmpRed -> new sideLongAmpRed(m_drivebase, m_shooter, m_intake, m_led, waittime.getDouble(0)); case pathPlannerExample -> new PathPlannerAuto("New Auto"); + case sysIdSequence -> new SysIdRunner(m_drivebase).sequence(); }; } } diff --git a/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java index 92ed4b5..8014c13 100644 --- a/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -27,9 +28,14 @@ import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.networktables.GenericEntry; - +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.smartdashboard.Field2d; - +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import frc.robot.Constants; import frc.robot.Constants.Drive; import frc.robot.sim.DrivebaseSim; @@ -463,4 +469,26 @@ public void simulationPeriodic() { m_drivebaseSim.update(period); } + + public void sysIdDrive(Measure voltage) { + // FIXME: Unclear whether we should set the speed control + // or the voltage control. For simulator, the voltage control + // does not work. + if (m_isSimulation) { + m_leftDriveMotorF.set(voltage.magnitude() / RobotController.getBatteryVoltage()); + m_rightDriveMotorF.set(voltage.magnitude() / RobotController.getBatteryVoltage()); + } else { + m_leftDriveMotorF.setVoltage(voltage.magnitude()); + m_rightDriveMotorF.setVoltage(voltage.magnitude()); + } + } + + public void sysIdUpdateMeasures(boolean isRight, MutableMeasure appliedVoltage, + MutableMeasure distance, MutableMeasure> velocity) { + appliedVoltage.mut_replace( + (isRight ? m_rightDriveMotorF : m_leftDriveMotorF).get() * RobotController.getBatteryVoltage(), Units.Volts); + var encoder = isRight ? m_rightDriveEncoder : m_leftDriveEncoder; + distance.mut_replace(encoder.getPosition(), Units.Meters); + velocity.mut_replace(encoder.getVelocity(), Units.MetersPerSecond); + } } diff --git a/src/main/java/frc/robot/sysid/SysIdRunner.java b/src/main/java/frc/robot/sysid/SysIdRunner.java new file mode 100644 index 0000000..b23e616 --- /dev/null +++ b/src/main/java/frc/robot/sysid/SysIdRunner.java @@ -0,0 +1,55 @@ +package frc.robot.sysid; + +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.subsystems.DrivebaseSubsystem; + +public class SysIdRunner { + private SysIdRoutine m_routine; + private final MutableMeasure m_appliedVoltage = MutableMeasure.zero(Units.Volts); + private final MutableMeasure m_distance = MutableMeasure.zero(Units.Meters); + private final MutableMeasure> m_velocity = MutableMeasure.zero(Units.MetersPerSecond); + + public SysIdRunner(DrivebaseSubsystem drivebase) { + // Default config is 1 volt/sec ramp rate and 7 volt step voltage. + var config = new SysIdRoutine.Config(); + var mechanism = new SysIdRoutine.Mechanism(voltage -> { + drivebase.sysIdDrive(voltage); + }, log -> { + drivebase.sysIdUpdateMeasures(false, m_appliedVoltage, m_distance, m_velocity); + log.motor("drive-left") + .voltage(m_appliedVoltage) + .linearPosition(m_distance) + .linearVelocity(m_velocity); + drivebase.sysIdUpdateMeasures(true, m_appliedVoltage, m_distance, m_velocity); + log.motor("drive-right") + .voltage(m_appliedVoltage) + .linearPosition(m_distance) + .linearVelocity(m_velocity); + }, drivebase); + m_routine = new SysIdRoutine(config, mechanism); + } + + public Command quasistatic(SysIdRoutine.Direction direction) { + return m_routine.quasistatic(direction); + } + + public Command dynamic(SysIdRoutine.Direction direction) { + return m_routine.dynamic(direction); + } + + public Command sequence() { + return new SequentialCommandGroup( + this.quasistatic(Direction.kForward), + this.quasistatic(Direction.kReverse), + this.dynamic(Direction.kForward), + this.dynamic(Direction.kReverse)); + } +} From 7586e59e248f7ce20fdd044ed7c7c0b7e252dc5f Mon Sep 17 00:00:00 2001 From: Jeremy Nicholl Date: Mon, 18 Nov 2024 15:28:09 -0500 Subject: [PATCH 2/2] Disable safety check when running sysid on simulator --- src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java index 8014c13..9a049bb 100644 --- a/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java @@ -475,6 +475,9 @@ public void sysIdDrive(Measure voltage) { // or the voltage control. For simulator, the voltage control // does not work. if (m_isSimulation) { + // WARNING: Drive safety disabled when running sysid in simulator to avoid + // the check stopping the motors prematurely. + m_drive.setSafetyEnabled(false); m_leftDriveMotorF.set(voltage.magnitude() / RobotController.getBatteryVoltage()); m_rightDriveMotorF.set(voltage.magnitude() / RobotController.getBatteryVoltage()); } else {