Build Session 1/24, swerve work, need to revalidate individual module CAN IDs

src/main/java/frc/robot/RobotContainer.java

@@ -11,8 +11,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -52,6 +50,8 @@ public class RobotContainer {
       )
     );
 
+    //drivetrain.setDefaultCommand(drivetrain.disableOutputs());
+
     configureShiftDisplay();
   }
 

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -9,9 +9,8 @@ public class DrivetrainConstants {
     public static final double kMaxSpeedMetersPerSecond = 4.125;
     public static final double kMaxAngularSpeed = 2 * Math.PI;
 
-    // TODO Replace zeros with real numbers
+    public static final double kTrackWidth = Units.inchesToMeters(23.75);
-    public static final double kTrackWidth = Units.inchesToMeters(0);
+    public static final double kWheelBase = Units.inchesToMeters(18.75);
-    public static final double kWheelBase = Units.inchesToMeters(0);
 
     // TODO Replace zeros with real numbers
     // These values should be determinable by writing the magnetic encoder output
@@ -24,26 +23,24 @@ public class DrivetrainConstants {
     public static final double kRearLeftMagEncoderOffset = 0;
     public static final double kRearRightMagEncoderOffset = 0;
 
-    // Kraken CAN IDs 
+    // Kraken CAN IDs
-    // TODO Real CAN IDs
     public static final int kFrontLeftDrivingCANID = 0;
-    public static final int kFrontRightDrivingCANID = 0;
+    public static final int kFrontRightDrivingCANID = 1;//3;
-    public static final int kRearLeftDrivingCANID = 0;
+    public static final int kRearLeftDrivingCANID = 3;//1;
-    public static final int kRearRightDrivingCANID = 0;
+    public static final int kRearRightDrivingCANID = 2;
 
     // SparkMAX CAN IDs
-    // TODO Real CAN IDs
+    public static final int kFrontLeftTurningCANID = 8;
-    public static final int kFrontLeftTurningCANID = 0;
+    public static final int kFrontRightTurningCANID = 7;//9;
-    public static final int kFrontRightTurningCANID = 0;
+    public static final int kRearLeftTurningCANID = 9;//7;
-    public static final int kRearLeftTurningCANID = 0;
+    public static final int kRearRightTurningCANID = 6;
-    public static final int kRearRightTurningCANID = 0;
 
     // Analog Encoder Input Ports 
     // TODO Real Port IDs
     public static final int kFrontLeftAnalogInPort = 0;
-    public static final int kFrontRightAnalogInPort = 0;
+    public static final int kFrontRightAnalogInPort = 1;
-    public static final int kRearLeftAnalogInPort = 0;
+    public static final int kRearLeftAnalogInPort = 2;
-    public static final int kRearRightAnalogInPort = 0;
+    public static final int kRearRightAnalogInPort = 3;
 
     public static final boolean kGyroReversed = true;
 

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -11,13 +11,11 @@ import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.util.Units;
 
 public class ModuleConstants {
     // DRIVING MOTOR CONFIG (Kraken)
-    // TODO Replace with something other than 0
+    public static final double kDrivingMotorReduction = (14.0 * 28.0 * 15.0) / (50 * 16 * 45);
-    public static final double kDrivingMotorReduction = 0;
 
     public static final double kDrivingMotorFeedSpeedRPS = KrakenMotorConstants.kFreeSpeedRPM / 60;
     public static final double kWheelDiameterMeters = Units.inchesToMeters(4);
@@ -44,14 +42,15 @@ public class ModuleConstants {
     public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
 
     // TURNING MOTOR CONFIG (NEO)
-
+    public static final double kTurningMotorReduction = 150.0/7.0;
-    // TODO Hold over from 2025, adjust?
-    public static final double kTurningMotorReduction = 0;
     public static final double kTurningFactor = 2 * Math.PI / kTurningMotorReduction;
+    // TODO Adjust? Let over from 2025
     public static final double kTurnP = 1;
     public static final double kTurnI = 0;
     public static final double kTurnD = 0;
 
+    public static final boolean kIsEncoderInverted = false;
+
     // TODO How sensitive is too sensitive?
     public static final double kAutoResetPositionDeadband = Units.degreesToRadians(.25);
     
@@ -93,7 +92,7 @@ public class ModuleConstants {
             .idleMode(kTurnIdleMode)
             .smartCurrentLimit(kTurnMotorCurrentLimit);
         turningConfig.encoder
-            .inverted(true)
+            //.inverted(true)
             .positionConversionFactor(kTurningFactor)
             .velocityConversionFactor(kTurningFactor / 60.0);
         turningConfig.closedLoop

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -48,34 +48,34 @@ public class Drivetrain extends SubsystemBase {
         frontLeft = new SwerveModule(
             "FrontLeft", 
             DrivetrainConstants.kFrontLeftDrivingCANID, 
-            DrivetrainConstants.kFrontLeftTurningCANID, 
+            DrivetrainConstants.kFrontLeftTurningCANID);/*, 
             DrivetrainConstants.kFrontLeftAnalogInPort, 
             DrivetrainConstants.kFrontLeftMagEncoderOffset
-        );
+        ); */
 
         frontRight = new SwerveModule(
             "FrontRight", 
             DrivetrainConstants.kFrontRightDrivingCANID, 
-            DrivetrainConstants.kFrontRightTurningCANID, 
+            DrivetrainConstants.kFrontRightTurningCANID);/*, 
             DrivetrainConstants.kFrontRightAnalogInPort, 
             DrivetrainConstants.kFrontRightMagEncoderOffset
-        );
+        );*/
 
         rearLeft = new SwerveModule(
             "RearLeft", 
             DrivetrainConstants.kRearLeftDrivingCANID, 
-            DrivetrainConstants.kRearLeftTurningCANID, 
+            DrivetrainConstants.kRearLeftTurningCANID);/*, 
             DrivetrainConstants.kRearLeftAnalogInPort, 
             DrivetrainConstants.kRearLeftMagEncoderOffset
-        );
+        );*/
 
         rearRight = new SwerveModule(
             "RearRight", 
             DrivetrainConstants.kRearRightDrivingCANID, 
-            DrivetrainConstants.kRearRightTurningCANID, 
+            DrivetrainConstants.kRearRightTurningCANID);/*, 
             DrivetrainConstants.kRearRightAnalogInPort, 
             DrivetrainConstants.kRearRightMagEncoderOffset
-        );
+        );*/
 
         gyro = new AHRS(NavXComType.kMXP_SPI);
 
@@ -130,9 +130,19 @@ public class Drivetrain extends SubsystemBase {
         rearRight.periodic();
 
         Logger.recordOutput("Drivetrain/Pose", getPose());
+        Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue());
         Logger.recordOutput("Drivetrain/Heading", getHeading());
     }
 
+    public Command disableOutputs() {
+        return run(() -> {
+            frontLeft.disableOutput();
+            frontRight.disableOutput();
+            rearLeft.disableOutput();
+            rearRight.disableOutput();
+        });  
+    }
+
     // TODO check both cameras
     public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
         if (camera1 == null) {

src/main/java/frc/robot/utilities/SwerveModule.java

@@ -114,6 +114,8 @@ public class SwerveModule {
             PersistMode.kPersistParameters
         );
 
+        driveVelocityRequest = new VelocityVoltage(0);
+
         if(isAbsoluteEncoderDisabled){
             turningRelativeEncoder.setPosition(0);
         } else {
@@ -130,9 +132,14 @@ public class SwerveModule {
     }
 
     public void periodic() {
-        Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
+        if(!isAbsoluteEncoderDisabled) {
+            Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
+        }
+        
+        Logger.recordOutput(moduleName + "ModuleTargetState", lastTargetState);
         Logger.recordOutput(moduleName + "/SwerveModuleState", getState());
         Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition());
+        Logger.recordOutput(moduleName + "/RelativeEncoderPosition", getTurningEncoderPosition());
 
         if(!isAbsoluteEncoderDisabled && turningEncoderAutoRezeroEnabled) {
             if(Math.abs(getState().angle.getRadians() - lastTargetState.angle.getRadians()) <= ModuleConstants.kAutoResetPositionDeadband) {
@@ -144,22 +151,27 @@ public class SwerveModule {
     public SwerveModuleState getState() {
         return new SwerveModuleState(
             drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, 
-            new Rotation2d(turningRelativeEncoder.getPosition())
+            new Rotation2d(getTurningEncoderPosition())
         );
     }
 
     public SwerveModulePosition getPosition() {
         return new SwerveModulePosition(
             drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, 
-            new Rotation2d(turningRelativeEncoder.getPosition())
+            new Rotation2d(getTurningEncoderPosition())
         );
     }
 
+    public void disableOutput() {
+        drive.disable();
+        turning.disable();
+    }
+
     public void setDesiredState(SwerveModuleState desiredState) {
         // TODO is this really necessary, the offset is managed by the Absolute Encoder
         // and its "source of truth" behavior in relation to the relative encoder
         // Probably doesn't *hurt* that it's here, but it may not be needed
-        desiredState.optimize(new Rotation2d(turningRelativeEncoder.getPosition()));
+        desiredState.optimize(new Rotation2d(getTurningEncoderPosition()));
 
         lastTargetState = desiredState;
 
@@ -177,6 +189,10 @@ public class SwerveModule {
         );
     }
 
+    public double getTurningEncoderPosition() {
+        return turningRelativeEncoder.getPosition() * (ModuleConstants.kIsEncoderInverted ? -1 : 1);
+    }
+
     public void resetEncoders() {
         drive.setPosition(0);