diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e597f05..dc2de77 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,8 +1,6 @@ package frc.robot; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.lib.Telemetry; import frc.robot.arm.Arm; @@ -16,6 +14,7 @@ import frc.robot.swerve.Swerve; import frc.robot.vision.Vision; +/** Initializes subsystems and commands. */ public class RobotContainer { /** Instance variable for the robot container singleton. */ @@ -63,11 +62,7 @@ private void initializeTelemetry() { private void configureBindings() { swerve.setDefaultCommand(new Drive(driverController)); - driverController.a().whileTrue(swerve.forwards()); - driverController.b().whileTrue(swerve.sideways()); - driverController - .y() - .onTrue(Commands.runOnce(() -> odometry.setRotation(Rotation2d.fromDegrees(0)))); + driverController.y().onTrue(odometry.tare()); driverController.x().whileTrue(swerve.cross()); operatorController.leftBumper().whileTrue(intake.intake()).whileTrue(shooter.intake()); diff --git a/src/main/java/frc/robot/odometry/Odometry.java b/src/main/java/frc/robot/odometry/Odometry.java index 718ace7..f17bc53 100644 --- a/src/main/java/frc/robot/odometry/Odometry.java +++ b/src/main/java/frc/robot/odometry/Odometry.java @@ -8,6 +8,8 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; import frc.lib.Telemetry; import frc.robot.odometry.GyroscopeIO.GyroscopeIOValues; @@ -127,6 +129,15 @@ public void setRotation(Rotation2d rotation) { setPosition(new Pose2d(position.getTranslation(), rotation)); } + /** + * Tares the rotation of the robot. + * + * @return a command that zeroes the rotation of the robot. + */ + public Command tare() { + return Commands.runOnce(() -> setRotation(Rotation2d.fromDegrees(0))); + } + /** * Gets the velocity of the robot on the field. * diff --git a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java index ec728aa..1b06dbb 100644 --- a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java +++ b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java @@ -2,9 +2,12 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import frc.lib.CAN; +import frc.robot.swerve.SwerveConstants.DriveMotorConstants; import frc.robot.swerve.SwerveConstants.MK4iConstants; /** TalonFX drive motor. */ @@ -13,6 +16,9 @@ public abstract class DriveMotorIOTalonFX implements DriveMotorIO { /** Hardware TalonFX. */ protected final TalonFX talonFX; + /** Hardware TalonFX base config. */ + protected final TalonFXConfiguration talonFXBaseConfig; + /** TalonFX's position and velocity status signals. */ protected final StatusSignal positionRotations, velocityRotationsPerSecond; @@ -23,6 +29,13 @@ public abstract class DriveMotorIOTalonFX implements DriveMotorIO { */ public DriveMotorIOTalonFX(CAN driveMotorCAN) { talonFX = new TalonFX(driveMotorCAN.id(), driveMotorCAN.bus()); + talonFXBaseConfig = new TalonFXConfiguration(); + + talonFXBaseConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + talonFXBaseConfig.Feedback.SensorToMechanismRatio = MK4iConstants.DRIVE_GEARING; + + talonFXBaseConfig.CurrentLimits = DriveMotorConstants.CURRENT_LIMITS.asCurrentLimitsConfigs(); positionRotations = talonFX.getPosition(); velocityRotationsPerSecond = talonFX.getVelocity(); diff --git a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java index bbd7e7e..f9118da 100644 --- a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java +++ b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java @@ -1,6 +1,5 @@ package frc.robot.swerve; -import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.controller.PIDController; @@ -33,9 +32,7 @@ public DriveMotorIOTalonFXPID(CAN can) { @Override public void configure() { - TalonFXConfiguration config = SwerveFactory.createDriveMotorConfig(); - - Configurator.configureTalonFX(talonFX.getConfigurator(), config); + Configurator.configureTalonFX(talonFX.getConfigurator(), talonFXBaseConfig); } @Override diff --git a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFX.java b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFX.java index c0f9d96..5322417 100644 --- a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFX.java +++ b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFX.java @@ -2,8 +2,11 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import frc.lib.CAN; +import frc.lib.MotorCurrentLimits; /** TalonFX steer motor. */ public abstract class SteerMotorIOTalonFX implements SteerMotorIO { @@ -11,6 +14,9 @@ public abstract class SteerMotorIOTalonFX implements SteerMotorIO { /** Hardware TalonFX. */ protected final TalonFX talonFX; + /** Hardware TalonFX base config. */ + protected final TalonFXConfiguration talonFXBaseConfig; + /** TalonFX"s position and velocity status signals. */ protected final StatusSignal positionRotations, velocityRotationsPerSecond; @@ -26,6 +32,15 @@ public abstract class SteerMotorIOTalonFX implements SteerMotorIO { */ public SteerMotorIOTalonFX(CAN steerMotorCAN, CAN azimuthEncoderCAN) { talonFX = new TalonFX(steerMotorCAN.id(), steerMotorCAN.bus()); + talonFXBaseConfig = new TalonFXConfiguration(); + + talonFXBaseConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + talonFXBaseConfig.ClosedLoopGeneral.ContinuousWrap = true; + + // TODO Copied from Nutrons 2023 release + talonFXBaseConfig.CurrentLimits = + new MotorCurrentLimits(0.0, 40.0, 3.0, 1.0).asCurrentLimitsConfigs(); positionRotations = talonFX.getPosition(); velocityRotationsPerSecond = talonFX.getVelocity(); diff --git a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXIntegrated.java b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXIntegrated.java index 0363899..3346daa 100644 --- a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXIntegrated.java +++ b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXIntegrated.java @@ -17,18 +17,23 @@ public SteerMotorIOTalonFXIntegrated(CAN steerMotorCAN, CAN azimuthEncoderCAN) { @Override public void configure() { - TalonFXConfiguration config = SwerveFactory.createSteerMotorConfig(); + TalonFXConfiguration talonFXIntegratedConfig = new TalonFXConfiguration(); - config.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING; + // TODO + talonFXIntegratedConfig.deserialize(talonFXBaseConfig.serialize()); - config.Slot0 = SwerveConstants.STEER_PIDF_CONSTANTS.asSlot0Configs(); + talonFXIntegratedConfig.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING; + + talonFXIntegratedConfig.Slot0 = SwerveConstants.STEER_PIDF_CONSTANTS.asSlot0Configs(); // TODO Copied from CTRE swerve library // https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.123 - config.MotionMagic.MotionMagicCruiseVelocity = 100.0 / MK4iConstants.STEER_GEARING; - config.MotionMagic.MotionMagicAcceleration = config.MotionMagic.MotionMagicCruiseVelocity * 10; + talonFXIntegratedConfig.MotionMagic.MotionMagicCruiseVelocity = + 100.0 / MK4iConstants.STEER_GEARING; + talonFXIntegratedConfig.MotionMagic.MotionMagicAcceleration = + talonFXIntegratedConfig.MotionMagic.MotionMagicCruiseVelocity * 10; - Configurator.configureTalonFX(talonFX.getConfigurator(), config); + Configurator.configureTalonFX(talonFX.getConfigurator(), talonFXIntegratedConfig); } @Override diff --git a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java index fae6e4c..175a564 100644 --- a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java +++ b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java @@ -29,11 +29,14 @@ public SteerMotorIOTalonFXPIDF(CAN steerMotorCAN, CAN azimuthEncoderCAN) { @Override public void configure() { - TalonFXConfiguration config = SwerveFactory.createSteerMotorConfig(); + TalonFXConfiguration talonFXPIDFConfig = new TalonFXConfiguration(); - config.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING; + // TODO + talonFXPIDFConfig.deserialize(talonFXBaseConfig.serialize()); - Configurator.configureTalonFX(talonFX.getConfigurator(), config); + talonFXPIDFConfig.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING; + + Configurator.configureTalonFX(talonFX.getConfigurator(), talonFXPIDFConfig); } @Override diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java index 16ab924..9fe7e2a 100644 --- a/src/main/java/frc/robot/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/swerve/SwerveConstants.java @@ -76,12 +76,16 @@ public static class MK4iConstants { /** * Calculates the maximum attainable open loop speed in meters per second. - * - * @param rotorVelocityRotationsPerSecondAt12Volts the rotor velocity of the drive motor in rotations per second when the motor is supplied 12 volts. + * + * @param rotorVelocityRotationsPerSecondAt12Volts the rotor velocity of the drive motor in + * rotations per second when the motor is supplied 12 volts. * @return the maximum attainable open loop speed in meters per second. */ - private static double calculateMaximumAttainableSpeed(double rotorVelocityRotationsPerSecondAt12Volts) { - return rotorVelocityRotationsPerSecondAt12Volts / MK4iConstants.DRIVE_GEARING * MK4iConstants.WHEEL_CIRCUMFERENCE; + private static double calculateMaximumAttainableSpeed( + double rotorVelocityRotationsPerSecondAt12Volts) { + return rotorVelocityRotationsPerSecondAt12Volts + / MK4iConstants.DRIVE_GEARING + * MK4iConstants.WHEEL_CIRCUMFERENCE; } /** Maximum attainable speed in meters per second. */ @@ -92,10 +96,10 @@ private static double calculateMaximumAttainableSpeed(double rotorVelocityRotati /** * Calculates an acceleration using a ramp duration. - * - * @param maximumSpeed the maximum speed. + * + * @param maximumSpeed the maximum speed in units per second. * @param rampDurationSeconds the desired duration to ramp from no speed to full speed. - * @return the acceleration. + * @return the acceleration in units per second per second. */ public static double calculateAcceleration(double maximumSpeed, double rampDurationSeconds) { return maximumSpeed / rampDurationSeconds; @@ -112,7 +116,8 @@ public static double calculateAcceleration(double maximumSpeed, double rampDurat public static final double MAXIMUM_ROTATION_SPEED = 0.5; /** Maximum acceleration in rotations per second per second. */ - public static final double MAXIMUM_ROTATION_ACCELERATION = calculateAcceleration(MAXIMUM_ROTATION_SPEED, 0.1); + public static final double MAXIMUM_ROTATION_ACCELERATION = + calculateAcceleration(MAXIMUM_ROTATION_SPEED, 0.1); public static class DriveMotorConstants { /** If true, use open-loop control. */ diff --git a/src/main/java/frc/robot/swerve/SwerveFactory.java b/src/main/java/frc/robot/swerve/SwerveFactory.java index 7553ebc..47a5c81 100644 --- a/src/main/java/frc/robot/swerve/SwerveFactory.java +++ b/src/main/java/frc/robot/swerve/SwerveFactory.java @@ -1,12 +1,7 @@ package frc.robot.swerve; import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.InvertedValue; -import frc.lib.MotorCurrentLimits; import frc.robot.Robot; -import frc.robot.swerve.SwerveConstants.DriveMotorConstants; -import frc.robot.swerve.SwerveConstants.MK4iConstants; /** Helper class for creating hardware for the swerve subsystem. */ public class SwerveFactory { @@ -57,25 +52,6 @@ public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) { return new SteerMotorIOSim(); } - /** - * Creates a steer motor configuration. - * - * @return a steer motor configuration. - */ - public static TalonFXConfiguration createSteerMotorConfig() { - TalonFXConfiguration steerMotorConfig = new TalonFXConfiguration(); - - steerMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - steerMotorConfig.ClosedLoopGeneral.ContinuousWrap = true; - - // TODO Copied from Nutrons 2023 release - steerMotorConfig.CurrentLimits = - new MotorCurrentLimits(0.0, 40.0, 3.0, 1.0).asCurrentLimitsConfigs(); - - return steerMotorConfig; - } - /** * Creates a drive motor. * @@ -86,21 +62,4 @@ public static DriveMotorIO createDriveMotor(SwerveModuleConfig config) { return new DriveMotorIOSim(); } - - /** - * Creates a drive motor configuration. - * - * @return a drive motor configuration. - */ - public static TalonFXConfiguration createDriveMotorConfig() { - TalonFXConfiguration driveMotorConfig = new TalonFXConfiguration(); - - driveMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - driveMotorConfig.Feedback.SensorToMechanismRatio = MK4iConstants.DRIVE_GEARING; - - driveMotorConfig.CurrentLimits = DriveMotorConstants.CURRENT_LIMITS.asCurrentLimitsConfigs(); - - return driveMotorConfig; - } } diff --git a/src/main/java/frc/robot/vision/VisionPoseEstimatorIONull.java b/src/main/java/frc/robot/vision/VisionPoseEstimatorIONull.java index 0e7c20d..34024e8 100644 --- a/src/main/java/frc/robot/vision/VisionPoseEstimatorIONull.java +++ b/src/main/java/frc/robot/vision/VisionPoseEstimatorIONull.java @@ -1,22 +1,20 @@ package frc.robot.vision; -import java.util.Optional; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import java.util.Optional; /** Vision pose estimator that does nothing. */ public class VisionPoseEstimatorIONull implements VisionPoseEstimatorIO { - @Override - public void configure() {} + @Override + public void configure() {} - @Override - public void setReferencePosition(Pose2d referencePosition) {} + @Override + public void setReferencePosition(Pose2d referencePosition) {} - @Override - public Optional getPosition() { - return Optional.empty(); - } - + @Override + public Optional getPosition() { + return Optional.empty(); + } }