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.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,76 @@
+package com.stuypulse.robot.commands.swerve;
+
+import java.util.Optional;
+
+import com.stuypulse.robot.constants.Settings.Swerve;
+import com.stuypulse.robot.subsystems.SwerveDrive;
+import com.stuypulse.stuylib.input.Gamepad;
+import com.stuypulse.stuylib.streams.IStream;
+import com.stuypulse.stuylib.streams.vectors.VStream;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+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;
+
+    public SwerveDriveDrive(Gamepad driver) {
+        this.driver = driver;
+        swerve = SwerveDrive.getInstance();
+
+        speed = VStream.create(driver::getLeftStick);
+        turn = IStream.create(driver::getRightX);       
+
+        holdAngle = Optional.empty();
+
+        addRequirements(swerve);
+
+
+    }
+
+    @Override
+    public void initialize() {
+        holdAngle = Optional.empty();
+    }
+
+    private boolean isWithinTurnDeadband() {
+        return Math.abs(turn.get()) < Swerve.MAX_MODULE_TURN.get();
+    }
+
+    private boolean isWithinDriveDeadband() {
+        return Math.abs(speed.get().magnitude()) < Swerve.MAX_MODULE_SPEED.get();
+    }
+
+   @Override
+    public void execute() {
+        swerve.drive(speed.get(), turn.get());
+
+
+        if(isWithinTurnDeadband()){
+            if(holdAngle.isEmpty()) {
+                holdAngle = Optional.of(swerve.getGyroAngle());
+            }
+
+        }
+        /*if (isWithinDriveDeadband()) {
+            if () {
+
+            }
+            if () {
+
+            }
+        }
+
+        else {
+
+        }*/
+    }
+}

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

@@ -5,9 +5,12 @@
 
 package com.stuypulse.robot.constants;
 
-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 +19,61 @@
  */
 public interface Settings {
 	double DT = 0.02;
+
+	public interface Swerve {
+        double WIDTH = Units.inchesToMeters(26.504);
+        double LENGTH = Units.inchesToMeters(20.508);
+
+		SmartNumber MODULE_SPEED_DEADBAND = new SmartNumber("Swerve/Module speed deadband", 0.02);
+		SmartNumber MAX_MODULE_SPEED = new SmartNumber("Swerve/Maximum module speed", 5);
+		SmartNumber MAX_MODULE_TURN = new SmartNumber("Swerve/Maximum module turn", 6.28);
+
+        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);
+        }
+
+ 		public interface FrontRight {
+            String ID = "Front Right";
+            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(357.213206) // recalibrated 4/21                
+                .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(249.731491) // recalibrated 3/24
+                .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(125.371964) // recalibrated 4/6
+                .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(7.748473) // recalibrated 5/24
+                .plus(Rotation2d.fromDegrees(90));
+            Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * -0.5);
+        }
+
+	}
 }

src/main/java/com/stuypulse/robot/subsystems/SL_SwerveModule.java

@@ -0,0 +1,98 @@
+package com.stuypulse.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxAbsoluteEncoder;
+import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
+import com.stuypulse.robot.constants.Settings.Swerve.Drive;
+import com.stuypulse.robot.constants.Settings.Swerve.Turn;
+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.control.feedback.PIDController;
+import com.stuypulse.stuylib.control.feedforward.MotorFeedforward;
+import com.stuypulse.stuylib.math.Angle;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+public class SL_SwerveModule extends SwerveModule {
+    // data
+    private final String id;
+    private SwerveModuleState targetState;
+    private Translation2d translationOffset;
+    private Rotation2d angleOffset;
+
+    // turn
+    private CANSparkMax turnMotor; 
+    private SparkMaxAbsoluteEncoder turnEncoder;
+
+    // drive
+    private CANSparkMax driveMotor;
+    private RelativeEncoder driveEncoder; 
+
+    // controllers
+    private Controller driveController; 
+    private AngleController turnController;
+
+    public SL_SwerveModule(String id, Translation2d translationOffset, Rotation2d angleOffset, int turnID, int driveID) {
+        this.id = id;
+        this.translationOffset = translationOffset; 
+
+        turnMotor = new CANSparkMax(turnID, MotorType.kBrushless);
+        driveMotor = new CANSparkMax(turnID, MotorType.kBrushless);
+
+        turnEncoder = turnMotor.getAbsoluteEncoder(Type.kDutyCycle);
+        driveEncoder = driveMotor.getEncoder();
+
+        driveController = new PIDController(Drive.kP, Drive.kI, Drive.kP)
+            .add(new MotorFeedforward(Drive.kS, Drive.kV, Drive.kA).velocity());
+
+        turnController = new AnglePIDController(Turn.kP, Turn.kI, Turn.kP);
+    }
+
+    public Translation2d getOffset() {
+        return translationOffset;
+    }
+
+    public String getID() {
+        return id;
+    }
+
+    public SwerveModuleState getState() {
+        return new SwerveModuleState(getVelocity(), getAngle());
+    }
+
+    public double getVelocity() {
+        return driveEncoder.getVelocity();
+    }
+
+    public Rotation2d getAngle() {
+        return Rotation2d.fromRotations(turnEncoder.getPosition()).minus(angleOffset);
+    }
+
+    public SwerveModulePosition getModulePosition() {
+        return new SwerveModulePosition(driveEncoder.getPosition(), getAngle());
+    }
+
+    public void setState(SwerveModuleState state) {
+        targetState = SwerveModuleState.optimize(state, getAngle());
+    }
+
+    @Override
+    public void periodic() {
+        turnMotor.setVoltage(turnController.update(
+            Angle.fromRotation2d(targetState.angle), 
+            Angle.fromRotation2d(getAngle()))
+        );
+
+        driveMotor.setVoltage(driveController.update(
+            targetState.speedMetersPerSecond,
+            getVelocity())
+        );
+    }
+}
+