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

Update CAN IDs

This commit is contained in:
Mike Silva 2026-02-28 16:26:03 -05:00
parent defa76eae0
commit 98474a4125
1 changed files with 166 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,166 +1,166 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.TestableSubsystem;
import org.fairportrobotics.frc.posty.test.PostTest; import org.fairportrobotics.frc.posty.test.PostTest;
import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder;
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem;
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule;
import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.Logger;
import com.fasterxml.jackson.annotation.JsonCreator.Mode; import com.fasterxml.jackson.annotation.JsonCreator.Mode;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
public class DriveSubsystem extends TestableSubsystem { public class DriveSubsystem extends TestableSubsystem {
private SwerveBuilder swerveBuilder = new SwerveBuilder(); private SwerveBuilder swerveBuilder = new SwerveBuilder();
private SwerveDriveSystem driveSystem; private SwerveDriveSystem driveSystem;
private double driveP = 0.1; private double driveP = 0.1;
private double driveI = 0.0; private double driveI = 0.0;
private double driveD = 0.01; private double driveD = 0.01;
private double driveKV = 0.2; private double driveKV = 0.2;
private double steerP = 20; private double steerP = 20;
private double steerI = 0; private double steerI = 0;
private double steerD = 0.0; private double steerD = 0.0;
private double steerKV = 0; private double steerKV = 0;
public DriveSubsystem(CommandXboxController xboxController) { public DriveSubsystem(CommandXboxController xboxController) {
driveSystem = swerveBuilder driveSystem = swerveBuilder
.withCanbusName("Drive") .withCanbusName("Drive")
.withPigeonId(20) .withPigeonId(1)
.withMaxLinearVelocity(3.0) .withMaxLinearVelocity(3.0)
.withMaxAngularVelocity(Math.PI * 2) .withMaxAngularVelocity(Math.PI * 2)
.withSwerveModule( .withSwerveModule(
swerveBuilder.new SwerveModuleBuilder() swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(12) .withDriveMotorId(12)
.withDriveKP(driveP) .withDriveKP(driveP)
.withDriveKI(driveI) .withDriveKI(driveI)
.withDriveKD(driveD) .withDriveKD(driveD)
.withDriveKV(driveKV) .withDriveKV(driveKV)
.withSteerMotorId(3) .withSteerMotorId(11)
.withSteerKP(steerP) .withSteerKP(steerP)
.withSteerKI(steerI) .withSteerKI(steerI)
.withSteerKD(steerD) .withSteerKD(steerD)
.withSteerKV(steerKV) .withSteerKV(steerKV)
.withSteerOffset(0.359375) .withSteerOffset(-0.338623)
.withSteerEncoderId(10) .withSteerEncoderId(13)
.withModuleLocation(new Translation2d(1, 1)) .withModuleLocation(new Translation2d(1, 1))
.withModuleName("Front Left") .withModuleName("Front Left")
.withGearRatio(8.14) .withGearRatio(8.14)
.withWheelDiameter(0.1016) .withWheelDiameter(0.1016)
.build()) .build())
.withSwerveModule( .withSwerveModule(
swerveBuilder.new SwerveModuleBuilder() swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(5) .withDriveMotorId(22)
.withDriveKP(driveP) .withDriveKP(driveP)
.withDriveKI(driveI) .withDriveKI(driveI)
.withDriveKD(driveD) .withDriveKD(driveD)
.withDriveKV(driveKV) .withDriveKV(driveKV)
.withDriveInverted() .withDriveInverted()
.withSteerMotorId(6) .withSteerMotorId(21)
.withSteerKP(steerP) .withSteerKP(steerP)
.withSteerKI(steerI) .withSteerKI(steerI)
.withSteerKD(steerD) .withSteerKD(steerD)
.withSteerKV(steerKV) .withSteerKV(steerKV)
.withSteerOffset(0.406494) .withSteerOffset(0.110840)
.withSteerEncoderId(1) .withSteerEncoderId(23)
.withModuleLocation(new Translation2d(1, -1)) .withModuleLocation(new Translation2d(1, -1))
.withModuleName("Front Right") .withModuleName("Front Right")
.withGearRatio(8.14) .withGearRatio(8.14)
.withWheelDiameter(0.1016) .withWheelDiameter(0.1016)
.build()) .build())
.withSwerveModule( .withSwerveModule(
swerveBuilder.new SwerveModuleBuilder() swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(8) .withDriveMotorId(32)
.withDriveKP(driveP) .withDriveKP(driveP)
.withDriveKI(driveI) .withDriveKI(driveI)
.withDriveKD(driveD) .withDriveKD(driveD)
.withDriveKV(driveKV) .withDriveKV(driveKV)
.withSteerMotorId(2) .withSteerMotorId(31)
.withSteerKP(steerP) .withSteerKP(steerP)
.withSteerKI(steerI) .withSteerKI(steerI)
.withSteerKD(steerD) .withSteerKD(steerD)
.withSteerKV(steerKV) .withSteerKV(steerKV)
.withSteerOffset(0.211670) .withSteerOffset(-0.496582)
.withSteerEncoderId(7) .withSteerEncoderId(33)
.withModuleLocation(new Translation2d(-1, 1)) .withModuleLocation(new Translation2d(-1, 1))
.withModuleName("Back Left") .withModuleName("Back Left")
.withGearRatio(8.14) .withGearRatio(8.14)
.withWheelDiameter(0.1016) .withWheelDiameter(0.1016)
.build()) .build())
.withSwerveModule( .withSwerveModule(
swerveBuilder.new SwerveModuleBuilder() swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(9) .withDriveMotorId(42)
.withDriveKP(driveP) .withDriveKP(driveP)
.withDriveKI(driveI) .withDriveKI(driveI)
.withDriveKD(driveD) .withDriveKD(driveD)
.withDriveKV(driveKV) .withDriveKV(driveKV)
.withDriveInverted() .withDriveInverted()
.withSteerMotorId(11) .withSteerMotorId(41)
.withSteerKP(steerP) .withSteerKP(steerP)
.withSteerKI(steerI) .withSteerKI(steerI)
.withSteerKD(steerD) .withSteerKD(steerD)
.withSteerKV(steerKV) .withSteerKV(steerKV)
.withSteerOffset(-0.235107) .withSteerOffset(0.463379)
.withSteerEncoderId(4) .withSteerEncoderId(43)
.withModuleLocation(new Translation2d(-1, -1)) .withModuleLocation(new Translation2d(-1, -1))
.withModuleName("Back Right") .withModuleName("Back Right")
.withGearRatio(8.14) .withGearRatio(8.14)
.withWheelDiameter(0.1016) .withWheelDiameter(0.1016)
.build()) .build())
.build(); .build();
this.setDefaultCommand(Commands.run(new Runnable() { this.setDefaultCommand(Commands.run(new Runnable() {
@Override @Override
public void run() { public void run() {
driveSystem.setChassisSpeedsFromJoystickRobotRelative( driveSystem.setChassisSpeedsFromJoystickRobotRelative(
-(xboxController.getLeftY() * Math.abs(xboxController.getLeftY())), -(xboxController.getLeftY() * Math.abs(xboxController.getLeftY())),
-(xboxController.getLeftX() * Math.abs(xboxController.getLeftX())), -(xboxController.getLeftX() * Math.abs(xboxController.getLeftX())),
-(xboxController.getRawAxis(4) * Math.abs(xboxController.getRawAxis(4)))); -(xboxController.getRawAxis(4) * Math.abs(xboxController.getRawAxis(4))));
// driveSubsystem.setChassisSpeed(new ChassisSpeeds(1, 0, 0)); // driveSubsystem.setChassisSpeed(new ChassisSpeeds(1, 0, 0));
Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates()); Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates());
Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates()); Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates());
Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose()); Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose());
} }
}, this)); }, this));
} }
@Override @Override
public void periodic() { public void periodic() {
super.periodic(); super.periodic();
driveSystem.periodic(); driveSystem.periodic();
} }
@Override @Override
public void simulationPeriodic() { public void simulationPeriodic() {
super.simulationPeriodic(); super.simulationPeriodic();
driveSystem.simulationPeriodic(); driveSystem.simulationPeriodic();
} }
@PostTest() @PostTest()
public void DriveSubsystem_CanDevicesConnected(){ public void DriveSubsystem_CanDevicesConnected(){
SwerveModule[] modules = driveSystem.getModules(); SwerveModule[] modules = driveSystem.getModules();
for(int i=0;i < modules.length; i++){ for(int i=0;i < modules.length; i++){
assertThat(modules[i].getDriveMotor().isConnected()).as(modules[i].getModuleName() + " drive motor is not connected").isTrue(); assertThat(modules[i].getDriveMotor().isConnected()).as(modules[i].getModuleName() + " drive motor is not connected").isTrue();
assertThat(modules[i].getSteerMotor().isConnected()).as(modules[i].getModuleName() + " steer motor is not connected").isTrue(); assertThat(modules[i].getSteerMotor().isConnected()).as(modules[i].getModuleName() + " steer motor is not connected").isTrue();
assertThat(modules[i].getSteerEncoder().isConnected()).as(modules[i].getModuleName() + " CANCoder is not connected").isTrue(); assertThat(modules[i].getSteerEncoder().isConnected()).as(modules[i].getModuleName() + " CANCoder is not connected").isTrue();
} }
assertThat(driveSystem.getGyro().isConnected()).as("Pigeon is not connected").isTrue(); assertThat(driveSystem.getGyro().isConnected()).as("Pigeon is not connected").isTrue();
} }
} }