From 1b1e583fffd60717907dbf4a1f831d0f256998fc Mon Sep 17 00:00:00 2001 From: WTMC Robotics Date: Tue, 5 Mar 2024 15:33:07 -0500 Subject: [PATCH 1/2] Add AutonRotateWithPIDCommand (#33) --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/QuickActions.java | 8 +++++ .../auton/AutonRotateWithPIDCommand.java | 29 +++++++++++++++ src/main/java/frc/robot/auton/RotatePID.java | 36 +++++++++++++++++++ 4 files changed, 75 insertions(+) create mode 100644 src/main/java/frc/robot/auton/AutonRotateWithPIDCommand.java create mode 100644 src/main/java/frc/robot/auton/RotatePID.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0e125f4..5235589 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -43,6 +43,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.); /*PHYSICAL ROBOT CONSTANTS */ 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/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(); + } +} From aa9fa29c810591986451be95fdf21d46c0010d64 Mon Sep 17 00:00:00 2001 From: WTMC Robotics Date: Tue, 5 Mar 2024 15:35:25 -0500 Subject: [PATCH 2/2] shutdown actions before removing them (#32) --- src/main/java/frc/robot/RobotComponents/Climber.java | 4 ++-- src/main/java/frc/robot/auton/ParallelActionRunner.java | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) 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/ParallelActionRunner.java b/src/main/java/frc/robot/auton/ParallelActionRunner.java index 4c604b1..2c912e4 100644 --- a/src/main/java/frc/robot/auton/ParallelActionRunner.java +++ b/src/main/java/frc/robot/auton/ParallelActionRunner.java @@ -31,6 +31,7 @@ public void removeActionsOfType(Class clazz) { while (it.hasNext()) { AutonAction auction = it.next(); if (auction.getClass().equals(clazz)) { + auction.shutdown(); actions.remove(auction); } }