Improved Swerve (#26)

* set swerve to use velocity

* it workssssssssss

* removed feedforward gains

* Update MotorSubsystem.java

Co-Authored-By: Nummun14 <[email protected]>

---------

Co-authored-by: Strflightmight09 <[email protected]>
Co-authored-by: Nummun14 <[email protected]>

Author: 3 people

src/main/java/frc/trigon/robot/commands/CommandConstants.java

@@ -48,13 +48,13 @@ public class CommandConstants {
                     AutonomousConstants.ROBOT_CONFIG.moduleLocations,
                     RobotContainer.SWERVE::getDriveWheelPositionsRadians,
                     () -> RobotContainer.SWERVE.getHeading().getRadians(),
-                    (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond), null),
+                    (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)),
                     RobotContainer.SWERVE
             ),
             CALCULATE_CAMERA_POSITION_COMMAND = new CameraPositionCalculationCommand(
                     RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose,
                     Rotation2d.fromDegrees(0),
-                    (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond), null),
+                    (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)),
                     RobotContainer.SWERVE
             );
 

src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java

@@ -34,7 +34,6 @@ public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.Subs
         DISABLED_TRIGGER.onFalse(new InstantCommand(() -> {
             setAllSubsystemsBrakeAsync(true);
             IS_BRAKING = true;
-            RobotContainer.SWERVE.resetSetpoint();
         }).ignoringDisable(true));
     }
 

src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java

@@ -2,7 +2,6 @@
 
 import com.pathplanner.lib.util.DriveFeedforwards;
 import com.pathplanner.lib.util.swerve.SwerveSetpoint;
-import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -19,7 +18,6 @@
 import frc.trigon.robot.subsystems.MotorSubsystem;
 import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule;
 import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants;
-import lib.hardware.RobotHardwareStats;
 import lib.hardware.phoenix6.Phoenix6SignalThread;
 import lib.hardware.phoenix6.pigeon2.Pigeon2Gyro;
 import lib.hardware.phoenix6.pigeon2.Pigeon2Signal;
@@ -33,10 +31,8 @@ public class Swerve extends MotorSubsystem {
     private final Pigeon2Gyro gyro = SwerveConstants.GYRO;
     private final SwerveModule[] swerveModules = SwerveConstants.SWERVE_MODULES;
     private final Phoenix6SignalThread phoenix6SignalThread = Phoenix6SignalThread.getInstance();
-    private final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(AutonomousConstants.ROBOT_CONFIG, SwerveModuleConstants.MAXIMUM_MODULE_ROTATIONAL_SPEED_RADIANS_PER_SECOND);
     public Pose2d targetPathPlannerPose = new Pose2d();
     public boolean isPathPlannerDriving = false;
-    private SwerveSetpoint previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(AutonomousConstants.ROBOT_CONFIG.numModules));
 
     public Swerve() {
         setName("Swerve");
@@ -53,7 +49,7 @@ public void setBrake(boolean brake) {
     public void sysIDDrive(double targetCurrent) {
         SwerveModuleState[] a = SwerveConstants.KINEMATICS.toSwerveModuleStates(new ChassisSpeeds(0, 0, 2));
         for (int i = 0; i < 4; i++) {
-            swerveModules[i].setDriveMotorTargetCurrent(targetCurrent);
+            swerveModules[i].setDriveMotorTargetVoltage(targetCurrent);
             swerveModules[i].setTargetAngle(a[i].angle);
         }
     }
@@ -176,33 +172,22 @@ public void drivePathPlanner(ChassisSpeeds targetPathPlannerFeedforwardSpeeds, b
      * Used for PathPlanner because it generates setpoints automatically.
      *
      * @param targetSpeeds the pre generated robot relative target speeds
-     * @param feedforwards the feedforwards to use
      */
-    public void selfRelativeDriveWithoutSetpointGeneration(ChassisSpeeds targetSpeeds, DriveFeedforwards feedforwards) {
+    public void selfRelativeDriveWithoutSetpointGeneration(ChassisSpeeds targetSpeeds) {
 //        if (isStill(targetSpeeds)) {
 //            stop();
 //            return;
 //        }
 
         final SwerveModuleState[] swerveModuleStates = SwerveConstants.KINEMATICS.toSwerveModuleStates(targetSpeeds);
-        final double[] targetForcesNm;
-        if (feedforwards == null)
-            targetForcesNm = new double[]{0, 0, 0, 0};
-        else
-            targetForcesNm = feedforwards.linearForcesNewtons();
-
-        setTargetModuleStates(swerveModuleStates, targetForcesNm);
+        setTargetModuleStates(swerveModuleStates);
     }
 
     public Rotation2d getDriveRelativeAngle() {
         final Rotation2d currentAngle = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation();
         return Flippable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle;
     }
 
-    public void resetSetpoint() {
-        previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(AutonomousConstants.ROBOT_CONFIG.numModules));
-    }
-
     public void initializeDrive(boolean shouldUseClosedLoop) {
         setClosedLoop(shouldUseClosedLoop);
         resetRotationController();
@@ -279,23 +264,18 @@ void selfRelativeDrive(double xPower, double yPower, FlippableRotation2d targetA
      * @param targetSpeeds the desired robot-relative targetSpeeds
      */
     public void selfRelativeDrive(ChassisSpeeds targetSpeeds) {
-        previousSetpoint = setpointGenerator.generateSetpoint(
-                previousSetpoint,
-                targetSpeeds,
-                RobotHardwareStats.getPeriodicTimeSeconds()
-        );
-
-//        if (isStill(previousSetpoint.robotRelativeSpeeds())) {
+//        if (isStill(targetSpeeds) {
 //            stop();
 //            return;
 //        }
 
-        setTargetModuleStates(previousSetpoint.moduleStates(), previousSetpoint.feedforwards().linearForcesNewtons());
+        final SwerveModuleState[] swerveModuleStates = SwerveConstants.KINEMATICS.toSwerveModuleStates(targetSpeeds);
+        setTargetModuleStates(swerveModuleStates);
     }
 
-    private void setTargetModuleStates(SwerveModuleState[] swerveModuleStates, double[] targetForcesNm) {
+    private void setTargetModuleStates(SwerveModuleState[] swerveModuleStates) {
         for (int i = 0; i < swerveModules.length; i++)
-            swerveModules[i].setTargetState(swerveModuleStates[i], targetForcesNm[i]);
+            swerveModules[i].setTargetState(swerveModuleStates[i]);
     }
 
     private void setClosedLoop(boolean shouldUseClosedLoop) {

src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java

@@ -1,8 +1,7 @@
 package frc.trigon.robot.subsystems.swerve.swervemodule;
 
 import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.controls.TorqueCurrentFOC;
-import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.controls.VoltageOut;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -25,9 +24,8 @@ public class SwerveModule {
     private final CANcoderEncoder steerEncoder;
     private final PositionVoltage steerPositionRequest = new PositionVoltage(0).withEnableFOC(SwerveModuleConstants.ENABLE_FOC);
     private final double wheelDiameter;
-    private final VelocityTorqueCurrentFOC driveVelocityRequest = new VelocityTorqueCurrentFOC(0).withUpdateFreqHz(1000);
+    private final VelocityVoltage driveVelocityRequest = new VelocityVoltage(0).withUpdateFreqHz(1000);
     private final VoltageOut driveVoltageRequest = new VoltageOut(0).withEnableFOC(SwerveModuleConstants.ENABLE_FOC);
-    private final TorqueCurrentFOC driveTorqueCurrentFOCRequest = new TorqueCurrentFOC(0);
     private boolean shouldDriveMotorUseClosedLoop = true;
     private SwerveModuleState targetState = new SwerveModuleState();
     private double[]
@@ -50,15 +48,14 @@ public SwerveModule(int moduleID, double offsetRotations, double wheelDiameter)
         configureHardware(offsetRotations);
     }
 
-    public void setTargetState(SwerveModuleState targetState, double targetForceNm) {
+    public void setTargetState(SwerveModuleState targetState) {
         if (willOptimize(targetState)) {
             targetState.optimize(getCurrentAngle());
-            targetForceNm *= -1;
         }
 
         this.targetState = targetState;
         setTargetAngle(targetState.angle);
-        setTargetVelocity(targetState.speedMetersPerSecond, targetForceNm);
+        setTargetVelocity(targetState.speedMetersPerSecond);
     }
 
     public void setBrake(boolean brake) {
@@ -96,8 +93,8 @@ public void shouldDriveMotorUseClosedLoop(boolean shouldDriveMotorUseClosedLoop)
         this.shouldDriveMotorUseClosedLoop = shouldDriveMotorUseClosedLoop;
     }
 
-    public void setDriveMotorTargetCurrent(double targetCurrent) {
-        driveMotor.setControl(driveTorqueCurrentFOCRequest.withOutput(targetCurrent));
+    public void setDriveMotorTargetVoltage(double targetVoltage) {
+        driveMotor.setControl(driveVoltageRequest.withOutput(targetVoltage));
     }
 
     public void setTargetAngle(Rotation2d angle) {
@@ -145,21 +142,19 @@ private boolean willOptimize(SwerveModuleState state) {
      * The target velocity is set using either closed loop or open loop depending on {@link this#shouldDriveMotorUseClosedLoop}.
      *
      * @param targetVelocityMetersPerSecond the target velocity, in meters per second
-     * @param targetForceNm                 the target force of the module in newton meters
      */
-    private void setTargetVelocity(double targetVelocityMetersPerSecond, double targetForceNm) {
+    private void setTargetVelocity(double targetVelocityMetersPerSecond) {
         if (shouldDriveMotorUseClosedLoop) {
-            setTargetClosedLoopVelocity(targetVelocityMetersPerSecond, targetForceNm);
+            setTargetClosedLoopVelocity(targetVelocityMetersPerSecond);
             return;
         }
 
         setTargetOpenLoopVelocity(targetVelocityMetersPerSecond);
     }
 
-    private void setTargetClosedLoopVelocity(double targetVelocityMetersPerSecond, double targetForceNm) {
+    private void setTargetClosedLoopVelocity(double targetVelocityMetersPerSecond) {
         final double targetVelocityRotationsPerSecond = metersToDriveWheelRotations(targetVelocityMetersPerSecond);
-        final double targetAccelerationRotationsPerSecondSquared = metersToDriveWheelRotations(targetForceNm);
-        driveMotor.setControl(driveVelocityRequest.withVelocity(targetVelocityRotationsPerSecond).withAcceleration(targetAccelerationRotationsPerSecondSquared));
+        driveMotor.setControl(driveVelocityRequest.withVelocity(targetVelocityRotationsPerSecond));
     }
 
     private void setTargetOpenLoopVelocity(double targetVelocityMetersPerSecond) {

src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java

@@ -58,7 +58,7 @@ static SimpleMotorSimulation createSteerMotorSimulation() {
         return new SimpleMotorSimulation(STEER_MOTOR_GEARBOX, REAR_STEER_MOTOR_GEAR_RATIO, STEER_MOMENT_OF_INERTIA);
     }
 
-    static TalonFXConfiguration generateDriveMotorConfiguration() {
+    public static TalonFXConfiguration generateDriveMotorConfiguration() {
         final TalonFXConfiguration config = new TalonFXConfiguration();
 
         config.Audio.BeepOnBoot = false;
@@ -77,12 +77,12 @@ static TalonFXConfiguration generateDriveMotorConfiguration() {
         config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1;
         config.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.1;
 
-        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 50 : 50;
+        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 50;
         config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
         config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0;
-        config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.4708 : 5.25;
-        config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0 : 0;
-        config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0 : DRIVE_MOTOR_GEAR_RATIO / (1 / DCMotor.getKrakenX60Foc(1).KtNMPerAmp);
+        config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 5.25;
+        config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.8774 : 0;
+        config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.020691 : 0;
 
         config.Feedback.VelocityFilterTimeConstant = 0;