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

This commit is contained in:
2026-01-24 18:02:05 -05:00
parent c6cc0c6b60
commit dae39c30a0
5 changed files with 59 additions and 37 deletions

View File

@@ -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) {