diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8a145b18..b26d6b80 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,17 +13,23 @@ package frc.robot; +import static edu.wpi.first.units.BaseUnits.Voltage; +import static edu.wpi.first.units.Units.*; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.commands.DriveCommands; -import frc.robot.commands.FeedForwardCharacterization; import frc.robot.subsystems.drive.*; import frc.robot.subsystems.feeder.Feeder; import frc.robot.subsystems.feeder.FeederIO; @@ -114,19 +120,28 @@ public RobotContainer() { autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); // Set up feedforward characterization - autoChooser.addOption( - "Drive FF Characterization", - new FeedForwardCharacterization( - drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity)); - autoChooser.addOption( - "Flywheel FF Characterization", - new FeedForwardCharacterization( - flywheel, flywheel::runVolts, flywheel::getCharacterizationVelocity)); + // autoChooser.addOption( + // "Drive FF Characterization", + // new FeedForwardCharacterization( + // drive, drive::runCharacterizationVolts, drive::getCharacterizationDriveVelocity)); + // autoChooser.addOption( + // "Flywheel FF Characterization", + // new FeedForwardCharacterization( + // flywheel, flywheel::runVolts, flywheel::getCharacterizationDriveVelocity)); // Configure the button bindings configureButtonBindings(); } + public static SysIDMode sysIDMode = SysIDMode.Disabled; + + enum SysIDMode { + Disabled, + DriveMotors, + TurnMotors, + EverythingElse + } + /** * Use this method to define your button->command mappings. Buttons can be created by * instantiating a {@link GenericHID} or one of its subclasses ({@link @@ -134,27 +149,84 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - drive.setDefaultCommand( - DriveCommands.joystickDrive( - drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - controller - .b() - .onTrue( - Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - drive) - .ignoringDisable(true)); - controller - .a() - .whileTrue( - Commands.startEnd( - () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)); + switch (sysIDMode) { + case Disabled: + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + controller + .b() + .onTrue( + Commands.runOnce( + () -> + drive.setPose( + new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + drive) + .ignoringDisable(true)); + controller + .a() + .whileTrue( + Commands.startEnd( + () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)); + break; + case DriveMotors: + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + var drivetrainDriveSysID = + new SysIdRoutine( + new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)), + new Mechanism( + drive::runCharacterizationVolts, + drive::populateDriveCharacterizationData, + drive, + "DrivetrainDriveMotors")); + controller + .x() + .whileTrue(drivetrainDriveSysID.dynamic(Direction.kForward)) + .onFalse(Commands.runOnce(drive::stopWithX, drive)); + controller + .y() + .whileTrue(drivetrainDriveSysID.dynamic(Direction.kReverse)) + .onFalse(Commands.runOnce(drive::stopWithX, drive)); + controller + .a() + .whileTrue(drivetrainDriveSysID.quasistatic(Direction.kForward).withTimeout(2.0)) + .onFalse(Commands.runOnce(drive::stopWithX, drive)); + controller + .b() + .whileTrue(drivetrainDriveSysID.quasistatic(Direction.kReverse).withTimeout(2.0)) + .onFalse(Commands.runOnce(drive::stopWithX, drive)); + break; + case TurnMotors: + var drivetrainTurnSysID = + new SysIdRoutine( + new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)), + new Mechanism( + drive::runCharacterizationVolts, + drive::populateTurnCharacterizationData, + drive, + "DrivetrainDriveMotors")); + controller + .x() + .whileTrue(drivetrainTurnSysID.dynamic(Direction.kForward) + .andThen(drivetrainTurnSysID.dynamic(Direction.kReverse)) + .andThen(drivetrainTurnSysID.quasistatic(Direction.kForward)) + .andThen(drivetrainTurnSysID.quasistatic(Direction.kReverse)) + .andThen(Commands.runOnce(drive::stopWithX, drive)) + ) + .onFalse(Commands.runOnce(drive::stopWithX, drive)); + break; + case EverythingElse: + break; + } } /** diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 196aecde..64613fda 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -13,6 +13,8 @@ package frc.robot.subsystems.drive; +import static edu.wpi.first.units.Units.*; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; @@ -27,8 +29,10 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.*; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.LocalADStarAK; import org.littletonrobotics.junction.AutoLogOutput; @@ -168,9 +172,9 @@ public void stopWithX() { } /** Runs forwards at the commanded voltage. */ - public void runCharacterizationVolts(double volts) { + public void runCharacterizationVolts(Measure voltageMeasure) { for (int i = 0; i < 4; i++) { - modules[i].runCharacterization(volts); + modules[i].runCharacterization(voltageMeasure.in(Volts)); } } @@ -178,11 +182,50 @@ public void runCharacterizationVolts(double volts) { public double getCharacterizationVelocity() { double driveVelocityAverage = 0.0; for (var module : modules) { - driveVelocityAverage += module.getCharacterizationVelocity(); + driveVelocityAverage += module.getCharacterizationVelocityRadPerSec(); } return driveVelocityAverage / 4.0; } + /** Returns the average drive velocity in radians/sec. */ + public void populateDriveCharacterizationData(SysIdRoutineLog routineLog) { + Measure> driveVelocityAverage = RadiansPerSecond.zero(); + Measure drivePositionAverage = Radians.zero(); + + for (var module : modules) { + var motor = routineLog.motor("DriveMotor #" + module.getIndex()); + var angularPosition = module.getCharacterizationDrivePosition(); + var angularVelocity = module.getCharacterizationDriveVelocity(); + motor.angularPosition(angularPosition); + motor.angularVelocity(angularVelocity); + + drivePositionAverage = drivePositionAverage.plus(angularPosition); + driveVelocityAverage = driveVelocityAverage.plus(angularVelocity); + } + var averageDriveMotor = routineLog.motor("Average DriveMotor"); + averageDriveMotor.angularVelocity(driveVelocityAverage.divide(4.0)); + averageDriveMotor.angularPosition(drivePositionAverage.divide(4.0)); + } + + public void populateTurnCharacterizationData(SysIdRoutineLog routineLog) { + Measure> driveVelocityAverage = RadiansPerSecond.zero(); + Measure drivePositionAverage = Radians.zero(); + + for (var module : modules) { + var motor = routineLog.motor("TurnMotor #" + module.getIndex()); + var angularPosition = module.getCharacterizationTurnPosition(); + var angularVelocity = module.getCharacterizationTurnVelocity(); + motor.angularPosition(angularPosition); + motor.angularVelocity(angularVelocity); + + driveVelocityAverage = driveVelocityAverage.plus(angularVelocity); + drivePositionAverage = drivePositionAverage.plus(angularPosition); + } + var averageDriveMotor = routineLog.motor("Average TurnMotor"); + averageDriveMotor.angularVelocity(driveVelocityAverage.divide(4.0)); + averageDriveMotor.angularPosition(drivePositionAverage.divide(4.0)); + } + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 6996faa7..fff7b785 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -19,6 +19,9 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import frc.robot.Constants; import org.littletonrobotics.junction.Logger; @@ -177,7 +180,28 @@ public SwerveModuleState getState() { } /** Returns the drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { + public double getCharacterizationVelocityRadPerSec() { return inputs.driveVelocityRadPerSec; } + + /** Returns the drive velocity unitless. */ + public Measure getCharacterizationDrivePosition() { + return edu.wpi.first.units.Units.Radians.of(inputs.drivePositionRad); + } + /** Returns the turn velocity unitless. */ + public Measure getCharacterizationTurnPosition() { + return edu.wpi.first.units.Units.Radians.of(inputs.turnPosition.getRadians()); + } + /** Returns the drive velocity unitless. */ + public Measure> getCharacterizationDriveVelocity() { + return edu.wpi.first.units.Units.RadiansPerSecond.of(inputs.driveVelocityRadPerSec); + } + /** Returns the turn velocity unitless. */ + public Measure> getCharacterizationTurnVelocity() { + return edu.wpi.first.units.Units.RadiansPerSecond.of(inputs.turnVelocityRadPerSec); + } + + public int getIndex() { + return index; + } }