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

Working but offsets are busted

This commit is contained in:
Tyler Wilcox 2026-01-29 20:54:04 -05:00
parent 4a252beb2b
commit bf25b5b4c7
2 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/RoboLib

@ -1 +1 @@
Subproject commit bb59c528b79864a79737b192cd058ad892d43307 Subproject commit f9c7df64baa94652ae0f8f154962aac47069d8f0

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

@ -11,6 +11,7 @@ import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
@ -44,10 +45,10 @@ public class RobotContainer {
private double driveD = 0.01; private double driveD = 0.01;
private double driveKV = 0.2; private double driveKV = 0.2;
private double steerP = 0.1; private double steerP = 50;
private double steerI = 0.0; private double steerI = 1;
private double steerD = 0.0; private double steerD = 1;
private double steerKV = 0.1; private double steerKV = 0;
/** The container for the robot. Contains subsystems, OI devices, and commands. */ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() { public RobotContainer() {
@ -73,6 +74,7 @@ public class RobotContainer {
.withSteerKI(steerI) .withSteerKI(steerI)
.withSteerKD(steerD) .withSteerKD(steerD)
.withSteerKV(steerKV) .withSteerKV(steerKV)
.withSteerOffset(0.048828)
.withSteerEncoderId(10) .withSteerEncoderId(10)
.withModuleLocation(new Translation2d(1, 1)) .withModuleLocation(new Translation2d(1, 1))
.withModuleName("Front Left") .withModuleName("Front Left")
@ -87,11 +89,13 @@ public class RobotContainer {
.withDriveKI(driveI) .withDriveKI(driveI)
.withDriveKD(driveD) .withDriveKD(driveD)
.withDriveKV(driveKV) .withDriveKV(driveKV)
.withDriveInverted()
.withSteerMotorId(6) .withSteerMotorId(6)
.withSteerKP(steerP) .withSteerKP(steerP)
.withSteerKI(steerI) .withSteerKI(steerI)
.withSteerKD(steerD) .withSteerKD(steerD)
.withSteerKV(steerKV) .withSteerKV(steerKV)
.withSteerOffset(0.244875)
.withSteerEncoderId(1) .withSteerEncoderId(1)
.withModuleLocation(new Translation2d(1, -1)) .withModuleLocation(new Translation2d(1, -1))
.withModuleName("Front Right") .withModuleName("Front Right")
@ -110,6 +114,7 @@ public class RobotContainer {
.withSteerKI(steerI) .withSteerKI(steerI)
.withSteerKD(steerD) .withSteerKD(steerD)
.withSteerKV(steerKV) .withSteerKV(steerKV)
.withSteerOffset(0.430176)
.withSteerEncoderId(7) .withSteerEncoderId(7)
.withModuleLocation(new Translation2d(-1, 1)) .withModuleLocation(new Translation2d(-1, 1))
.withModuleName("Back Left") .withModuleName("Back Left")
@ -123,11 +128,13 @@ public class RobotContainer {
.withDriveKI(driveI) .withDriveKI(driveI)
.withDriveKD(driveD) .withDriveKD(driveD)
.withDriveKV(driveKV) .withDriveKV(driveKV)
.withDriveInverted()
.withSteerMotorId(11) .withSteerMotorId(11)
.withSteerKP(steerP) .withSteerKP(steerP)
.withSteerKI(steerI) .withSteerKI(steerI)
.withSteerKD(steerD) .withSteerKD(steerD)
.withSteerKV(steerKV) .withSteerKV(steerKV)
.withSteerOffset(0.012695)
.withSteerEncoderId(4) .withSteerEncoderId(4)
.withModuleLocation(new Translation2d(-1, -1)) .withModuleLocation(new Translation2d(-1, -1))
.withModuleName("Back Right") .withModuleName("Back Right")
@ -164,23 +171,16 @@ public class RobotContainer {
@Override @Override
public void run() { public void run() {
driveSubsystem.setChassisSpeedsFromJoystickFieldRelative( driveSubsystem.setChassisSpeedsFromJoystickRobotRelative(
-(m_driverController.getLeftY() * Math.abs(m_driverController.getLeftY())), -(m_driverController.getLeftY() * Math.abs(m_driverController.getLeftY())),
-(m_driverController.getLeftX() * Math.abs(m_driverController.getLeftX())), -(m_driverController.getLeftX() * Math.abs(m_driverController.getLeftX())),
-(m_driverController.getRawAxis(4) * Math.abs(m_driverController.getRawAxis(4)))); -(m_driverController.getRawAxis(4) * Math.abs(m_driverController.getRawAxis(4))));
// driveSubsystem.setChassisSpeed(new ChassisSpeeds(0, 0, 0)); // driveSubsystem.setChassisSpeed(new ChassisSpeeds(1, 0, 0));
Logger.recordOutput("SwerveState", driveSubsystem.getModuleStates()); Logger.recordOutput("RequestedSwerveState", driveSubsystem.getRequestedModuleStates());
Logger.recordOutput("ActualSwerveState", driveSubsystem.getActualModuleStates());
Logger.recordOutput("Pose Estimation", driveSubsystem.getRobotPose()); Logger.recordOutput("Pose Estimation", driveSubsystem.getRobotPose());
for(int i = 0;i<driveSubsystem.getNumberOfModules();i++){
SwerveModule mod = driveSubsystem.getModules()[i];
String modName = mod.getModuleName();
Logger.recordOutput(modName + " wheel speed", mod.getDriveWheelSpeed());
Logger.recordOutput(modName + " steer rotation", mod.getSteerRotations());
}
} }
}, driveSubsystem); }, driveSubsystem);