add updated drive state and fix relevant commands + fix 0 pid

team401 · Feb 3, 2024 · 22cddc8 · 22cddc8
1 parent 76c5cb7
commit 22cddc8
Show file tree

Hide file tree

Showing 5 changed files with 66 additions and 21 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -22,7 +22,7 @@
  */
 public final class Constants {
 
-    public static enum driveTrainState {
+    public static enum DriveTrainState {
         /*Manual control */
         MANUAL,
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -88,6 +88,17 @@ public RobotContainer() {
                         gyroSupplier,
                         simulatedPoseSupplier);
 
+        // add suppliers for drive
+        drive.setCurrentHeadingSupplier(
+                () -> {
+                    return nav.getCurrentHeading();
+                });
+
+        drive.setTargetHeadingSupplier(
+                () -> {
+                    return nav.getTargetHeading();
+                });
+
         // Set up auto routines
 
         // Set up feedforward characterization

diff --git a/src/main/java/frc/robot/commands/AimAtTarget.java b/src/main/java/frc/robot/commands/AimAtTarget.java
@@ -1,6 +1,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.DriveTrainState;
 import frc.robot.subsystems.Navigation;
 import frc.robot.subsystems.drive.Drive;
 
@@ -20,10 +21,19 @@ public AimAtTarget(Drive driveSubsystem, Navigation navSubsystem, int targetId)
     @Override
     public void initialize() {
         navSubsystem.setDesiredTarget(targetId);
+        driveSubsystem.setDriveState(DriveTrainState.AIM);
     }
 
     @Override
-    public void execute() {
-        driveSubsystem.aim(navSubsystem.getCurrentHeading(), navSubsystem.getTargetHeading());
+    public void execute() {}
+
+    @Override
+    public void end(boolean interrupted) {
+        driveSubsystem.setDriveState(DriveTrainState.MANUAL);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(navSubsystem.getCurrentHeading() - navSubsystem.getTargetHeading()) < 1e-5;
     }
 }
diff --git a/src/main/java/frc/robot/commands/ArcadeDrive.java b/src/main/java/frc/robot/commands/ArcadeDrive.java
@@ -1,7 +1,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants.driveTrainState;
+import frc.robot.Constants.DriveTrainState;
 import frc.robot.subsystems.drive.Drive;
 import java.util.function.DoubleSupplier;
 
@@ -20,12 +20,13 @@ public ArcadeDrive(Drive d, DoubleSupplier xPcent, DoubleSupplier yPcent) {
     }
 
     @Override
-    public void initialize() {}
+    public void initialize() {
+        drivesub.setDriveState(DriveTrainState.MANUAL);
+    }
 
     @Override
     public void execute() {
-        drivesub.setArcadeDrive(
-                xPercent.getAsDouble(), yPercent.getAsDouble(), driveTrainState.MANUAL);
+        drivesub.setArcadeDrive(xPercent.getAsDouble(), yPercent.getAsDouble());
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -6,24 +6,35 @@
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
-import frc.robot.Constants.driveTrainState;
+import frc.robot.Constants.DriveTrainState;
+import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 public class Drive extends SubsystemBase {
 
     private final DriveIO io;
-    private driveTrainState mode;
+    private DriveTrainState mode;
+    private DoubleSupplier targetHeading;
+    private DoubleSupplier currentHeading;
     private final DriveIOInputsAutoLogged inputs = new DriveIOInputsAutoLogged();
 
-    private PIDController thetaController = new PIDController(0, 0, 0); // placeholders
+    private PIDController thetaController = new PIDController(0.05, 0.05, 0.05); // placeholders
 
     private double forward;
     private double rotation;
 
     public Drive(DriveIO io) {
         this.io = io;
-        this.mode = driveTrainState.MANUAL;
+        this.targetHeading =
+                () -> {
+                    return 0.0;
+                };
+        this.currentHeading =
+                () -> {
+                    return 0.0;
+                };
+        this.mode = DriveTrainState.MANUAL;
     }
 
     @Override
@@ -33,10 +44,22 @@ public void periodic() {
         controlDriveTrain();
     }
 
-    public void setArcadeDrive(double forward, double rotation, driveTrainState mode) {
+    public void setDriveState(DriveTrainState state) {
+        mode = state;
+    }
+
+    public void setCurrentHeadingSupplier(DoubleSupplier currentHeading) {
+        this.currentHeading = currentHeading;
+    }
+
+    public void setTargetHeadingSupplier(DoubleSupplier targetHeading) {
+        this.targetHeading = targetHeading;
+    }
+
+    public void setArcadeDrive(double forward, double rotation) {
         this.forward = forward;
         this.rotation = rotation;
-        this.mode = mode;
+        mode = DriveTrainState.MANUAL;
     }
 
     /** Run open loop based on stick positions. */
@@ -50,10 +73,14 @@ public void stop() {
         io.setVoltage(0.0, 0.0);
     }
 
-    public void aim(double targetHeading, double currentHeading) {
+    public void aim() {
         this.forward = 0;
-        this.rotation = thetaController.calculate(currentHeading, targetHeading);
-        this.mode = driveTrainState.AIM;
+        this.rotation =
+                thetaController.calculate(
+                        currentHeading.getAsDouble(), targetHeading.getAsDouble());
+        if (Math.abs(currentHeading.getAsDouble() - targetHeading.getAsDouble()) < 1e-5) {
+            mode = DriveTrainState.MANUAL;
+        }
     }
 
     public Rotation2d getGyroRotation2d() {
@@ -88,17 +115,13 @@ public Pose2d getSimulatedPose() {
         return inputs.simulatedPose;
     }
 
-    public void updateMode(driveTrainState mode) {
-        this.mode = mode;
-    }
-
     public void controlDriveTrain() {
         switch (mode) {
             case MANUAL:
                 this.driveArcade(forward, rotation);
                 break;
             case AIM:
-                this.driveArcade(forward, rotation);
+                this.aim();
                 break;
             default:
                 this.stop();