Merge pull request #4 from StuyPulse/se/swerve

Add Swerve subsystem

build.gradle

@@ -12,7 +12,7 @@ allprojects {
 
 // Set this to the latest version of StuyLib.
 // You can check here: https://github.com/StuyPulse/StuyLib/releases.
-final String STUYLIB_VERSION = 'v2024.1.3'
+final String STUYLIB_VERSION = 'v2024.1.4'
 
 def ROBOT_MAIN_CLASS = "com.stuypulse.robot.Main"
 

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

@@ -8,6 +8,7 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
+
 import com.revrobotics.CANSparkMax;
 import static com.revrobotics.CANSparkMax.IdleMode;
 
@@ -21,9 +22,12 @@
  *  - The Open Loop Ramp Rate
  */
 public interface Motors {
-
     /** Classes to store all of the values a motor needs */
-
+    public interface Swerve {
+        public CANSparkMaxConfig DRIVE_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake, 60);
+        public CANSparkMaxConfig TURN_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake, 60);
+    }
+
     public static class TalonSRXConfig {
         public final boolean INVERTED;
         public final NeutralMode NEUTRAL_MODE;

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

@@ -8,10 +8,60 @@
 import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
 
+import edu.wpi.first.math.util.Units;
+
 /*-
  * File containing tunable settings for every subsystem on the robot.
  *
  * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable
  * values that we can edit on Shuffleboard.
  */
-public interface Settings {}
+public interface Settings {
+    double DT = 0.02;
+    public interface Swerve {
+        SmartNumber MAX_MODULE_SPEED = new SmartNumber("Swerve/Max Module Speed (meter per s)", 5.0);
+        SmartNumber MAX_TURNING = new SmartNumber("Swerve/Max Turn Velocity (rad per s)", 6.28);
+
+        SmartNumber MODULE_VELOCITY_DEADBAND = new SmartNumber("Swerve/Module Velocity Deadband (m per s)", 0.02);
+        public interface Encoder {
+            public interface Drive {
+                double WHEEL_DIAMETER = Units.inchesToMeters(3);
+                double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI;
+                double GEAR_RATIO = 1.0 / 4.71; //TODO: Find Gear Ratio
+
+                //TODO: CHECK THESE
+                double POSITION_CONVERSION = WHEEL_CIRCUMFERENCE * GEAR_RATIO;
+                double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0;
+            }
+
+            public interface Turn {
+                //TODO: CHECK THESE
+                double POSITION_CONVERSION = 1;
+                double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0;
+            }
+        }
+
+        public interface Controller {
+            //TODO: CHECK THESE
+            public interface Turn {
+                SmartNumber kP = new SmartNumber("Swerve/Turn/kP", 1.0);
+                SmartNumber kI = new SmartNumber("Swerve/Turn/kI", 0.0);
+                SmartNumber kD = new SmartNumber("Swerve/Turn/kD", 0.0);
+
+                SmartNumber kS = new SmartNumber("Swerve/Turn/kS", 0.25);
+                SmartNumber kV = new SmartNumber("Swerve/Turn/kV", 0.00);
+            }
+
+            public interface Drive {
+                SmartNumber kP = new SmartNumber("Swerve/Drive/kP", 0.10);
+                SmartNumber kI = new SmartNumber("Swerve/Drive/kI", 0.00);
+                SmartNumber kD = new SmartNumber("Swerve/Drive/kD", 0.00);
+
+                SmartNumber kS = new SmartNumber("Swerve/Drive/kS", 0.25);
+                SmartNumber kV = new SmartNumber("Swerve/Drive/kV", 0.00);
+                SmartNumber kA = new SmartNumber("Swerve/Drive/kA", 0.00);
+            }
+        }
+    }
+
+}

src/main/java/com/stuypulse/robot/subsystems/swerve/AbstractSwerveDrive.java

@@ -0,0 +1,18 @@
+package com.stuypulse.robot.subsystems.swerve;
+import com.stuypulse.stuylib.math.Vector2D;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public abstract class AbstractSwerveDrive extends SubsystemBase {
+    public abstract void initFieldObject(Field2d field);
+    public abstract SwerveModuleState[] getModuleStates();
+    public abstract Translation2d[] getModulePositions();
+    public abstract void setChassisSpeed(ChassisSpeeds speeds);
+    public abstract void setModuleStates(SwerveModuleState[] states);
+    public abstract void drive(Vector2D velocity, double Rotation2D);
+    public abstract void stop(double Rotation2D); 
+}   

src/main/java/com/stuypulse/robot/subsystems/swerve/AbstractSwerveModule.java

@@ -0,0 +1,34 @@
+package com.stuypulse.robot.subsystems.swerve;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public abstract class AbstractSwerveModule extends SubsystemBase {
+    public abstract SwerveModuleState getState();
+    public abstract double getVelocity();
+    public abstract Rotation2d getAngle();
+    public abstract String getId();
+    public abstract Translation2d getModulePosition();
+    public abstract void setState(SwerveModuleState state);
+    public abstract void periodic();
+	public Object getRotation2d() {
+		// TODO Auto-generated method stub
+		throw new UnsupportedOperationException("Unimplemented method 'getRotation2d'");
+	}
+
+
+
+
+
+
+
+
+
+
+
+
+}
+

src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java

@@ -0,0 +1,194 @@
+package com.stuypulse.robot.subsystems.swerve;
+
+import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState;
+import com.ctre.phoenix6.sim.ChassisReference;
+import com.kauailabs.navx.frc.AHRS;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.stuylib.math.Vector2D;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/*   
+*  Fields: 
+*    - swerveModules:  AbstractSwerveModule...
+*    - kinematics:  SwerveDriveKinematics
+*    - gyro: AHRS
+*    - modules2D: FieldObject2D[]
+*
+*   Tasks:
+*    - drive
+*    - followDrive 
+*    - trackingDrive
+*    - aligning (Trap, Speaker, Amp)
+*    - GTADrive (shooting while driving)
+*
+*   Methods:
+*    + singleton
+*    + initFieldObject(Field2D field): void
+*    + getModulePositions(): Translation2D[]
+*    + getModuleStates(): SwerveModuleStates[]
+*    + getModuleOffsets(): Rotation2D[]
+*    + getChassisSpeeds(): ChassisSpeed[]
+*    + setModuleStates(SwerveModuleState... states): void
+*    + setChassisSpeed(ChassisSpeed): void 
+*    + drive(double, Rotation2D)
+*    + stop(double, Rotation2D)
+*
+*  SwerveDrive.java
+*   Methods:
+*    - getGyroAngle(): Rotation2D
+*    - getGyroYaw(): Rotation2D
+*    - getGyroPitch(): Rotation2D
+*    - getGyroRoll(): Rotation2D
+*    - getKinematics(): SwerveDriveKinematics
+*    + periodic(): void
+*    
+*
+*/
+public class SwerveDrive extends AbstractSwerveDrive {
+
+    private AbstractSwerveModule[] modules;
+    private SwerveDriveKinematics kinematics;
+    private AHRS gyro;
+    private FieldObject2d[] modules2D;
+
+    public SwerveDrive(AbstractSwerveModule[] modules){
+        this.modules = modules;
+        this.kinematics = new SwerveDriveKinematics(getModulePositions());
+        this.gyro = new AHRS(SPI.Port.kMXP);
+        this.modules2D = new FieldObject2d[modules.length];
+    }   
+
+    @Override
+    public void initFieldObject(Field2d field) {
+        for (int i = 0; i < modules.length; i++){
+            modules2D[i] = field.getObject(modules[i].getId() + "-2d");
+        }
+    }
+
+    public SwerveDriveKinematics getKinematics() {
+        return kinematics;
+    }
+
+    @Override
+    public SwerveModuleState[] getModuleStates() {
+        SwerveModuleState[] states = new SwerveModuleState[modules.length];
+        for (int i = 0; i< modules.length; i++){
+            states[i] = modules[i].getState();
+        }
+        return states;
+    }
+
+    @Override
+    public Translation2d[] getModulePositions() {
+        Translation2d[] offsets = new Translation2d[modules.length];
+        for (int i = 0; i < modules.length; i++){
+            offsets[i] = modules[i].getModulePosition();
+        }
+        return offsets;
+    }
+
+    public ChassisSpeeds getChassisSpeed() {
+        return getKinematics().toChassisSpeeds(getModuleStates());
+    }
+
+    /*Setters */
+
+    private static SwerveModuleState filterModuleState(SwerveModuleState state) {
+        if (Math.abs(state.speedMetersPerSecond) > Settings.Swerve.MODULE_VELOCITY_DEADBAND.get())
+            return state;
+
+        return new SwerveModuleState(0, state.angle);
+    }
+
+    @Override
+    public void setModuleStates(SwerveModuleState[] states) {
+        if (states.length != modules.length) {
+            throw new IllegalArgumentException("Provided incorrect number of states for swerve drive modules");
+        }
+
+        SwerveDriveKinematics.desaturateWheelSpeeds(states, Settings.Swerve.MAX_MODULE_SPEED.get());
+
+        for (int i = 0; i < modules.length; i++){
+            modules[i].setState(filterModuleState(states[i]));
+        }
+    }
+
+    @Override
+    public void setChassisSpeed(ChassisSpeeds robotSpeeds) {
+        setModuleStates(kinematics.toSwerveModuleStates(robotSpeeds));
+    }
+
+    @Override
+    public void drive(Vector2D velocity, double rotation) {
+        ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+            velocity.x, -velocity.y,
+            rotation,
+            getGyroAngle()
+        );
+
+        Pose2d pose = new Pose2d(
+            Settings.DT * speeds.vxMetersPerSecond,
+            Settings.DT * speeds.vyMetersPerSecond,
+            Rotation2d.fromDegrees(Settings.DT * speeds.omegaRadiansPerSecond)
+        );
+
+        Twist2d twistVel = new Pose2d().log(pose);   
+
+        setChassisSpeed( new ChassisSpeeds(
+            twistVel.dx / Settings.DT,
+            twistVel.dy / Settings.DT,
+            twistVel.dtheta / Settings.DT
+        ));
+    }
+
+    @Override
+    public void stop(double Rotation2D) {
+        setChassisSpeed(new ChassisSpeeds());
+    } 
+
+    /*Gyro */
+    public Rotation2d getGyroPitch() {
+        return Rotation2d.fromDegrees(gyro.getPitch());
+    }
+
+    public Rotation2d getGyroAngle() {
+        return gyro.getRotation2d();
+    }
+
+    public Rotation2d getGyroRoll() {
+        return Rotation2d.fromDegrees(gyro.getRoll());
+    }
+
+    public Rotation2d getGyroYaw() {
+        return Rotation2d.fromDegrees(gyro.getYaw());
+    }
+
+    public double getForwardAccelerationGs() {
+        return gyro.getWorldLinearAccelY();
+    }
+
+    @Override
+    public void periodic() {
+        /*XXX: WAITING FOR ODOMETRY TO WRITE PERIODIC */ 
+
+        SmartDashboard.putNumber("Swerve/Gyro Angle (deg)", getGyroAngle().getDegrees());
+        SmartDashboard.putNumber("Swerve/Gyro Pitch", getGyroPitch().getDegrees());
+        SmartDashboard.putNumber("Swerve/Gyro Roll", getGyroRoll().getDegrees());
+
+        SmartDashboard.putNumber("Swerve/Forward Acceleration (Gs)", getForwardAccelerationGs());
+        SmartDashboard.putNumber("Swerve/X Acceleration (Gs)", gyro.getWorldLinearAccelX());
+        SmartDashboard.putNumber("Swerve/Y Acceleration (Gs)", gyro.getWorldLinearAccelY());
+        SmartDashboard.putNumber("Swerve/Z Acceleration (Gs)", gyro.getWorldLinearAccelZ());
+    }
+}