diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a390c3fb..b8f702af 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,9 +22,9 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final Mode currentMode = Mode.SIM; + public static final Mode currentMode = Mode.REAL; - public static enum Mode { + public enum Mode { /** Running on a real robot. */ REAL, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b26d6b80..d99ca312 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,7 +37,6 @@ import frc.robot.subsystems.flywheel.Flywheel; import frc.robot.subsystems.flywheel.FlywheelIO; import frc.robot.subsystems.flywheel.FlywheelIOSim; -import frc.robot.subsystems.flywheel.FlywheelIOSparkMax; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -69,11 +68,11 @@ public RobotContainer() { drive = new Drive( new GyroIOPigeon2(), - new ModuleIOSparkMax(0), - new ModuleIOSparkMax(1), - new ModuleIOSparkMax(2), - new ModuleIOSparkMax(3)); - flywheel = new Flywheel(new FlywheelIOSparkMax()); + new ModuleIOTalonFX(0), + new ModuleIOTalonFX(1), + new ModuleIOTalonFX(2), + new ModuleIOTalonFX(3)); + flywheel = new Flywheel(new FlywheelIOSim()); feeder = new Feeder(new FeederIO() {}); // drive = new Drive( // new GyroIOPigeon2(), @@ -133,7 +132,7 @@ public RobotContainer() { configureButtonBindings(); } - public static SysIDMode sysIDMode = SysIDMode.Disabled; + static SysIDMode sysIDMode = SysIDMode.DriveMotors; enum SysIDMode { Disabled, @@ -171,7 +170,9 @@ private void configureButtonBindings() { .a() .whileTrue( Commands.startEnd( - () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)); + () -> flywheel.runVelocity(flywheelSpeedInput.get()), + flywheel::stop, + flywheel)); break; case DriveMotors: drive.setDefaultCommand( @@ -181,48 +182,49 @@ private void configureButtonBindings() { () -> -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")); + 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)); + .x() + .whileTrue(drivetrainDriveSysID.quasistatic(Direction.kForward)) + .onFalse(Commands.runOnce(drive::stopWithX, drive)); controller - .y() - .whileTrue(drivetrainDriveSysID.dynamic(Direction.kReverse)) - .onFalse(Commands.runOnce(drive::stopWithX, drive)); + .y() + .whileTrue(drivetrainDriveSysID.quasistatic(Direction.kReverse)) + .onFalse(Commands.runOnce(drive::stopWithX, drive)); controller - .a() - .whileTrue(drivetrainDriveSysID.quasistatic(Direction.kForward).withTimeout(2.0)) - .onFalse(Commands.runOnce(drive::stopWithX, drive)); + .a() + .whileTrue(drivetrainDriveSysID.dynamic(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)); + .b() + .whileTrue(drivetrainDriveSysID.dynamic(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")); + 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)); + .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 c38edfd2..56f36240 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -50,6 +50,7 @@ public class Drive extends SubsystemBase { private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR + @SuppressWarnings("FieldMayBeFinal") private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Pose2d pose = new Pose2d(); private Rotation2d lastGyroRotation = new Rotation2d(); @@ -191,39 +192,49 @@ public double getCharacterizationVelocity() { public void populateDriveCharacterizationData(SysIdRoutineLog routineLog) { Measure> driveVelocityAverage = RadiansPerSecond.zero(); Measure drivePositionAverage = Radians.zero(); + Measure driveVoltageAverage = Volts.zero(); for (var module : modules) { var motor = routineLog.motor("DriveMotor #" + module.getIndex()); var angularPosition = module.getCharacterizationDrivePosition(); var angularVelocity = module.getCharacterizationDriveVelocity(); + var voltage = module.getCharacterizationDriveAppliedVoltage(); motor.angularPosition(angularPosition); motor.angularVelocity(angularVelocity); + motor.voltage(voltage); drivePositionAverage = drivePositionAverage.plus(angularPosition); driveVelocityAverage = driveVelocityAverage.plus(angularVelocity); + driveVoltageAverage = driveVoltageAverage.plus(voltage); } var averageDriveMotor = routineLog.motor("Average DriveMotor"); averageDriveMotor.angularVelocity(driveVelocityAverage.divide(4.0)); averageDriveMotor.angularPosition(drivePositionAverage.divide(4.0)); + averageDriveMotor.voltage(driveVoltageAverage.divide(4.0)); } public void populateTurnCharacterizationData(SysIdRoutineLog routineLog) { Measure> driveVelocityAverage = RadiansPerSecond.zero(); Measure drivePositionAverage = Radians.zero(); + Measure driveVoltageAverage = Volts.zero(); for (var module : modules) { var motor = routineLog.motor("TurnMotor #" + module.getIndex()); var angularPosition = module.getCharacterizationTurnPosition(); var angularVelocity = module.getCharacterizationTurnVelocity(); + var voltage = module.getCharacterizationDriveAppliedVoltage(); motor.angularPosition(angularPosition); motor.angularVelocity(angularVelocity); + motor.voltage(voltage); driveVelocityAverage = driveVelocityAverage.plus(angularVelocity); drivePositionAverage = drivePositionAverage.plus(angularPosition); + driveVoltageAverage = driveVoltageAverage.plus(voltage); } var averageDriveMotor = routineLog.motor("Average TurnMotor"); averageDriveMotor.angularVelocity(driveVelocityAverage.divide(4.0)); averageDriveMotor.angularPosition(drivePositionAverage.divide(4.0)); + averageDriveMotor.voltage(driveVoltageAverage.divide(4.0)); } /** Returns the module states (turn angles and drive velocities) for all of the modules. */ diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index fff7b785..bae9148c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -13,6 +13,8 @@ package frc.robot.subsystems.drive; +import static edu.wpi.first.units.Units.*; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; @@ -22,6 +24,7 @@ import edu.wpi.first.units.Angle; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; import frc.robot.Constants; import org.littletonrobotics.junction.Logger; @@ -48,6 +51,10 @@ public Module(ModuleIO io, int index) { // separate robot with different tuning) switch (Constants.currentMode) { case REAL: + driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + driveFeedback = new PIDController(0.05, 0.0, 0.0); + turnFeedback = new PIDController(7.0, 0.0, 0.0); + break; case REPLAY: driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); driveFeedback = new PIDController(0.05, 0.0, 0.0); @@ -186,19 +193,27 @@ public double getCharacterizationVelocityRadPerSec() { /** Returns the drive velocity unitless. */ public Measure getCharacterizationDrivePosition() { - return edu.wpi.first.units.Units.Radians.of(inputs.drivePositionRad); + return Radians.of(inputs.drivePositionRad); } /** Returns the turn velocity unitless. */ public Measure getCharacterizationTurnPosition() { - return edu.wpi.first.units.Units.Radians.of(inputs.turnPosition.getRadians()); + return Radians.of(inputs.turnPosition.getRadians()); } /** Returns the drive velocity unitless. */ public Measure> getCharacterizationDriveVelocity() { - return edu.wpi.first.units.Units.RadiansPerSecond.of(inputs.driveVelocityRadPerSec); + return RadiansPerSecond.of(inputs.driveVelocityRadPerSec); } /** Returns the turn velocity unitless. */ public Measure> getCharacterizationTurnVelocity() { - return edu.wpi.first.units.Units.RadiansPerSecond.of(inputs.turnVelocityRadPerSec); + return RadiansPerSecond.of(inputs.turnVelocityRadPerSec); + } + + public Measure getCharacterizationDriveAppliedVoltage() { + return Volts.of(inputs.driveAppliedVolts); + } + + public Measure getCharacterizationTurnAppliedVoltage() { + return Volts.of(inputs.turnAppliedVolts); } public int getIndex() {