From 9f017968e149d419ad704b21c923f10f251dac76 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Mon, 10 Feb 2025 18:33:37 -0500 Subject: [PATCH] swerve qui fonctionne --- Reefscape-tuner-project.json | 1 + src/main/java/frc/robot/RobotContainer.java | 7 +-- .../robot/TunerConstants/TunerConstants.java | 54 +++++++++---------- tuner-swerve-project2025.json | 1 + 4 files changed, 33 insertions(+), 30 deletions(-) create mode 100644 Reefscape-tuner-project.json create mode 100644 tuner-swerve-project2025.json diff --git a/Reefscape-tuner-project.json b/Reefscape-tuner-project.json new file mode 100644 index 0000000..ac720ea --- /dev/null +++ b/Reefscape-tuner-project.json @@ -0,0 +1 @@ +{"Version":"1.0.0.0","LastState":11,"Modules":[{"ModuleName":"Front Left","ModuleId":0,"Encoder":{"Id":12,"Name":"avant droit","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":5,"Name":"AvantDroitAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":4,"Name":"AvantDroitDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.353271484375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":5,"ValidatedDriveId":4,"ValidatedEncoderId":12},{"ModuleName":"Front Right","ModuleId":1,"Encoder":{"Id":9,"Name":"avant gauche","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":6,"Name":"avantGaucheAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":2,"Name":"AvantGaucheDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":0.2119140625,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":6,"ValidatedDriveId":2,"ValidatedEncoderId":9},{"ModuleName":"Back Left","ModuleId":2,"Encoder":{"Id":11,"Name":"arriere droit","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":8,"Name":"ArriereDroitAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":18,"Name":"ArriereDroiteDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":0.236572265625,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":8,"ValidatedDriveId":18,"ValidatedEncoderId":11},{"ModuleName":"Back Right","ModuleId":3,"Encoder":{"Id":10,"Name":"arriere gauche","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":7,"Name":"ArriereGaucheAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":3,"Name":"ArriereGaucheDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.330078125,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":7,"ValidatedDriveId":3,"ValidatedEncoderId":10}],"SwerveOptions":{"kSpeedAt12Volts":5.213368288877142,"Gyro":{"Id":13,"Name":"Pigeon 2 vers. S (Device ID 13)","Model":"Pigeon 2 vers. S","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"IsValidGyroCANbus":true,"VerticalTrackSizeInches":27.0,"HorizontalTrackSizeInches":21.0,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":4,"SwerveModuleConfiguration":{"ModuleBrand":4,"DriveRatio":6.122448979591837,"SteerRatio":21.428571428571427,"CouplingRatio":3.5714285714285716,"CustomName":"L3"},"HasVerifiedSteer":true,"SelectedModuleManufacturer":"Swerve Drive Specialties (SDS)","HasVerifiedDrive":true,"IsValidConfiguration":true}} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e7bb386..0a0ac8f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -59,9 +60,9 @@ public class RobotContainer { drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> - drive.withVelocityX(-manette1.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) - .withVelocityY(-manette1.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-manette1.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) + drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY(), 0.5)) // Drive forward with negative Y (forward) + .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX(), 0.5)) // Drive left with negative X (left) + .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.5)) // Drive counterclockwise with negative X (left) ) ); diff --git a/src/main/java/frc/robot/TunerConstants/TunerConstants.java b/src/main/java/frc/robot/TunerConstants/TunerConstants.java index fb38c67..e45fc2a 100644 --- a/src/main/java/frc/robot/TunerConstants/TunerConstants.java +++ b/src/main/java/frc/robot/TunerConstants/TunerConstants.java @@ -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 FrontLeft = diff --git a/tuner-swerve-project2025.json b/tuner-swerve-project2025.json new file mode 100644 index 0000000..116bb6d --- /dev/null +++ b/tuner-swerve-project2025.json @@ -0,0 +1 @@ +{"Version":"1.0.0.0","LastState":10,"Modules":[{"ModuleName":"Front Left","ModuleId":0,"Encoder":{"Id":9,"Name":"avant gauche","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":6,"Name":"avantGaucheAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":2,"Name":"AvantGaucheDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.046142578125,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":6,"ValidatedDriveId":2,"ValidatedEncoderId":9},{"ModuleName":"Front Right","ModuleId":1,"Encoder":{"Id":12,"Name":"avant droit","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":5,"Name":"AvantDroitAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":4,"Name":"AvantDroitDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.116455078125,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":5,"ValidatedDriveId":4,"ValidatedEncoderId":12},{"ModuleName":"Back Left","ModuleId":2,"Encoder":{"Id":10,"Name":"arriere gauche","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":7,"Name":"ArriereGaucheAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":3,"Name":"ArriereGaucheDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.046630859375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":7,"ValidatedDriveId":3,"ValidatedEncoderId":10},{"ModuleName":"Back Right","ModuleId":3,"Encoder":{"Id":11,"Name":"arriere droit","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":8,"Name":"ArriereDroitAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":18,"Name":"ArriereDroiteDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.016357421875,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":8,"ValidatedDriveId":18,"ValidatedEncoderId":11}],"SwerveOptions":{"kSpeedAt12Volts":5.213368288877142,"Gyro":{"Id":13,"Name":"Pigeon 2 vers. S (Device ID 13)","Model":"Pigeon 2 vers. S","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"IsValidGyroCANbus":true,"VerticalTrackSizeInches":27.0,"HorizontalTrackSizeInches":21.0,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":4,"SwerveModuleConfiguration":{"ModuleBrand":4,"DriveRatio":6.122448979591837,"SteerRatio":21.428571428571427,"CouplingRatio":3.5714285714285716,"CustomName":"L3"},"HasVerifiedSteer":true,"SelectedModuleManufacturer":"Swerve Drive Specialties (SDS)","HasVerifiedDrive":true,"IsValidConfiguration":true}} \ No newline at end of file