diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5246417..e901dae 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -52,6 +52,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; diff --git a/src/main/java/frc/robot/QuickActions.java b/src/main/java/frc/robot/QuickActions.java index f679b48..266fd4a 100644 --- a/src/main/java/frc/robot/QuickActions.java +++ b/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); diff --git a/src/main/java/frc/robot/RobotComponents/Climber.java b/src/main/java/frc/robot/RobotComponents/Climber.java index 0df2840..51e58c5 100644 --- a/src/main/java/frc/robot/RobotComponents/Climber.java +++ b/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() { diff --git a/src/main/java/frc/robot/auton/AutonRotateWithPIDCommand.java b/src/main/java/frc/robot/auton/AutonRotateWithPIDCommand.java new file mode 100644 index 0000000..603b6d4 --- /dev/null +++ b/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(); + } +} diff --git a/src/main/java/frc/robot/auton/RotatePID.java b/src/main/java/frc/robot/auton/RotatePID.java new file mode 100644 index 0000000..6ef3e09 --- /dev/null +++ b/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(); + } +}