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..3ae3fdd --- /dev/null +++ b/src/main/deploy/swerve/modules/backleft.json @@ -0,0 +1,26 @@ +{ + "location": { + "front": -12.375, + "left": 12.375 + }, + "absoluteEncoderOffset": 0, + "drive": { + "type": "sparkmax", + "id": 0, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 0, + "canbus": null + }, + "encoder": { + "type": "cancoder", + "id": 0, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + } +} \ 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..c896a6e --- /dev/null +++ b/src/main/deploy/swerve/modules/backright.json @@ -0,0 +1,26 @@ +{ + "location": { + "front": -12.375, + "left": -12.375 + }, + "absoluteEncoderOffset": 0, + "drive": { + "type": "sparkmax", + "id": 0, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 0, + "canbus": null + }, + "encoder": { + "type": "cancoder", + "id": 0, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + } +} \ 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..018c523 --- /dev/null +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -0,0 +1,26 @@ +{ + "location": { + "front": 12.375, + "left": 12.375 + }, + "absoluteEncoderOffset": 0, + "drive": { + "type": "sparkmax", + "id": 0, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 0, + "canbus": null + }, + "encoder": { + "type": "cancoder", + "id": 0, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + } +} \ 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..d3ccba2 --- /dev/null +++ b/src/main/deploy/swerve/modules/frontright.json @@ -0,0 +1,26 @@ +{ + "location": { + "front": 12.375, + "left": -12.375 + }, + "absoluteEncoderOffset": 0, + "drive": { + "type": "sparkmax", + "id": 0, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 0, + "canbus": null + }, + "encoder": { + "type": "cancoder", + "id": 0, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + } +} \ 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..d463416 --- /dev/null +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -0,0 +1,20 @@ +{ + "wheelDiameter": 4, + "wheelGripCoefficientOfFriction": 1.19, + "gearRatio": { + "drive": 8.14, + "angle": 21.43 + }, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + }, + "encoderPulsePerRotation": { + "drive": 1, + "angle": 1 + } +} \ 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..3e2c5df --- /dev/null +++ b/src/main/deploy/swerve/swervedrive.json @@ -0,0 +1,16 @@ +{ + "maxSpeed": 12, + "optimalVoltage": 12, + "imu": { + "type": "navx", + "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/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index d73f6e4..7337a17 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -5,14 +5,31 @@ package frc.robot.subsystems; import java.io.File; +import java.io.IOException; +import edu.wpi.first.math.geometry.Translation2d; 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 { + SwerveDrive swerveDrive; File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(),"swerve"); + + public void drive(double x, double y, double zRotation){ + swerveDrive.drive(new Translation2d(x, y), zRotation, true, true); + } + /** Creates a new Drive. */ - public Drive() {} + public Drive() { + try { + this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(); + } catch (IOException e) { + e.printStackTrace(); + } + } @Override public void periodic() {