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

Reorganization

This commit is contained in:
Tyler Wilcox 2026-01-30 22:25:14 -05:00
parent dddd9096a0
commit 568d3b0bf8
3 changed files with 169 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/RoboLib

@ -1 +1 @@
Subproject commit 8defb590f40120ea22befb0e0880bcbb2636b502
Subproject commit 817515901ca237b9f47e2001f7d65f9e38249eb1

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

@ -4,18 +4,13 @@
package frc.robot;
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder;
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSubsystem;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.OperatorConstants;
import frc.robot.commands.Autos;
import frc.robot.commands.ExampleCommand;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.ExampleSubsystem;
/**
@ -34,113 +29,13 @@ public class RobotContainer {
// private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
public static SwerveDriveSubsystem driveSubsystem;
private double driveP = 0.1;
private double driveI = 0.0;
private double driveD = 0.01;
private double driveKV = 0.2;
private double steerP = 50;
private double steerI = 1;
private double steerD = 1;
private double steerKV = 0;
public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the trigger bindings
configureBindings();
SwerveBuilder swerveBuilder = new SwerveBuilder();
driveSubsystem = swerveBuilder
.withCanbusName("Drive")
.withPigeonId(20)
.withMaxLinearVelocity(3.0)
.withMaxAngularVelocity(Math.PI * 2)
.withSwerveModule(
swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(12)
.withDriveKP(driveP)
.withDriveKI(driveI)
.withDriveKD(driveD)
.withDriveKV(driveKV)
.withSteerMotorId(3)
.withSteerKP(steerP)
.withSteerKI(steerI)
.withSteerKD(steerD)
.withSteerKV(steerKV)
.withSteerOffset(0.048828)
.withSteerEncoderId(10)
.withModuleLocation(new Translation2d(1, 1))
.withModuleName("Front Left")
.withGearRatio(8.14)
.withWheelDiameter(0.1016)
.build()
)
.withSwerveModule(
swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(5)
.withDriveKP(driveP)
.withDriveKI(driveI)
.withDriveKD(driveD)
.withDriveKV(driveKV)
.withDriveInverted()
.withSteerMotorId(6)
.withSteerKP(steerP)
.withSteerKI(steerI)
.withSteerKD(steerD)
.withSteerKV(steerKV)
.withSteerOffset(0.244875)
.withSteerEncoderId(1)
.withModuleLocation(new Translation2d(1, -1))
.withModuleName("Front Right")
.withGearRatio(8.14)
.withWheelDiameter(0.1016)
.build())
.withSwerveModule(
swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(8)
.withDriveKP(driveP)
.withDriveKI(driveI)
.withDriveKD(driveD)
.withDriveKV(driveKV)
.withSteerMotorId(2)
.withSteerKP(steerP)
.withSteerKI(steerI)
.withSteerKD(steerD)
.withSteerKV(steerKV)
.withSteerOffset(0.430176)
.withSteerEncoderId(7)
.withModuleLocation(new Translation2d(-1, 1))
.withModuleName("Back Left")
.withGearRatio(8.14)
.withWheelDiameter(0.1016)
.build())
.withSwerveModule(
swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(9)
.withDriveKP(driveP)
.withDriveKI(driveI)
.withDriveKD(driveD)
.withDriveKV(driveKV)
.withDriveInverted()
.withSteerMotorId(11)
.withSteerKP(steerP)
.withSteerKI(steerI)
.withSteerKD(steerD)
.withSteerKV(steerKV)
.withSteerOffset(0.012695)
.withSteerEncoderId(4)
.withModuleLocation(new Translation2d(-1, -1))
.withModuleName("Back Right")
.withGearRatio(8.14)
.withWheelDiameter(0.1016)
.build())
.build();
driveSubsystem.setDefaultCommand(getDriveCommand());
}
/**
@ -162,26 +57,6 @@ public class RobotContainer {
// m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
}
public Command getDriveCommand(){
return Commands.run(new Runnable() {
@Override
public void run() {
driveSubsystem.setChassisSpeedsFromJoystickRobotRelative(
-(m_driverController.getLeftY() * Math.abs(m_driverController.getLeftY())),
-(m_driverController.getLeftX() * Math.abs(m_driverController.getLeftX())),
-(m_driverController.getRawAxis(4) * Math.abs(m_driverController.getRawAxis(4))));
// driveSubsystem.setChassisSpeed(new ChassisSpeeds(1, 0, 0));
Logger.recordOutput("RequestedSwerveState", driveSubsystem.getRequestedModuleStates());
Logger.recordOutput("ActualSwerveState", driveSubsystem.getActualModuleStates());
Logger.recordOutput("Pose Estimation", driveSubsystem.getRobotPose());
}
}, driveSubsystem);
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*

166
src/main/java/frc/robot/subsystems/DriveSubsystem.java Normal file
View File

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