From 0cf4858be909bedf135f18f154eeab1e14468705 Mon Sep 17 00:00:00 2001
From: Naowal Rahman <naowal.ar@gmail.com>
Date: Fri, 29 Sep 2023 00:08:48 -0700
Subject: [PATCH] Swerve changes necessary to drive

Signed-off-by: Naowal Rahman <naowal.ar@gmail.com>
---
 .../stuypulse/robot/constants/Settings.java   |  5 +-
 .../stuypulse/robot/subsystems/Swerve.java    | 58 +++++++++++++++----
 2 files changed, 50 insertions(+), 13 deletions(-)

diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
index 66c1018..4664b59 100644
--- a/src/main/java/com/stuypulse/robot/constants/Settings.java
+++ b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -5,7 +5,6 @@
 
 package com.stuypulse.robot.constants;
 
-import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
 
 /*-
@@ -17,6 +16,8 @@
 public interface Settings {
 	double DT = 0.02;
 
-	public interface Swerve { 
+	public interface Swerve {
+		SmartNumber MODULE_SPEED_DEADBAND = new SmartNumber("Module speed deadband", 0.02);
+		SmartNumber MAX_MODULE_SPEED = new SmartNumber("Maximum module speed", 5);
 	}
 }
diff --git a/src/main/java/com/stuypulse/robot/subsystems/Swerve.java b/src/main/java/com/stuypulse/robot/subsystems/Swerve.java
index 9cc07dc..47e51cc 100644
--- a/src/main/java/com/stuypulse/robot/subsystems/Swerve.java
+++ b/src/main/java/com/stuypulse/robot/subsystems/Swerve.java
@@ -2,6 +2,7 @@
 
 import com.kauailabs.navx.frc.AHRS;
 import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.constants.Settings.Swerve;
 import com.stuypulse.stuylib.math.Vector2D;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -46,15 +47,14 @@
  * chassisspeeds.fromfieldrelative(speeds towards opponent, speed left, angles we want to turn per sec, current angle)
  */
 
-
-public class Swerve {
+public class SwerveDrive {
     //creating singleton
-    public final static Swerve instance;
+    public final static SwerveDrive instance;
     static {
-        instance = new Swerve();
+        instance = new SwerveDrive();
     }
 
-    public static Swerve getInstance() {
+    public static SwerveDrive getInstance() {
         return instance;
     }
     
@@ -63,13 +63,16 @@ public static Swerve getInstance() {
     private final SwerveDriveKinematics kinematics;
     private final AHRS gyro;
     
-    public Swerve(SwerveModule... modules) {    
+    public SwerveDrive(SwerveModule... modules) {    
         this.modules = modules;
         kinematics = new SwerveDriveKinematics(getModuleOffsets());
         gyro = new AHRS(SPI.Port.kMXP);
         moduleObjects = new FieldObject2d[modules.length];
     }
 
+    /**
+     * @return Returns translation of each module relative to the center of the robot.
+     */
     public Translation2d[] getModuleOffsets() {
         var offsets = new Translation2d[modules.length];
         for (int i = 0; i < modules.length; i++) {
@@ -78,6 +81,9 @@ public Translation2d[] getModuleOffsets() {
         return offsets;
     }
 
+    /** 
+     * @return Array with current state of each swerve module.
+     */
     public SwerveModuleState[] getModuleStates() {
         SwerveModuleState[] states = new SwerveModuleState[modules.length];
         for(int i = 0; i < modules.length; i++) {
@@ -87,12 +93,33 @@ public SwerveModuleState[] getModuleStates() {
     }
 
     public ChassisSpeeds getChassisSpeeds() {
-        return kinematics.toChassisSpeeds(getModuleStates())        
+        return kinematics.toChassisSpeeds(getModuleStates());      
     }
 
-    public void setModuleStates(SwerveModuleState[] states) {
+    /**
+     * Applies deadband to module state.
+     * @param state
+     * @return The original state if the speed is greater than the deadband, otherwise a state with zero speed
+     */
+    public SwerveModuleState filterModuleState(SwerveModuleState state) {
+        if(Math.abs(state.speedMetersPerSecond) > Swerve.MODULE_SPEED_DEADBAND.get()) {
+            return state;
+        }
+
+        return new SwerveModuleState(0, state.angle);
+    }
+
+    public void setModuleStates(SwerveModuleState... states) {
+        if(states.length != modules.length) {
+            throw new IllegalArgumentException(
+                String.format("state count mismath error: %d states does not equal %d modules", states.length, modules.length)
+            );
+        }
+
+        SwerveDriveKinematics.desaturateWheelSpeeds(states, Swerve.MAX_MODULE_SPEED.get());
+
         for(int i = 0; i < modules.length; i++) {
-            modules[i].setState(states[i]);
+            modules[i].setState(filterModuleState(states[i]));
         }
     }
 
@@ -105,14 +132,23 @@ public void drive(Vector2D translation, double rotation) {
             translation.y, -translation.x, -rotation, Rotation2d.fromDegrees(gyro.getAngle())
         );
 
+        // straight spinning drift fix
         Pose2d robotVelocity = new Pose2d(
             Settings.DT * speeds.vxMetersPerSecond,
             Settings.DT * speeds.vyMetersPerSecond,
             Rotation2d.fromRadians(Settings.DT * speeds.omegaRadiansPerSecond)
         );
         Twist2d twistVel = new Pose2d().log(robotVelocity);
+
+        ChassisSpeeds updatedSpeeds = new ChassisSpeeds(
+            twistVel.dx / Settings.DT,
+            twistVel.dy / Settings.DT,
+            twistVel.dtheta / Settings.DT
+        );
+        setChassisSpeeds(updatedSpeeds);
     }
     
-
-
+    public void stop() {
+        setChassisSpeeds(new ChassisSpeeds());
+    }
 }