Se/swerve

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -6,7 +6,9 @@
 package com.stuypulse.robot;
 
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
+import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
 import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.robot.util.BootlegXbox;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
@@ -38,7 +40,10 @@ public RobotContainer() {
     /*** DEFAULTS ***/
     /****************/
 
-    private void configureDefaultCommands() {}
+    private void configureDefaultCommands() {
+        // Swerve
+        SwerveDrive.getInstance().setDefaultCommand(new SwerveDriveDrive(driver));
+    }
 
     /***************/
     /*** BUTTONS ***/

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java

@@ -0,0 +1,87 @@
+package com.stuypulse.robot.commands.swerve;
+
+import java.util.Optional;
+
+import com.stuypulse.robot.constants.Settings.Driver.Drive;
+import com.stuypulse.robot.constants.Settings.Driver.Turn;
+import com.stuypulse.robot.constants.Settings.Driver.Turn.GyroFeedback;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.stuylib.control.Controller;
+import com.stuypulse.stuylib.control.angle.AngleController;
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
+import com.stuypulse.stuylib.input.Gamepad;
+import com.stuypulse.stuylib.math.Angle;
+import com.stuypulse.stuylib.streams.IStream;
+import com.stuypulse.stuylib.streams.vectors.VStream;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class SwerveDriveDrive extends CommandBase {
+
+    private SwerveDrive swerve;
+
+    private VStream speed; 
+    private IStream turn;
+
+    private final Gamepad driver;
+
+    private Optional<Rotation2d> holdAngle;
+    private AngleController gyroFeedback;
+
+    public SwerveDriveDrive(Gamepad driver) {
+        this.driver = driver;
+        swerve = SwerveDrive.getInstance();
+
+        speed = VStream.create(driver::getLeftStick);
+        turn = IStream.create(driver::getRightX);       
+
+        holdAngle = Optional.empty();
+        gyroFeedback = new AnglePIDController(GyroFeedback.P, GyroFeedback.I, GyroFeedback.D);
+
+        addRequirements(swerve);
+    }
+
+    @Override
+    public void initialize() {
+        holdAngle = Optional.empty();
+    }
+
+    private boolean isWithinTurnDeadband() {
+        return Math.abs(turn.get()) < Turn.DEADBAND.get();
+    }
+
+    private boolean isWithinDriveDeadband() {
+        return Math.abs(speed.get().magnitude()) < Drive.DEADBAND.get();
+    }
+
+   @Override
+    public void execute() {
+        double angularVel = turn.get();
+
+        if(isWithinTurnDeadband()){
+            if(holdAngle.isEmpty()) {
+                holdAngle = Optional.of(swerve.getGyroAngle());
+            }
+
+            if(GyroFeedback.GYRO_FEEDBACK_ENABLED.get() && !isWithinDriveDeadband()) {
+                angularVel = -gyroFeedback.update(
+                    Angle.fromRotation2d(holdAngle.get()),
+                    Angle.fromRotation2d(swerve.getGyroAngle())
+                );
+            }
+        }
+        else {
+            holdAngle = Optional.empty(); 
+        }
+
+        if(driver.getRawStartButton() || driver.getRawSelectButton()) {
+            swerve.setXMode();
+        }
+        else {
+            swerve.drive(speed.get(), angularVel);
+        }
+
+    }
+}

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -8,6 +8,10 @@
 import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+
 /*-
  * File containing tunable settings for every subsystem on the robot.
  *
@@ -16,4 +20,92 @@
  */
 public interface Settings {
 	double DT = 0.02;
+
+	public interface Swerve {
+        // TODO: Ask Harry for drivetrain dimensions
+        double WIDTH = Units.inchesToMeters(0);
+        double LENGTH = Units.inchesToMeters(0);
+
+		SmartNumber MODULE_VELOCITY_DEADBAND = new SmartNumber("Swerve/Module velocity deadband (m per s)", 0.02);
+		SmartNumber MAX_MODULE_SPEED = new SmartNumber("Swerve/Maximum module speed (rad per s)", 5); // Ask Harry for max theoretical speed
+		SmartNumber MAX_MODULE_TURN = new SmartNumber("Swerve/Maximum module turn (m per s)", 6.28); // Ask Harry for max theoretical turn
+
+        public interface Turn {
+            SmartNumber kP = new SmartNumber("Swerve/Turn/kP", 3.5);
+            SmartNumber kI = new SmartNumber("Swerve/Turn/kI", 0.0);
+            SmartNumber kD = new SmartNumber("Swerve/Turn/kD", 0.1);
+
+            SmartNumber kV = new SmartNumber("Swerve/Turn/kV", 0.25);
+            SmartNumber kA = new SmartNumber("Swerve/Turn/kA", 0.007);
+        }
+
+        public interface Drive {
+            SmartNumber kP = new SmartNumber("Swerve/Drive/kP", 0.8);
+            SmartNumber kI = new SmartNumber("Swerve/Drive/kI", 0.0);
+            SmartNumber kD = new SmartNumber("Swerve/Drive/kD", 0.0);
+
+            SmartNumber kS = new SmartNumber("Swerve/Drive/kV", 0.2230);
+            SmartNumber kV = new SmartNumber("Swerve/Drive/kV", 2.4899);
+            SmartNumber kA = new SmartNumber("Swerve/Drive/kA", 0.41763);
+        }
+
+        // measure offsets
+ 		public interface FrontRight {
+            String ID = "Front Right";
+            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(0)
+                .plus(Rotation2d.fromDegrees(0));
+            Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * -0.5);
+        }
+
+        public interface FrontLeft {
+            String ID = "Front Left";
+            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(0)
+                .plus(Rotation2d.fromDegrees(270));
+            Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * +0.5);
+        }
+
+        public interface BackLeft {
+            String ID = "Back Left";
+            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(0)
+                .plus(Rotation2d.fromDegrees(180));
+            Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * +0.5);
+        }
+
+        public interface BackRight {
+            String ID = "Back Right";
+            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(0)
+                .plus(Rotation2d.fromDegrees(90));
+            Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * -0.5);
+        }
+	}
+
+    public interface Driver {
+        public interface Drive {
+            SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.03);
+
+            SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.2);
+            SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 1.5);
+
+            SmartNumber MAX_TELEOP_SPEED = new SmartNumber("Driver Settings/Drive/Max Speed", Swerve.MAX_MODULE_SPEED.get());
+            SmartNumber MAX_TELEOP_ACCEL = new SmartNumber("Driver Settings/Drive/Max Accleration", 15);
+        }
+
+        public interface Turn {
+            SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.05);
+
+            SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.15);
+            SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2);
+
+            SmartNumber MAX_TELEOP_TURNING = new SmartNumber("Driver Settings/Turn/Max Turning", 6.0);
+
+            public interface GyroFeedback {
+                SmartBoolean GYRO_FEEDBACK_ENABLED = new SmartBoolean("Driver Settings/Gyro Feedback/Enabled", true);
+
+                SmartNumber P = new SmartNumber("Driver Settings/Gyro Feedback/kP", 0.5);
+                SmartNumber I = new SmartNumber("Driver Settings/Gyro Feedback/kI", 0.0);
+                SmartNumber D = new SmartNumber("Driver Settings/Gyro Feedback/kD", 0.1);
+            }
+        }
+
+    }
 }