Initial YAGSL based swerve implementation (#13)

* Basic Swerve YAGSL implementation

---------

Co-authored-by: Alex <[email protected]>

src/main/deploy/swervePracticeBot/controllerproperties.json

@@ -0,0 +1,8 @@
+{
+  "angleJoystickRadiusDeadband": 0.5,
+  "heading": {
+    "p": 0.4,
+    "i": 0,
+    "d": 0.01
+  }
+}

src/main/deploy/swervePracticeBot/modules/backleft.json

@@ -0,0 +1,30 @@
+{
+  "location": {
+    "front": -9.75,
+    "left": 9.75
+  },
+  "absoluteEncoderOffset": -257.2,
+  "drive": {
+    "type": "sparkmax",
+    "id": 12,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": false,
+    "angle": true
+  },
+  "angle": {
+    "type": "sparkmax",
+    "id": 16,
+    "canbus": null
+  },
+  "conversionFactor": {
+    "angle": 0,
+    "drive": 0
+  },
+  "encoder": {
+    "type": "cancoder",
+    "id": 20,
+    "canbus": null
+  }
+}

src/main/deploy/swervePracticeBot/modules/backright.json

@@ -0,0 +1,30 @@
+{
+  "location": {
+    "front": -9.75,
+    "left": -9.75
+  },
+  "absoluteEncoderOffset": -293,
+  "drive": {
+    "type": "sparkmax",
+    "id": 13,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": true,
+    "angle": true
+  },
+  "angle": {
+    "type": "sparkmax",
+    "id": 17,
+    "canbus": null
+  },
+  "conversionFactor": {
+    "angle": 0,
+    "drive": 0
+  },
+  "encoder": {
+    "type": "cancoder",
+    "id": 21,
+    "canbus": null
+  }
+}

src/main/deploy/swervePracticeBot/modules/frontleft.json

@@ -0,0 +1,30 @@
+{
+  "location": {
+    "front": 9.75,
+    "left": 9.75
+  },
+  "absoluteEncoderOffset": -230,
+  "drive": {
+    "type": "sparkmax",
+    "id": 10,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": false,
+    "angle": true
+  },
+  "angle": {
+    "type": "sparkmax",
+    "id": 14,
+    "canbus": null
+  },
+  "conversionFactor": {
+    "angle": 0,
+    "drive": 0
+  },
+  "encoder": {
+    "type": "cancoder",
+    "id": 18,
+    "canbus": null
+  }
+}

src/main/deploy/swervePracticeBot/modules/frontright.json

@@ -0,0 +1,30 @@
+{
+  "location": {
+    "front": 9.75,
+    "left": -9.75
+  },
+  "absoluteEncoderOffset": -290,
+  "drive": {
+    "type": "sparkmax",
+    "id": 11,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": false,
+    "angle": true
+  },
+  "angle": {
+    "type": "sparkmax",
+    "id": 15,
+    "canbus": null
+  },
+  "conversionFactor": {
+    "angle": 0,
+    "drive": 0
+  },
+  "encoder": {
+    "type": "cancoder",
+    "id": 19,
+    "canbus": null
+  }
+}

src/main/deploy/swervePracticeBot/modules/physicalproperties.json

@@ -0,0 +1,16 @@
+{
+  "optimalVoltage": 12,
+  "wheelGripCoefficientOfFriction": 1.19,
+  "currentLimit": {
+    "drive": 40,
+    "angle": 20
+  },
+  "conversionFactor": {
+    "angle": 16.8,
+    "drive": 0.047286787200699704
+  },
+  "rampRate": {
+    "drive": 0.25,
+    "angle": 0.25
+  }
+}

src/main/deploy/swervePracticeBot/modules/pidfproperties.json

@@ -0,0 +1,16 @@
+{
+  "drive": {
+    "p": 0.0020645,
+    "i": 0,
+    "d": 0,
+    "f": 0,
+    "iz": 0
+  },
+  "angle": {
+    "p": 0.01,
+    "i": 0,
+    "d": 0,
+    "f": 0,
+    "iz": 0
+  }
+}

src/main/deploy/swervePracticeBot/swervedrive.json

@@ -0,0 +1,14 @@
+{
+  "imu": {
+    "type": "navx_mxp",
+    "id": 0,
+    "canbus": null
+  },
+  "invertedIMU": true,
+  "modules": [
+    "frontleft.json",
+    "frontright.json",
+    "backleft.json",
+    "backright.json"
+  ]
+}

src/main/java/frc/robot/Robot.java

@@ -57,7 +57,7 @@ public void robotInit() {
     } else {*/
     if (isReal()) {
       // Running on real robot
-      Logger.addDataReceiver(new WPILOGWriter("/media/sda1/")); // Log to a USB stick
+      //      Logger.addDataReceiver(new WPILOGWriter("/media/sda1/")); // Log to a USB stick
       Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
     } else {
       // Running in simulation

src/main/java/frc/robot/RobotContainer.java

@@ -4,12 +4,15 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.IntakeBaseCommand;
 import frc.robot.commands.ShooterEnable;
+import frc.robot.commands.drive.DriveCommand;
+import frc.robot.subsystems.drive.DriveSwerveYAGSL;
 import frc.robot.subsystems.intake.IntakeBase;
 import frc.robot.subsystems.intake.IntakeIOSim;
 import frc.robot.subsystems.intake.IntakeIOSparkMax;
@@ -21,6 +24,7 @@ public class RobotContainer {
   public final CommandXboxController controller;
   public final ShooterSubsystem shooter;
   public final IntakeBase intake;
+  public final DriveSwerveYAGSL drive;
 
   public RobotContainer() {
     controller = new CommandXboxController(0);
@@ -40,6 +44,8 @@ public RobotContainer() {
       intake = new IntakeBase(new IntakeIOSim());
     }
 
+    drive = new DriveSwerveYAGSL();
+
     configureBindings();
   }
 
@@ -52,6 +58,19 @@ private void configureBindings() {
             intake,
             () -> controller.rightBumper().getAsBoolean(),
             () -> controller.leftBumper().getAsBoolean()));
+
+    drive.setDefaultCommand(
+        new DriveCommand(
+            drive,
+            () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.01),
+            () -> MathUtil.applyDeadband(-controller.getLeftX(), 0.01),
+            () -> MathUtil.applyDeadband(-controller.getRightX(), 0.01)));
+    // TODO: Move deadband to constants file
+
+    controller
+        .start()
+        .onTrue(
+            new InstantCommand(() -> drive.setFieldOrientedDrive(!drive.isFieldOrientedDrive())));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/drive/DriveCommand.java

@@ -0,0 +1,34 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.drive.DriveBase;
+import java.util.function.DoubleSupplier;
+
+public class DriveCommand extends Command {
+  DriveBase drive;
+  DoubleSupplier speedX;
+  DoubleSupplier speedY;
+  DoubleSupplier rot;
+
+  public DriveCommand(
+      DriveBase drive, DoubleSupplier speedX, DoubleSupplier speedY, DoubleSupplier rot) {
+    this.drive = drive;
+    this.speedX = speedX;
+    this.speedY = speedY;
+    this.rot = rot;
+
+    addRequirements(drive);
+  }
+
+  @Override
+  public void execute() {
+    ChassisSpeeds speeds =
+        new ChassisSpeeds(
+            speedX.getAsDouble() * drive.getMaxLinearSpeed(),
+            speedY.getAsDouble() * drive.getMaxLinearSpeed(),
+            rot.getAsDouble() * drive.getMaxAngularSpeed());
+
+    drive.runVelocity(speeds);
+  }
+}

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -0,0 +1,26 @@
+package frc.robot.subsystems.drive;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+public interface Drive {
+  /**
+   * Sets the desired chassis speed
+   *
+   * @param velocity specifies the setpoint velocity for the chassis
+   */
+  public void runVelocity(ChassisSpeeds velocity);
+
+  /**
+   * This returns the robot's max linear speed in meter/sec
+   *
+   * @return speed in meter/sec
+   */
+  public double getMaxLinearSpeed();
+
+  /**
+   * This returns the robot's max angular speed in radian/sec
+   *
+   * @return speed in radian/sec
+   */
+  public double getMaxAngularSpeed();
+}

src/main/java/frc/robot/subsystems/drive/DriveBase.java

@@ -0,0 +1,19 @@
+package frc.robot.subsystems.drive;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class DriveBase extends SubsystemBase implements Drive {
+  @Override
+  public void runVelocity(ChassisSpeeds velocity) {}
+
+  @Override
+  public double getMaxLinearSpeed() {
+    return 0;
+  }
+
+  @Override
+  public double getMaxAngularSpeed() {
+    return 0;
+  }
+}

src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java

@@ -0,0 +1,52 @@
+package frc.robot.subsystems.drive;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Filesystem;
+import java.io.File;
+import swervelib.SwerveDrive;
+import swervelib.parser.SwerveParser;
+
+public class DriveSwerveYAGSL extends DriveBase {
+  private final double maximumSpeed =
+      Units.feetToMeters(4.5); // * TODO: Calculate actual max speed */
+  private final File swerveJsonDirectory =
+      new File(Filesystem.getDeployDirectory(), "swervePracticeBot");
+  private SwerveDrive swerveDrive;
+  private boolean fieldOrientedDrive = false;
+
+  public DriveSwerveYAGSL() {
+    try {
+      swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(maximumSpeed);
+    } catch (Exception e) {
+      throw new RuntimeException(e);
+    }
+  }
+
+  @Override
+  public void runVelocity(ChassisSpeeds velocity) {
+    if (fieldOrientedDrive) {
+      swerveDrive.driveFieldOriented(velocity);
+    } else {
+      swerveDrive.drive(velocity);
+    }
+  }
+
+  @Override
+  public double getMaxLinearSpeed() {
+    return swerveDrive.getMaximumVelocity();
+  }
+
+  @Override
+  public double getMaxAngularSpeed() {
+    return swerveDrive.getMaximumAngularVelocity();
+  }
+
+  public void setFieldOrientedDrive(boolean enable) {
+    fieldOrientedDrive = enable;
+  }
+
+  public boolean isFieldOrientedDrive() {
+    return fieldOrientedDrive;
+  }
+}