pid controllers working albeit a tad bit slow

SFUnity · Jan 10, 2024 · 4e193cb · 4e193cb
1 parent 7c9f8ee
commit 4e193cb
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/commands/DriveStraightCmd.java b/src/main/java/frc/robot/commands/DriveStraightCmd.java
@@ -4,16 +4,22 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.SwerveSubsystem;
+import edu.wpi.first.math.controller.PIDController;
+import frc.robot.Constants.DriveConstants;
+import frc.robot.Constants.ModuleConstants;
 
 public class DriveStraightCmd extends CommandBase {
     private final SwerveSubsystem m_swerveSubsystem;
     private final Double rotationDegrees, meters; 
     private Double initialPosX, initialPosY;
+    private final PIDController pidController;
 
     public DriveStraightCmd(Double rotationDegrees, Double meters, SwerveSubsystem swerveSubsystem) {
         m_swerveSubsystem = swerveSubsystem;
         this.rotationDegrees = rotationDegrees;
         this.meters = meters;
+        pidController = new PIDController(ModuleConstants.kPTurning*10, 0, 0); // Consider adding the kI & kD
+        pidController.enableContinuousInput(-15, 15);
         addRequirements(swerveSubsystem);
     }
 
@@ -23,13 +29,22 @@ public void initialize() {
         initialPosY = m_swerveSubsystem.getPose().getY();
     }
 
+    private double normalizeSpeed(double desiredSpeed) {
+        return Math.min(1.0, desiredSpeed / DriveConstants.kPhysicalMaxSpeedMetersPerSecond);
+    }
+
+    private double getDistance(double distX, double distY) {
+        return Math.sqrt(Math.pow(distX - initialPosX, 2) + Math.pow(distY - initialPosY, 2));
+    }
+
     @Override
     public void execute() {
+        double desiredRunSpeed = pidController.calculate(getDistance(m_swerveSubsystem.getPose().getX(), m_swerveSubsystem.getPose().getY()), meters);
         SwerveModuleState[] module_states = {
-            new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)),
-            new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)),
-            new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)),
-            new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees))
+            new SwerveModuleState(normalizeSpeed(desiredRunSpeed), Rotation2d.fromDegrees(rotationDegrees)),
+            new SwerveModuleState(normalizeSpeed(desiredRunSpeed), Rotation2d.fromDegrees(rotationDegrees)),
+            new SwerveModuleState(normalizeSpeed(desiredRunSpeed), Rotation2d.fromDegrees(rotationDegrees)),
+            new SwerveModuleState(normalizeSpeed(desiredRunSpeed), Rotation2d.fromDegrees(rotationDegrees))
         };
         m_swerveSubsystem.setModuleStates(module_states);
     }
@@ -41,6 +56,6 @@ public void end(boolean interrupted) {
 
     @Override
     public boolean isFinished() {
-        return Math.sqrt(Math.pow(m_swerveSubsystem.getPose().getX() - initialPosX, 2) + Math.pow(m_swerveSubsystem.getPose().getY() - initialPosY, 2)) >= meters;
+        return Math.abs(getDistance(m_swerveSubsystem.getPose().getX(), m_swerveSubsystem.getPose().getY()) - meters) < 1;
     }
 }
diff --git a/src/main/java/frc/robot/commands/RotateDegreesCmd.java b/src/main/java/frc/robot/commands/RotateDegreesCmd.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.ModuleConstants;
 import frc.robot.subsystems.SwerveSubsystem;
 import edu.wpi.first.math.controller.PIDController;
@@ -19,8 +20,8 @@ public RotateDegreesCmd(Double rotationDegrees, Boolean rotateRight, SwerveSubsy
         m_swerveSubsystem = swerveSubsystem;
         this.rotationDegrees = rotationDegrees;
         this.rotateRight = rotateRight;
-        pidController = new PIDController(ModuleConstants.kPTurning, 0, 0.1); // Consider adding the kI & kD
-        pidController.enableContinuousInput(-Math.PI, Math.PI);
+        pidController = new PIDController(ModuleConstants.kPTurning*3, 0, 0); // Consider adding the kI & kD
+        pidController.enableContinuousInput(-360, 360);
         addRequirements(swerveSubsystem);
     }
 
@@ -29,14 +30,18 @@ public void initialize() {
         initialRotation = m_swerveSubsystem.getHeading();
     }
 
+    private double normalizeTurnSpeed(double desiredSpeed) {
+        return Math.min(1.0, desiredSpeed / DriveConstants.kPhysicalMaxSpeedMetersPerSecond);
+    }
+
     @Override
     public void execute() {
         double desiredTurnSpeed = pidController.calculate(m_swerveSubsystem.getHeading(), rotationDegrees);
         SwerveModuleState[] module_states = {
-            new SwerveModuleState(desiredTurnSpeed, Rotation2d.fromDegrees(135 * (rotateRight ? 1 : -1))),
-            new SwerveModuleState(desiredTurnSpeed, Rotation2d.fromDegrees(45 * (rotateRight ? 1 : -1))),
-            new SwerveModuleState(desiredTurnSpeed, Rotation2d.fromDegrees(-135 * (rotateRight ? 1 : -1))),
-            new SwerveModuleState(desiredTurnSpeed, Rotation2d.fromDegrees(-45 * (rotateRight ? 1 : -1)))
+            new SwerveModuleState(normalizeTurnSpeed(desiredTurnSpeed), Rotation2d.fromDegrees(135 * (rotateRight ? 1 : -1))),
+            new SwerveModuleState(normalizeTurnSpeed(desiredTurnSpeed), Rotation2d.fromDegrees(45 * (rotateRight ? 1 : -1))),
+            new SwerveModuleState(normalizeTurnSpeed(desiredTurnSpeed), Rotation2d.fromDegrees(-135 * (rotateRight ? 1 : -1))),
+            new SwerveModuleState(normalizeTurnSpeed(desiredTurnSpeed), Rotation2d.fromDegrees(-45 * (rotateRight ? 1 : -1)))
         };
         m_swerveSubsystem.setModuleStates(module_states);
     }