swerve qui fonctionne

This commit is contained in:
samuel desharnais
2025-02-10 18:33:37 -05:00
parent 029bba7bb6
commit 9f017968e1
4 changed files with 33 additions and 30 deletions

View File

@ -25,7 +25,7 @@ public class TunerConstants {
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
private static final Slot0Configs steerGains = new Slot0Configs()
.withKP(100).withKI(0).withKD(0.5)
.withKS(0.1).withKV(1.91).withKA(0)
.withKS(0.1).withKV(2.66).withKA(0)
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
// When using closed-loop control, the drive motor uses the control
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
@ -78,10 +78,10 @@ public class TunerConstants {
// Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
// This may need to be tuned to your individual robot
private static final double kCoupleRatio = 0;
private static final double kCoupleRatio = 3.5714285714285716;
private static final double kDriveGearRatio = 6.122448979591837;
private static final double kSteerGearRatio = 15.42857142857143;
private static final double kSteerGearRatio = 21.428571428571427;
private static final Distance kWheelRadius = Inches.of(2);
private static final boolean kInvertLeftSide = false;
@ -126,48 +126,48 @@ public class TunerConstants {
// Front Left
private static final int kFrontLeftDriveMotorId = 2;
private static final int kFrontLeftSteerMotorId = 6;
private static final int kFrontLeftEncoderId = 9;
private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.046142578125);
private static final int kFrontLeftDriveMotorId = 4;
private static final int kFrontLeftSteerMotorId = 5;
private static final int kFrontLeftEncoderId = 12;
private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.353271484375);
private static final boolean kFrontLeftSteerMotorInverted = true;
private static final boolean kFrontLeftEncoderInverted = false;
private static final Distance kFrontLeftXPos = Inches.of(13.75);
private static final Distance kFrontLeftYPos = Inches.of(13.75);
private static final Distance kFrontLeftXPos = Inches.of(13.5);
private static final Distance kFrontLeftYPos = Inches.of(10.5);
// Front Right
private static final int kFrontRightDriveMotorId = 4;
private static final int kFrontRightSteerMotorId = 5;
private static final int kFrontRightEncoderId = 12;
private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.116455078125);
private static final int kFrontRightDriveMotorId = 2;
private static final int kFrontRightSteerMotorId = 6;
private static final int kFrontRightEncoderId = 9;
private static final Angle kFrontRightEncoderOffset = Rotations.of(0.2119140625);
private static final boolean kFrontRightSteerMotorInverted = true;
private static final boolean kFrontRightEncoderInverted = false;
private static final Distance kFrontRightXPos = Inches.of(13.75);
private static final Distance kFrontRightYPos = Inches.of(-13.75);
private static final Distance kFrontRightXPos = Inches.of(13.5);
private static final Distance kFrontRightYPos = Inches.of(-10.5);
// Back Left
private static final int kBackLeftDriveMotorId = 3;
private static final int kBackLeftSteerMotorId = 7;
private static final int kBackLeftEncoderId = 10;
private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.046630859375);
private static final int kBackLeftDriveMotorId = 18;
private static final int kBackLeftSteerMotorId = 8;
private static final int kBackLeftEncoderId = 11;
private static final Angle kBackLeftEncoderOffset = Rotations.of(0.236572265625);
private static final boolean kBackLeftSteerMotorInverted = true;
private static final boolean kBackLeftEncoderInverted = false;
private static final Distance kBackLeftXPos = Inches.of(-13.75);
private static final Distance kBackLeftYPos = Inches.of(13.75);
private static final Distance kBackLeftXPos = Inches.of(-13.5);
private static final Distance kBackLeftYPos = Inches.of(10.5);
// Back Right
private static final int kBackRightDriveMotorId = 18;
private static final int kBackRightSteerMotorId = 8;
private static final int kBackRightEncoderId = 11;
private static final Angle kBackRightEncoderOffset = Rotations.of(-0.016357421875);
private static final int kBackRightDriveMotorId = 3;
private static final int kBackRightSteerMotorId = 7;
private static final int kBackRightEncoderId = 10;
private static final Angle kBackRightEncoderOffset = Rotations.of(-0.330078125);
private static final boolean kBackRightSteerMotorInverted = true;
private static final boolean kBackRightEncoderInverted = false;
private static final Distance kBackRightXPos = Inches.of(-13.75);
private static final Distance kBackRightYPos = Inches.of(-13.75);
private static final Distance kBackRightXPos = Inches.of(-13.5);
private static final Distance kBackRightYPos = Inches.of(-10.5);
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =