Merge branch 'main' into add-auton-intake

src/main/java/frc/robot/Constants.java

@@ -54,6 +54,8 @@ public class Constants {
 
     /*PID GAINS *///0.00001, 100
     public static final Gains ROTATION_GAINS = new Gains(0.011, 0.0001, 0.000935, 0, 0, 0.3);
+    public static final double ROTATION_DEGREE_TOLERANCE = 3;
+    public static final double ROTATION_DEGREE_PER_SECOND_TOLERANCE = 30;
     public static final Gains NORMAL_ROBOT_GAINS = new Gains(0.0005, 0.0, 0, 0, 0, 1.);
     public static final double ROTATION_ERROR_DEGREES = 5.0;
 

src/main/java/frc/robot/QuickActions.java

@@ -12,6 +12,14 @@ public static void stopDriveMotors() {
         Robot.motors.getDriveRightParent().set(0.0);
     }
 
+    public static void turn(double percent) {
+        if (percent > 0) {
+            turn(TurnDirection.RIGHT, percent);
+        } else {
+            turn(TurnDirection.LEFT, percent);
+        }
+    }
+
     public static void turn(TurnDirection direction, double percent) {
         if (direction == TurnDirection.LEFT) {
             Robot.motors.getDriveLeftParent().set(-percent);

src/main/java/frc/robot/RobotComponents/Climber.java

@@ -8,13 +8,13 @@
 public class Climber {
 
     private static void extendArms() {
-        Robot.getTeleopActionRunner().addActionToRun(new ExtendArms());
         Robot.getTeleopActionRunner().removeActionsOfType(RetractArms.class);
+        Robot.getTeleopActionRunner().addActionToRun(new ExtendArms());
     }
 
     private static void retractArms() {
-        Robot.getTeleopActionRunner().addActionToRun(new RetractArms());
         Robot.getTeleopActionRunner().removeActionsOfType(ExtendArms.class);
+        Robot.getTeleopActionRunner().addActionToRun(new RetractArms());
     }
 
     public static void manualExtendArms() {

src/main/java/frc/robot/auton/AutonRotateWithPIDCommand.java

@@ -0,0 +1,29 @@
+package frc.robot.auton;
+
+import frc.robot.QuickActions;
+
+public class AutonRotateWithPIDCommand extends AutonAction {
+
+    private RotatePID pidCommand;
+    private double targetRelativeDegrees;
+
+    public AutonRotateWithPIDCommand(double targetRelativeDegrees) {
+        this.targetRelativeDegrees = targetRelativeDegrees;
+    }
+
+    @Override
+    public boolean isDone() {
+        pidCommand.execute();
+        return pidCommand.isFinished();
+    }
+
+    @Override
+    public void initiate() {
+        pidCommand = new RotatePID(targetRelativeDegrees);
+    }
+
+    @Override
+    public void shutdown() {
+        QuickActions.stopDriveMotors();
+    }
+}

src/main/java/frc/robot/auton/RotatePID.java

@@ -0,0 +1,36 @@
+package frc.robot.auton;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+import frc.robot.Constants;
+import frc.robot.QuickActions;
+import frc.robot.Robot;
+
+public class RotatePID extends PIDCommand {
+
+    public RotatePID(double relativeTurnDegree) {
+        super(
+            // The controller that the command will use
+            new PIDController(Constants.ROTATION_GAINS.P, Constants.ROTATION_GAINS.I, Constants.ROTATION_GAINS.D),
+            // This should return the measurement
+            () -> Robot.getGyroscope().getAngle(),
+            // This should return the setpoint (can also be a constant)
+            () -> Robot.getGyroscope().getAngle() + relativeTurnDegree,
+            // This uses the output
+            output -> {
+                QuickActions.turn(output);
+            }
+        );
+        // Use addRequirements() here to declare subsystem dependencies.
+        getController().enableContinuousInput(-180, 180);
+        // Configure additional PID options by calling `getController` here.
+        getController()
+            .setTolerance(Constants.ROTATION_DEGREE_TOLERANCE, Constants.ROTATION_DEGREE_PER_SECOND_TOLERANCE);
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return getController().atSetpoint();
+    }
+}