Se/swerve part 2

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

@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 public class RobotContainer {
 
@@ -33,6 +34,10 @@ public class RobotContainer {
     public final Arm arm = Arm.getInstance();
     public final SwerveDrive swerve = SwerveDrive.getInstance();
 
+    // Controller
+    // private final CommandXboxController driver = new CommandXboxController(Ports.Gamepad.DRIVER);
+    // private final CommandXboxController operator = new CommandXboxController(Ports.Gamepad.OPERATOR);
+
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
 

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

@@ -1,15 +1,13 @@
 package com.stuypulse.robot.constants;
 
-import com.pathplanner.lib.path.PathConstraints;
-import com.pathplanner.lib.util.PIDConstants;
-
 import com.stuypulse.robot.util.ShooterSpeeds;
 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.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 
 /*-
@@ -146,49 +144,51 @@ public interface FF {
     }
 
     public interface Swerve {
-        // between wheel centers
-        double WIDTH = Units.inchesToMeters(20.75);
-        double LENGTH = Units.inchesToMeters(20.75);
+        double WIDTH = Units.inchesToMeters(32.5);
+        double LENGTH = Units.inchesToMeters(27.375);
         double CENTER_TO_INTAKE_FRONT = Units.inchesToMeters(13.0);
 
         double MAX_MODULE_SPEED = 4.9;
         double MAX_MODULE_ACCEL = 15.0;
 
+        double MAX_LINEAR_VELOCITY = 15.0;
+        double MAX_ANGULAR_VELOCITY = 12.0;
+
         double MODULE_VELOCITY_DEADBAND = 0.05;
 
         SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Swerve/Align Omega Deadband", 0.05); // TODO: make 0.25 and test
 
-        public interface Assist {
-            SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); //change
+        // public interface Assist {
+        //     SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); //change
 
-            double BUZZ_INTENSITY = 1;
+        //     double BUZZ_INTENSITY = 1;
 
-            // angle PID
-            SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0);
-            SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0);
-            SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0);
+        //     // angle PID
+        //     SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0);
+        //     SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0);
+        //     SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0);
 
-            double ANGLE_DERIV_RC = 0.05;
-            double REDUCED_FF_DIST = 0.75;
-        }
+        //     double ANGLE_DERIV_RC = 0.05;
+        //     double REDUCED_FF_DIST = 0.75;
+        // }
 
-        // TODO: Tune these values
-        public interface Motion {
-            SmartNumber MAX_VELOCITY = new SmartNumber("Swerve/Motion/Max Velocity", 3.0);
-            SmartNumber MAX_ACCELERATION = new SmartNumber("Swerve/Motion/Max Acceleration", 4.0);
-            SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Swerve/Motion/Max Angular Velocity", Units.degreesToRadians(540));
-            SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Swerve/Motion/Max Angular Acceleration", Units.degreesToRadians(720));
-
-            PathConstraints DEFAULT_CONSTRAINTS =
-                new PathConstraints(
-                    MAX_VELOCITY.get(),
-                    MAX_ACCELERATION.get(),
-                    MAX_ANGULAR_VELOCITY.get(),
-                    MAX_ANGULAR_ACCELERATION.get());
-
-            PIDConstants XY = new PIDConstants(2.5, 0, 0.02);
-            PIDConstants THETA = new PIDConstants(4, 0, 0.1);
-        }
+        // // TODO: Tune these values
+        // public interface Motion {
+        //     SmartNumber MAX_VELOCITY = new SmartNumber("Swerve/Motion/Max Velocity", 3.0);
+        //     SmartNumber MAX_ACCELERATION = new SmartNumber("Swerve/Motion/Max Acceleration", 4.0);
+        //     SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Swerve/Motion/Max Angular Velocity", Units.degreesToRadians(540));
+        //     SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Swerve/Motion/Max Angular Acceleration", Units.degreesToRadians(720));
+
+        //     PathConstraints DEFAULT_CONSTRAINTS =
+        //         new PathConstraints(
+        //             MAX_VELOCITY.get(),
+        //             MAX_ACCELERATION.get(),
+        //             MAX_ANGULAR_VELOCITY.get(),
+        //             MAX_ANGULAR_ACCELERATION.get());
+
+        //     PIDConstants XY = new PIDConstants(2.5, 0, 0.02);
+        //     PIDConstants THETA = new PIDConstants(4, 0, 0.1);
+        // }
 
         public interface Encoder {
             public interface Drive {
@@ -214,6 +214,7 @@ public interface Turn {
             SmartNumber kS = new SmartNumber("Swerve/Turn/FF/kS", 0.25582);
             SmartNumber kV = new SmartNumber("Swerve/Turn/FF/kV", 0.00205);
             SmartNumber kA = new SmartNumber("Swerve/Turn/FF/kA", 0.00020123);
+            double kT = 1.0 / DCMotor.getKrakenX60Foc(1).KtNMPerAmp;
 
             SmartBoolean INVERTED = new SmartBoolean("Swerve/Turn/INVERTED", true);
 

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

@@ -9,6 +9,7 @@
 import com.stuypulse.robot.constants.Settings.Swerve.FrontLeft;
 import com.stuypulse.robot.constants.Settings.Swerve.FrontRight;
 import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.controllers.DriveController;
 import com.stuypulse.robot.subsystems.swerve.modules.KrakenSwerveModule;
 import com.stuypulse.robot.subsystems.swerve.modules.SwerveModule;
 
@@ -22,61 +23,27 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StructArrayPublisher;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
+import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.hardware.Pigeon2;
 
-/*
- *  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 SubsystemBase {
 
     private static final SwerveDrive instance;
 
     static {
         instance = new SwerveDrive(
-            new KrakenSwerveModule(FrontRight.ID, FrontRight.MODULE_OFFSET, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.DRIVE, Ports.Swerve.FrontRight.TURN, Ports.Swerve.FrontRight.ENCODER),
-            new KrakenSwerveModule(FrontLeft.ID, FrontLeft.MODULE_OFFSET, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.DRIVE, Ports.Swerve.FrontLeft.TURN, Ports.Swerve.FrontLeft.ENCODER),
-            new KrakenSwerveModule(BackLeft.ID, BackLeft.MODULE_OFFSET, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.DRIVE, Ports.Swerve.BackLeft.TURN, Ports.Swerve.BackLeft.ENCODER),
-            new KrakenSwerveModule(BackRight.ID, BackRight.MODULE_OFFSET, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.DRIVE, Ports.Swerve.BackRight.TURN, Ports.Swerve.BackRight.ENCODER)
+            new KrakenSwerveModule(FrontRight.ID, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.DRIVE, Ports.Swerve.FrontRight.TURN, Ports.Swerve.FrontRight.ENCODER),
+            new KrakenSwerveModule(FrontLeft.ID,  FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.DRIVE, Ports.Swerve.FrontLeft.TURN, Ports.Swerve.FrontLeft.ENCODER),
+            new KrakenSwerveModule(BackLeft.ID,  BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.DRIVE, Ports.Swerve.BackLeft.TURN, Ports.Swerve.BackLeft.ENCODER),
+            new KrakenSwerveModule(BackRight.ID, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.DRIVE, Ports.Swerve.BackRight.TURN, Ports.Swerve.BackRight.ENCODER)
         );
     }
 
@@ -89,8 +56,16 @@ public static SwerveDrive getInstance() {
     private final Pigeon2 gyro;
     private final FieldObject2d[] modules2D;
 
+    private ChassisSpeeds desiredSpeeds = new ChassisSpeeds();
+    // TODO: Add SSG
+    private final DriveController driveController;
+
     private final StructArrayPublisher<SwerveModuleState> statesPub;
 
+    // Status Signals
+    private final StatusSignal<Double> yaw;
+    private final StatusSignal<Double> yawVelocity;
+
     /**
      * Creates a new Swerve Drive using the provided modules
      *
@@ -102,8 +77,18 @@ protected SwerveDrive(SwerveModule... modules) {
         gyro = new Pigeon2(Ports.Gyro.PIGEON2, "*");
         modules2D = new FieldObject2d[modules.length];
 
+        driveController = new DriveController();
+
         statesPub = NetworkTableInstance.getDefault()
             .getStructArrayTopic("SmartDashboard/Swerve/States", SwerveModuleState.struct).publish();
+
+        yaw = gyro.getYaw();
+        yawVelocity = gyro.getAngularVelocityZWorld();
+        gyro.getConfigurator().apply(new Pigeon2Configuration());
+        gyro.getConfigurator().setYaw(0.0);
+        yaw.setUpdateFrequency(250);
+        yawVelocity.setUpdateFrequency(100);
+        gyro.optimizeBusUtilization();
     }
 
     public void initFieldObject(Field2d field) {
@@ -133,12 +118,14 @@ public SwerveModulePosition[] getModulePositions() {
         return offsets;
     }
 
+    // TODO: Offset should be to the center of wheels, not corner of robot
     public Translation2d[] getModuleOffsets() {
-        Translation2d[] offsets = new Translation2d[modules.length];
-        for (int i = 0; i < modules.length; i++) {
-            offsets[i] = modules[i].getModuleOffset();
-        }
-        return offsets;
+        return new Translation2d[] {
+            new Translation2d(Swerve.WIDTH / 2.0, Swerve.LENGTH / 2.0),
+            new Translation2d(-Swerve.WIDTH / 2.0, Swerve.LENGTH / 2.0),
+            new Translation2d(-Swerve.WIDTH / 2.0, -Swerve.LENGTH / 2.0),
+            new Translation2d(Swerve.WIDTH / 2.0, -Swerve.LENGTH / 2.0)
+        };
     }
 
     public ChassisSpeeds getChassisSpeeds() {
@@ -207,26 +194,38 @@ public void stop() {
         setChassisSpeeds(new ChassisSpeeds());
     }
 
+    public void acceptTeleopInput(
+        double controllerX, double controllerY, double controllerOmega) {
+            if(DriverStation.isTeleopEnabled()) {
+                driveController.acceptDriveInput(controllerX, controllerY, controllerOmega);
+            }
+
+    }
+
     /** Gyro **/
     public Rotation2d getGyroAngle() {
         return gyro.getRotation2d();
     }
 
     public StatusSignal<Double> getGyroYaw() {
-        return gyro.getYaw();
+        return yaw;
     }
 
     public StatusSignal<Double> getGyroYawVelocity() {
-        return gyro.getAngularVelocityZWorld();
+        return yawVelocity;
     }
 
     @Override
     public void periodic() {
         statesPub.set(getModuleStates());
 
+        desiredSpeeds = driveController.update();
+
+        BaseStatusSignal.refreshAll(yaw, yawVelocity);
+
         SmartDashboard.putNumber("Swerve/Gyro/Angle (deg)", getGyroAngle().getDegrees());
-        SmartDashboard.putNumber("Swerve/Gyro/Yaw (deg)", getGyroYaw().getValueAsDouble());
-        SmartDashboard.putNumber("Swerve/Gyro/Yaw Velocity (deg)", getGyroYawVelocity().getValueAsDouble());
+        SmartDashboard.putNumber("Swerve/Gyro/Yaw (deg)", yaw.getValueAsDouble());
+        SmartDashboard.putNumber("Swerve/Gyro/Yaw Velocity (deg)", yawVelocity.getValueAsDouble());
 
         SmartDashboard.putNumber("Swerve/X Acceleration (Gs)", gyro.getAccelerationX().getValueAsDouble());
         SmartDashboard.putNumber("Swerve/Y Acceleration (Gs)", gyro.getAccelerationY().getValueAsDouble());

src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java

@@ -0,0 +1,68 @@
+package com.stuypulse.robot.subsystems.swerve.controllers;
+
+import com.stuypulse.robot.constants.Settings.Swerve;
+import com.stuypulse.stuylib.network.SmartNumber;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+public class DriveController {
+    private static final SmartNumber controllerDeadband =
+        new SmartNumber("Controller/DriveController/Deadband", 0.1);
+    private static final SmartNumber maxAngularVelocityScalar =
+        new SmartNumber("Controller/DriveController/MaxAngularVelocityScalar", 0.65);
+
+
+    private double controllerX = 0;
+    private double controllerY = 0;
+    private double controllerOmega = 0;
+
+    /**
+   * Accepts new drive input from joysticks.
+   *
+   * @param x Desired x velocity scalar, -1..1
+   * @param y Desired y velocity scalar, -1..1
+   * @param omega Desired omega velocity scalar, -1..1
+   */
+    public void acceptDriveInput(double x, double y, double omega) {
+        controllerX = x;
+        controllerY = y;
+        controllerOmega = omega;
+    }
+
+    public ChassisSpeeds update() {
+        Translation2d linearVelocity = calcLinearVelocity(controllerX, controllerY);
+        double omega = MathUtil.applyDeadband(controllerOmega, controllerDeadband.get());
+        omega = Math.copySign(omega * omega, omega);
+
+        final double maxLinearVelocity = Swerve.MAX_LINEAR_VELOCITY;
+        final double maxAngularVelocity =
+            Swerve.MAX_ANGULAR_VELOCITY * (maxAngularVelocityScalar.get());
+
+        return new ChassisSpeeds(
+            linearVelocity.getX() * maxLinearVelocity,
+            linearVelocity.getY() * maxLinearVelocity,
+            omega * maxAngularVelocity);
+    }
+
+
+    public static Translation2d calcLinearVelocity(double x, double y) {
+        // Apply deadband
+        double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), controllerDeadband.get());
+        Rotation2d linearDirection = new Rotation2d(x, y);
+
+        // Square magnitude
+        linearMagnitude = linearMagnitude * linearMagnitude;
+
+        // Calcaulate new linear velocity
+        Translation2d linearVelocity =
+            new Pose2d(new Translation2d(), linearDirection)
+                .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
+                .getTranslation();
+        return linearVelocity;
+    }
+}