testing pid loops

SFUnity · Jan 10, 2024 · 7c9f8ee · 7c9f8ee
1 parent 30d61f8
commit 7c9f8ee
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 26 deletions.
diff --git a/src/main/java/frc/robot/commands/CustomTestCmd.java b/src/main/java/frc/robot/commands/CustomTestCmd.java
@@ -9,9 +9,9 @@ public class CustomTestCmd extends SequentialCommandGroup{
 
     public CustomTestCmd(SwerveSubsystem swerveSubsystem) {
         addCommands(
-            new DriveStraightCmd(3.0, 0.0, 5.0, swerveSubsystem),
-            new RotateDegreesCmd(3.0, 1080.0, true, swerveSubsystem),
-            new DriveStraightCmd(-3.0, 0.0, 5.0, swerveSubsystem)
+            new DriveStraightCmd(0.0, 5.0, swerveSubsystem),
+            new RotateDegreesCmd(180.0, true, swerveSubsystem),
+            new DriveStraightCmd(0.0, 5.0, swerveSubsystem)
         );
     }
 

diff --git a/src/main/java/frc/robot/commands/DriveStraightCmd.java b/src/main/java/frc/robot/commands/DriveStraightCmd.java
@@ -7,21 +7,16 @@
 
 public class DriveStraightCmd extends CommandBase {
     private final SwerveSubsystem m_swerveSubsystem;
-    private final Double rotationDegrees, meters, speed; 
+    private final Double rotationDegrees, meters; 
     private Double initialPosX, initialPosY;
 
-    public DriveStraightCmd(Double speed, Double rotationDegrees, Double meters, SwerveSubsystem swerveSubsystem) {
+    public DriveStraightCmd(Double rotationDegrees, Double meters, SwerveSubsystem swerveSubsystem) {
         m_swerveSubsystem = swerveSubsystem;
         this.rotationDegrees = rotationDegrees;
         this.meters = meters;
-        this.speed = speed;
         addRequirements(swerveSubsystem);
     }
 
-    public DriveStraightCmd(Double rotationDegrees, Double meters, SwerveSubsystem swerveSubsystem) {
-        this(rotationDegrees, 1.0, meters, swerveSubsystem);
-    }
-
     @Override
     public void initialize() {
         initialPosX = m_swerveSubsystem.getPose().getX();
@@ -31,10 +26,10 @@ public void initialize() {
     @Override
     public void execute() {
         SwerveModuleState[] module_states = {
-            new SwerveModuleState(speed, Rotation2d.fromDegrees(rotationDegrees)),
-            new SwerveModuleState(speed, Rotation2d.fromDegrees(rotationDegrees)),
-            new SwerveModuleState(speed, Rotation2d.fromDegrees(rotationDegrees)),
-            new SwerveModuleState(speed, 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(1.0, Rotation2d.fromDegrees(rotationDegrees))
         };
         m_swerveSubsystem.setModuleStates(module_states);
     }

diff --git a/src/main/java/frc/robot/commands/RotateDegreesCmd.java b/src/main/java/frc/robot/commands/RotateDegreesCmd.java
@@ -3,39 +3,40 @@
 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.ModuleConstants;
 import frc.robot.subsystems.SwerveSubsystem;
+import edu.wpi.first.math.controller.PIDController;
 
 public class RotateDegreesCmd extends CommandBase {
     private final SwerveSubsystem m_swerveSubsystem;
-    private final Double rotationDegrees, speed;
+    private final Double rotationDegrees;
     private final Boolean rotateRight;
     private Double initialRotation;
+    private final PIDController pidController;
 
 
-    public RotateDegreesCmd(Double speed, Double rotationDegrees, Boolean rotateRight, SwerveSubsystem swerveSubsystem) {
+    public RotateDegreesCmd(Double rotationDegrees, Boolean rotateRight, SwerveSubsystem swerveSubsystem) {
         m_swerveSubsystem = swerveSubsystem;
         this.rotationDegrees = rotationDegrees;
         this.rotateRight = rotateRight;
-        this.speed = speed;
+        pidController = new PIDController(ModuleConstants.kPTurning, 0, 0.1); // Consider adding the kI & kD
+        pidController.enableContinuousInput(-Math.PI, Math.PI);
         addRequirements(swerveSubsystem);
     }
 
-    public RotateDegreesCmd(Double rotationDegrees, Boolean rotateRight, SwerveSubsystem swerveSubsystem) {
-        this(1.0, rotationDegrees, rotateRight, swerveSubsystem);
-    }
-
     @Override
     public void initialize() {
         initialRotation = m_swerveSubsystem.getHeading();
     }
 
     @Override
     public void execute() {
+        double desiredTurnSpeed = pidController.calculate(m_swerveSubsystem.getHeading(), rotationDegrees);
         SwerveModuleState[] module_states = {
-            new SwerveModuleState(speed, Rotation2d.fromDegrees(135 * (rotateRight ? 1 : -1))),
-            new SwerveModuleState(speed, Rotation2d.fromDegrees(45 * (rotateRight ? 1 : -1))),
-            new SwerveModuleState(speed, Rotation2d.fromDegrees(-135 * (rotateRight ? 1 : -1))),
-            new SwerveModuleState(speed, 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(desiredTurnSpeed, Rotation2d.fromDegrees(-135 * (rotateRight ? 1 : -1))),
+            new SwerveModuleState(desiredTurnSpeed, Rotation2d.fromDegrees(-45 * (rotateRight ? 1 : -1)))
         };
         m_swerveSubsystem.setModuleStates(module_states);
     }
@@ -47,6 +48,6 @@ public void end(boolean interrupted) {
 
     @Override
     public boolean isFinished() {
-        return Math.abs(initialRotation - m_swerveSubsystem.getHeading()) >= rotationDegrees;
+        return Math.abs(Math.abs(initialRotation - m_swerveSubsystem.getHeading()) - rotationDegrees) < 1;
     }
 }