FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #4 from FairportRobotics/swervetuning 

Swervetuning
This commit is contained in:
The-DOMinator-G 2026-03-02 20:21:34 -05:00 committed by GitHub
parent 1e19354a9d 940614c790
commit a562f12195
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 58 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/main/java/frc/robot/Constants.java
View File

@ -23,7 +23,43 @@ public final class Constants {
REAL,
REPLAY
}
public static final RobotType CURRENT_MODE = RobotType.REAL;
}
// Swerve offsets as of 3/2/26
public static class SwerveDriveOffsets {
public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
}
/*
* Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot
* Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot
* Numbers starting with 3 (E.g. 31, 32, 33) are in the Back Left of the robot
* Numbers starting with 4 (E.g. 41, 42, 43) are in the Back Right of the robot
* Laters numbers will be added for more subsystems
* Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon
*/
public static class SwerveDriveIDs {
public static final int FRONT_LEFT_STEER_ID = 11;
public static final int FRONT_LEFT_DRIVE_ID = 12;
public static final int FRONT_LEFT_ENCODER_ID = 13;
public static final int FRONT_RIGHT_STEER_ID = 21;
public static final int FRONT_RIGHT_DRIVE_ID = 22;
public static final int FRONT_RIGHT_ENCODER_ID = 23;
public static final int BACK_LEFT_STEER_ID = 31;
public static final int BACK_LEFT_DRIVE_ID = 32;
public static final int BACK_LEFT_ENCODER_ID = 33;
public static final int BACK_RIGHT_STEER_ID = 41;
public static final int BACK_RIGHT_DRIVE_ID = 42;
public static final int BACK_RIGHT_ENCODER_ID = 43;
}
//Bird
public static class ExtraIDEntities {
public static final int PIGEON_ID = 1;
}
}

39
src/main/java/frc/robot/subsystems/DriveSubsystem.java
View File

@ -14,6 +14,8 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants;
public class DriveSubsystem extends TestableSubsystem {
private SwerveBuilder swerveBuilder = new SwerveBuilder();
@ -33,23 +35,24 @@ public class DriveSubsystem extends TestableSubsystem {
driveSystem = swerveBuilder
.withCanbusName("Drive")
.withPigeonId(1)
.withPigeonId(Constants.ExtraIDEntities.PIGEON_ID)
.withMaxLinearVelocity(3.0)
.withMaxAngularVelocity(Math.PI * 2)
.withMaxAngularVelocity(Math.PI)
.withSwerveModule(
swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(12)
.withDriveMotorId(Constants.SwerveDriveIDs.FRONT_LEFT_DRIVE_ID)
.withDriveKP(driveP)
.withDriveKI(driveI)
.withDriveKD(driveD)
.withDriveKV(driveKV)
.withSteerMotorId(11)
.withSteerMotorId(Constants.SwerveDriveIDs.FRONT_LEFT_STEER_ID)
.withSteerKP(steerP)
.withSteerKI(steerI)
.withSteerKD(steerD)
.withSteerKV(steerKV)
.withSteerOffset(-0.336426)
.withSteerEncoderId(13)
//If the offsets don't work, contact Tyler W or Dom G
.withSteerOffset(Constants.SwerveDriveOffsets.FRONT_LEFT_MOTOR_OFFSET)
.withSteerEncoderId(Constants.SwerveDriveIDs.FRONT_LEFT_ENCODER_ID)
.withModuleLocation(new Translation2d(1, 1))
.withModuleName("Front Left")
.withGearRatio(8.14)
@ -57,19 +60,19 @@ public class DriveSubsystem extends TestableSubsystem {
.build())
.withSwerveModule(
swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(22)
.withDriveMotorId(Constants.SwerveDriveIDs.FRONT_RIGHT_DRIVE_ID)
.withDriveKP(driveP)
.withDriveKI(driveI)
.withDriveKD(driveD)
.withDriveKV(driveKV)
.withDriveInverted()
.withSteerMotorId(21)
.withSteerMotorId(Constants.SwerveDriveIDs.FRONT_RIGHT_STEER_ID)
.withSteerKP(steerP)
.withSteerKI(steerI)
.withSteerKD(steerD)
.withSteerKV(steerKV)
.withSteerOffset(0.108887)
.withSteerEncoderId(23)
.withSteerOffset(Constants.SwerveDriveOffsets.FRONT_RIGHT_MOTOR_OFFSET)
.withSteerEncoderId(Constants.SwerveDriveIDs.FRONT_RIGHT_ENCODER_ID)
.withModuleLocation(new Translation2d(1, -1))
.withModuleName("Front Right")
.withGearRatio(8.14)
@ -77,18 +80,18 @@ public class DriveSubsystem extends TestableSubsystem {
.build())
.withSwerveModule(
swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(32)
.withDriveMotorId(Constants.SwerveDriveIDs.BACK_LEFT_DRIVE_ID)
.withDriveKP(driveP)
.withDriveKI(driveI)
.withDriveKD(driveD)
.withDriveKV(driveKV)
.withSteerMotorId(31)
.withSteerMotorId(Constants.SwerveDriveIDs.BACK_LEFT_STEER_ID)
.withSteerKP(steerP)
.withSteerKI(steerI)
.withSteerKD(steerD)
.withSteerKV(steerKV)
.withSteerOffset(-0.492432)
.withSteerEncoderId(33)
.withSteerOffset(Constants.SwerveDriveOffsets.BACK_LEFT_MOTOR_OFFSET)
.withSteerEncoderId(Constants.SwerveDriveIDs.BACK_LEFT_ENCODER_ID)
.withModuleLocation(new Translation2d(-1, 1))
.withModuleName("Back Left")
.withGearRatio(8.14)
@ -96,19 +99,19 @@ public class DriveSubsystem extends TestableSubsystem {
.build())
.withSwerveModule(
swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(42)
.withDriveMotorId(Constants.SwerveDriveIDs.BACK_RIGHT_DRIVE_ID)
.withDriveKP(driveP)
.withDriveKI(driveI)
.withDriveKD(driveD)
.withDriveKV(driveKV)
.withDriveInverted()
.withSteerMotorId(41)
.withSteerMotorId(Constants.SwerveDriveIDs.BACK_RIGHT_STEER_ID)
.withSteerKP(steerP)
.withSteerKI(steerI)
.withSteerKD(steerD)
.withSteerKV(steerKV)
.withSteerOffset(0.470215)
.withSteerEncoderId(43)
.withSteerOffset(Constants.SwerveDriveOffsets.BACK_RIGHT_MOTOR_OFFSET)
.withSteerEncoderId(Constants.SwerveDriveIDs.BACK_RIGHT_ENCODER_ID)
.withModuleLocation(new Translation2d(-1, -1))
.withModuleName("Back Right")
.withGearRatio(8.14)