Team_2648/2026-Robot-Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Bradley Bickford
2026-01-24 18:02:05 -05:00
parent c6cc0c6b60
commit dae39c30a0
5 changed files with 59 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/java/frc/robot/RobotContainer.java
View File

@@ -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();
}

27
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

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

13
src/main/java/frc/robot/constants/ModuleConstants.java
View File

@@ -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 = 0;
public static final double kDrivingMotorReduction = (14.0 * 28.0 * 15.0) / (50 * 16 * 45);
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)
// TODO Hold over from 2025, adjust?
public static final double kTurningMotorReduction = 0;
public static final double kTurningMotorReduction = 150.0/7.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

26
src/main/java/frc/robot/subsystems/Drivetrain.java
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) {

22
src/main/java/frc/robot/utilities/SwerveModule.java
View File

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