From b1efbc83733866b0090143d2ae7c54e9c9302fae Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Mon, 22 Jan 2024 19:33:18 -0500 Subject: [PATCH] Swerve + Constant --- .../deploy/swerve/controllerproperties.json | 8 ++++ src/main/deploy/swerve/modules/backleft.json | 30 ++++++++++++ src/main/deploy/swerve/modules/backright.json | 30 ++++++++++++ src/main/deploy/swerve/modules/frontleft.json | 30 ++++++++++++ .../deploy/swerve/modules/frontright.json | 30 ++++++++++++ .../swerve/modules/physicalproperties.json | 16 +++++++ .../deploy/swerve/modules/pidfproperties.json | 16 +++++++ src/main/deploy/swerve/swervedrive.json | 14 ++++++ src/main/java/frc/robot/Constants.java | 48 ++++++++++++++----- src/main/java/frc/robot/subsystem/Drive.java | 31 +++++++++++- 10 files changed, 240 insertions(+), 13 deletions(-) create mode 100644 src/main/deploy/swerve/controllerproperties.json create mode 100644 src/main/deploy/swerve/modules/backleft.json create mode 100644 src/main/deploy/swerve/modules/backright.json create mode 100644 src/main/deploy/swerve/modules/frontleft.json create mode 100644 src/main/deploy/swerve/modules/frontright.json create mode 100644 src/main/deploy/swerve/modules/physicalproperties.json create mode 100644 src/main/deploy/swerve/modules/pidfproperties.json create mode 100644 src/main/deploy/swerve/swervedrive.json diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json new file mode 100644 index 0000000..669097e --- /dev/null +++ b/src/main/deploy/swerve/controllerproperties.json @@ -0,0 +1,8 @@ +{ + "angleJoystickRadiusDeadband": 0.5, + "heading": { + "p": 0.4, + "i": 0, + "d": 0.01 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json new file mode 100644 index 0000000..a9a2d30 --- /dev/null +++ b/src/main/deploy/swerve/modules/backleft.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": -12.375, + "left": 12.375 + }, + "absoluteEncoderOffset": 80.332, + "drive": { + "type": "sparkmax", + "id": 8, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 9, + "canbus": null + }, + "inverted": { + "angle": true, + "drive": false + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "encoder": { + "type": "cancoder", + "id": 7, + "canbus": null + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json new file mode 100644 index 0000000..679f08d --- /dev/null +++ b/src/main/deploy/swerve/modules/backright.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": -12.375, + "left": -12.375 + }, + "absoluteEncoderOffset": 28.74, + "drive": { + "type": "sparkmax", + "id": 11, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 12, + "canbus": null + }, + "inverted": { + "angle": true, + "drive": false + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "encoder": { + "type": "cancoder", + "id": 6, + "canbus": null + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json new file mode 100644 index 0000000..3c37753 --- /dev/null +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": 12.375, + "left": 12.375 + }, + "absoluteEncoderOffset": 98.438, + "drive": { + "type": "sparkmax", + "id": 2, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 3, + "canbus": null + }, + "inverted": { + "angle": true, + "drive": false + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "encoder": { + "type": "cancoder", + "id": 4, + "canbus": null + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json new file mode 100644 index 0000000..87f8cd9 --- /dev/null +++ b/src/main/deploy/swerve/modules/frontright.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": 12.375, + "left": -12.375 + }, + "absoluteEncoderOffset": 17.227, + "drive": { + "type": "sparkmax", + "id": 17, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 18, + "canbus": null + }, + "inverted": { + "angle": true, + "drive": false + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "encoder": { + "type": "cancoder", + "id": 5, + "canbus": null + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json new file mode 100644 index 0000000..4994775 --- /dev/null +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -0,0 +1,16 @@ +{ + "optimalVoltage": 12, + "wheelGripCoefficientOfFriction": 1.19, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "conversionFactor": { + "angle": 16.8, + "drive": 0.04 + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json new file mode 100644 index 0000000..3834a36 --- /dev/null +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -0,0 +1,16 @@ +{ + "drive": { + "p": 0.0020645, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + }, + "angle": { + "p": 0.0020645, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json new file mode 100644 index 0000000..1604d61 --- /dev/null +++ b/src/main/deploy/swerve/swervedrive.json @@ -0,0 +1,14 @@ +{ + "imu": { + "type": "pigeon", + "id": 0, + "canbus": null + }, + "invertedIMU": false, + "modules": [ + "frontleft.json", + "frontright.json", + "backleft.json", + "backright.json" + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 04557d8..27c116c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,17 +6,41 @@ package frc.robot; /** Add your docs here. */ public class Constants { - //moteur - public static int lanceur = 1; - public static int lancer2 = 2; - public static int roue = 3; - public static int etoile = 4; - public static int Moteuracc2 = 5; - public static int Moteuracc = 6; - public static int guideur = 7; - // limit switsh - public static int guideurhaut = 8; - public static int guideurbas = 9; - public static int limitacc = 58; + // Lanceur + public static int lanceur = 10; + public static int lancer2 = 13; + public static int lancer3 = 14; + public static int lancer4 = 15; + // Ballayeuse + public static int roue = 16; + public static int etoile = 19; + + // Accumulateur + public static int Moteuracc2 = 20; + public static int Moteuracc = 21; + + // Guideur + public static int guideur = 22; + + // Swerve + public static int AvantDroitDrive = 17; + public static int AvantDroitAngle = 18; + public static int AvantGaucheDrive = 2; + public static int AvantGaucheAngle = 3; + public static int ArriereDroitDrive = 11; + public static int ArriereDroitAngle = 12; + public static int ArriereGaucheDrive = 8; + public static int ArriereGaucheAngle = 9; + + // CanCoder + public static int AvantDroit = 5; + public static int AvantGauche = 4; + public static int ArriereDroit = 6; + public static int ArriereGauche = 7; + + // Limit switsh + public static int guideurhaut = 23; + public static int guideurbas = 24; + public static int limitacc = 25; } diff --git a/src/main/java/frc/robot/subsystem/Drive.java b/src/main/java/frc/robot/subsystem/Drive.java index 9d4d95a..fa7ba39 100644 --- a/src/main/java/frc/robot/subsystem/Drive.java +++ b/src/main/java/frc/robot/subsystem/Drive.java @@ -4,11 +4,40 @@ package frc.robot.subsystem; +import java.io.File; +import java.io.IOException; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import swervelib.SwerveDrive; +import swervelib.parser.SwerveParser; public class Drive extends SubsystemBase { /** Creates a new Drive. */ - public Drive() {} + + SwerveDrive swerveDrive; + File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(),"swerve"); + public void drive(double x, double y, double zRotation){ + swerveDrive.drive(new Translation2d(x*2, y*2), zRotation, true, false); + } + + public Drive() { + + try { + this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(2); + } catch (IOException e) { + e.printStackTrace(); + } + } + + public SwerveModulePosition[] distance(){ + return swerveDrive.getModulePositions(); + } +public void reset(){ + +} @Override public void periodic() {