Merge branch 'dcmp' of https://github.com/team4909/2024-Crescendo int…

…o dcmp

src/main/java/frc/robot/Constants.java

@@ -14,7 +14,16 @@
 public final class Constants {
   public static final Mode kCurrentMode = Mode.kReal;
   public static final RobotName kRobot = RobotName.kBlackMamba;
-  public static final boolean kIsViper = kRobot.equals(RobotName.kViper);
+
+  // static {
+  //   final String rioSerialNumber = RobotController.getSerialNumber();
+  //   if (rioSerialNumber.equals(RobotName.kViper.rioSerialNumber)) kRobot = RobotName.kViper;
+  //   else if (rioSerialNumber.equals(RobotName.kBlackMamba.rioSerialNumber))
+  //     kRobot = RobotName.kBlackMamba;
+  //   else kRobot = RobotName.kBlackMamba;
+  // }
+
+  public static final boolean kIsViper = false;
   public static final boolean kIsSim = Constants.kCurrentMode.equals(Mode.kSim);
   public static final String kDrivetrainCanBus = "CANivore1";
   public static final String kSuperstructureCanBus = "CANivore2";
@@ -50,7 +59,13 @@ public static enum Mode {
   }
 
   public static enum RobotName {
-    kViper,
-    kBlackMamba
+    kViper("032380FD"),
+    kBlackMamba("032243C9");
+
+    public String rioSerialNumber;
+
+    private RobotName(String rioSerialNumber) {
+      this.rioSerialNumber = rioSerialNumber;
+    }
   }
 }

src/main/java/frc/robot/arm/ArmIOTalonFX.java

@@ -55,7 +55,7 @@ public ArmIOTalonFX() {
     final TalonFXConfiguration elbowLeftMotorConfig = new TalonFXConfiguration();
     elbowLeftMotorConfig.CurrentLimits = currentLimitsConfig;
     elbowLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-    elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
     elbowLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kElbowReduction;
     elbowLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
     elbowLeftMotorConfig.Slot0.kP = 62.122;
@@ -66,14 +66,16 @@ public ArmIOTalonFX() {
     elbowLeftMotorConfig.Slot0.kG = 0.37766;
     elbowLeftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0.5;
     elbowLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0;
+    elbowLeftMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40.0;
+    elbowLeftMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40.0;
     elbowLeftMotorConfig.Voltage.PeakForwardVoltage = 6.0;
     elbowLeftMotorConfig.Voltage.PeakReverseVoltage = -6.0;
     m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig, 1.0);
 
     final TalonFXConfiguration wristLeftMotorConfig = new TalonFXConfiguration();
     wristLeftMotorConfig.CurrentLimits = currentLimitsConfig;
     wristLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-    wristLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    wristLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
     wristLeftMotorConfig.ClosedLoopGeneral.ContinuousWrap = true;
     wristLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kWristReduction;
     wristLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
@@ -87,17 +89,20 @@ public ArmIOTalonFX() {
     wristLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0;
     wristLeftMotorConfig.Voltage.PeakForwardVoltage = 3.0;
     wristLeftMotorConfig.Voltage.PeakReverseVoltage = -3.0;
+    wristLeftMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40.0;
+    wristLeftMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40.0;
+
     m_wristLeftMotor.getConfigurator().apply(wristLeftMotorConfig, 1.0);
 
     final TalonFXConfiguration elbowRightMotorConfig = new TalonFXConfiguration();
     elbowRightMotorConfig.CurrentLimits = currentLimitsConfig;
-    elbowRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    elbowRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
     m_elbowRightFollowerMotor.getConfigurator().apply(elbowRightMotorConfig, 1.0);
     m_elbowRightFollowerMotor.setControl(new Follower(m_elbowLeftMotor.getDeviceID(), true));
 
     final TalonFXConfiguration wristRightMotorConfig = new TalonFXConfiguration();
     wristRightMotorConfig.CurrentLimits = currentLimitsConfig;
-    wristRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    wristRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
     m_wristRightFollowerMotor.getConfigurator().apply(wristRightMotorConfig, 1.0);
     m_wristRightFollowerMotor.setControl(new Follower(m_wristLeftMotor.getDeviceID(), true));
 

src/main/java/frc/robot/climber/Climber.java

@@ -6,10 +6,10 @@
 import org.littletonrobotics.junction.Logger;
 
 public class Climber extends SubsystemBase {
-  public static final double kWinchReduction = 1.0;
+  public static final double kWinchReduction = 20.0;
   private final ClimberIO m_io;
   private final ClimberIOInputsAutoLogged m_inputs = new ClimberIOInputsAutoLogged();
-  private final double kTrapLimit = 200.0;
+  private final double kTrapLimit = 10.0;
 
   public Climber(ClimberIO climberIO) {
     m_io = climberIO;

src/main/java/frc/robot/drivetrain/Module.java

@@ -4,17 +4,20 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import frc.robot.Constants;
 import org.littletonrobotics.junction.Logger;
 
 public class Module {
 
   // https://www.swervedrivespecialties.com/products/mk4i-swerve-module
   // However, our first stage is 50:13 instead of 50:14 because we have a different gear.
-  public static final double kDriveRatio = (50.0 / 13.0) * (16.0 / 28.0) * (45.0 / 15.0);
+  private static final double kDriveRatioFirstStage =
+      Constants.kIsViper ? (50.0 / 13.0) : (50.0 / 14.0);
+  public static final double kDriveRatio = kDriveRatioFirstStage * (16.0 / 28.0) * (45.0 / 15.0);
   public static final double kSteerRatio = 150.0 / 7.0;
   private final double kWheelDiameterMeters = Units.inchesToMeters(1.87 * 2);
   private final double kWheelRadiusMeters = kWheelDiameterMeters / 2.0;
-  private final double kCouplingGearRatio = 50.0 / 13.0;
+  private final double kCouplingRatio = kDriveRatioFirstStage;
 
   private final ModuleIO m_io;
   private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged();
@@ -39,7 +42,7 @@ public void periodic() {
     for (int i = 0; i < sampleCount; i++) {
       double driveRotations = Units.radiansToRotations(m_inputs.odometryDrivePositionsRad[i]);
       Rotation2d steerAngle = m_inputs.odometryTurnPositions[i];
-      driveRotations -= steerAngle.getRotations() * kCouplingGearRatio;
+      driveRotations -= steerAngle.getRotations() * kCouplingRatio;
       double driveRadians = Units.rotationsToRadians(driveRotations);
       double positionMeters = driveRadians * kWheelRadiusMeters;
       m_odometryPositions[i] = new SwerveModulePosition(positionMeters, steerAngle);
@@ -58,7 +61,7 @@ public SwerveModuleState setSetpoint(SwerveModuleState state) {
 
     if (optimizedState.speedMetersPerSecond != 0.0) {
       double azimuthVelocityRPS = Units.radiansToRotations(m_inputs.steerVelocityRadPerSec);
-      double driveRateBackOut = azimuthVelocityRPS *= kCouplingGearRatio;
+      double driveRateBackOut = azimuthVelocityRPS *= kCouplingRatio;
       setpointVelocityRPS += driveRateBackOut;
     }
 

src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java

@@ -61,7 +61,7 @@ public ModuleIOTalonFX(int index) {
         m_driveMotor = new TalonFX(4, Constants.kDrivetrainCanBus);
         m_steerMotor = new TalonFX(3, Constants.kDrivetrainCanBus);
         m_azimuthEncoder = new CANcoder(12, Constants.kDrivetrainCanBus);
-        m_absoluteEncoderMagnetOffset = Constants.kIsViper ? 0.37353515625 : 0.047607421875;
+        m_absoluteEncoderMagnetOffset = Constants.kIsViper ? 0.37353515625 : 0.066162109375;
         kDrivekS = 0.092972;
         kDrivekV = 0.82452;
         break;
@@ -127,6 +127,7 @@ public ModuleIOTalonFX(int index) {
     steerMotorConfig.MotionMagic.MotionMagicExpo_kA = 0.1;
     steerMotorConfig.Slot0.kP = kSteerkP;
     steerMotorConfig.Slot0.kD = kSteerkD;
+    steerMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
     m_steerMotor.getConfigurator().apply(steerMotorConfig);
 
     m_drivePositionSignal = m_driveMotor.getPosition();

src/main/java/frc/robot/feeder/FeederIOTalonFX.java

@@ -5,12 +5,15 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.wpilibj.DigitalGlitchFilter;
 import edu.wpi.first.wpilibj.DigitalInput;
 import frc.robot.Constants;
+import java.time.Duration;
 
 public class FeederIOTalonFX implements FeederIO {
   private final TalonFX m_feederMotor;
   private final DigitalInput m_topNoteSensor;
+  private final DigitalGlitchFilter m_glitchFilter;
 
   private final StatusSignal<Double> m_rollerPositionSignal,
       m_rollerVelocitySignal,
@@ -20,6 +23,9 @@ public class FeederIOTalonFX implements FeederIO {
   public FeederIOTalonFX() {
     m_feederMotor = new TalonFX(19, Constants.kSuperstructureCanBus);
     m_topNoteSensor = new DigitalInput(0);
+    m_glitchFilter = new DigitalGlitchFilter();
+    m_glitchFilter.setPeriodNanoSeconds(Duration.ofMillis(1).toNanos());
+    m_glitchFilter.add(m_topNoteSensor);
 
     final TalonFXConfiguration feederMotorConfig = new TalonFXConfiguration();
     m_feederMotor.getConfigurator().apply(feederMotorConfig);

src/main/java/frc/robot/lights/Lights.java

@@ -57,7 +57,7 @@ public Command startGreenFlow() {
                         color.green,
                         color.blue,
                         0,
-                        0.5,
+                        0.1,
                         kLedCount,
                         Direction.Forward,
                         kCandleLedOffset)))

src/main/java/frc/robot/shooter/Shooter.java

@@ -17,7 +17,7 @@
 
 public class Shooter extends SubsystemBase {
 
-  public static final double kShooterStepUp = Constants.kIsViper ? 0.0 : 0.5;
+  public static final double kShooterStepUp = Constants.kIsViper ? 0.0 : 0.0;
   public static final double kFarShotVelocityRpm = 5600.0;
   private final double kTrapShot = 400.0;
   private final double kAmpShot = 5000.0;