From fe784870742bea57aed9b299550dd99643b9acb4 Mon Sep 17 00:00:00 2001 From: team6101 Date: Mon, 26 Feb 2024 15:55:38 -0500 Subject: [PATCH 1/5] Improve ParallelActionRunner --- .../frc/robot/auton/ParallelActionRunner.java | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/auton/ParallelActionRunner.java b/src/main/java/frc/robot/auton/ParallelActionRunner.java index aa8fb30..45dd32c 100644 --- a/src/main/java/frc/robot/auton/ParallelActionRunner.java +++ b/src/main/java/frc/robot/auton/ParallelActionRunner.java @@ -1,6 +1,7 @@ package frc.robot.auton; import java.util.ArrayList; +import java.util.Iterator; import java.util.List; public class ParallelActionRunner { @@ -13,11 +14,24 @@ public void addActionToRun(AutonAction action) { } public void onEveryFrame() { - for (int i = 0; i < actions.size(); i++) { - AutonAction action = actions.get(i); + Iterator it = actions.iterator(); + while (it.hasNext()) { + AutonAction action = it.next(); if (action.isDone()) { - actions.remove(i); - i--; + actions.remove(action); + } + } + } + + // This method is important if you have code that could add two of the same action with opposite goals. + // That would mean they would fight each other and never finish, which is bad news bears if you ask me. + public void removeActionsOfType(AutonAction action) { + Class clazz = action.getClass(); + Iterator it = actions.iterator(); + while (it.hasNext()) { + AutonAction auction = it.next(); + if (auction.getClass().equals(clazz)) { + actions.remove(auction); } } } From 4bd889bc5a445cd57cbef54b2cc782c6ca37e09a Mon Sep 17 00:00:00 2001 From: team6101 Date: Mon, 26 Feb 2024 16:44:00 -0500 Subject: [PATCH 2/5] made the arms use encoders instead of time --- src/main/java/frc/robot/Constants.java | 12 +++++------- src/main/java/frc/robot/auton/ExtendArms.java | 5 +---- 2 files changed, 6 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 30c47fe..92ff99a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -18,10 +18,8 @@ public class Constants { public static final double REV_TIME = 1.0; /*CLIMB CONFIG */ - /**the time we give the arms to extend in seconds */ - public static final double ARM_EXTENSION_TIME = 3; - /**the time we give the arms to retract in seconds */ - public static final double ARM_RETRACTION_TIME = 3; + public static final double CLIMB_EXTENDED_ENCODER_POSITION = 5; + public static final double CLIMB_RETRACTED_ENCODER_POSITION = 0.5; /**the % of max power we give to the arm motors while extending */ public static final double ARM_EXTENSION_SPEED = .25; /**the % of max power we give to the arm motors while retracting */ @@ -37,8 +35,6 @@ public class Constants { public static final int CAMERA_WIDTH = 640; public static final int CAMERA_BIT_CORRECTION_AMOUNT = 0; /**This is the amount of frames in a row that an april tag needs in frame to be to be considered confidently an april tag */ - public static final int APRIL_TAG_CONFIDENCE_FRAMES = 3; - public static final double APRIL_TAG_ROTATION_ZONE = 5; /*PID GAINS *///0.00001, 100 public static final Gains ROTATION_GAINS = new Gains(0.011, 0.0001, 0.000935, 0, 0, 0.3); @@ -58,7 +54,9 @@ public class Constants { /** amount of time in ms to wait for confirmation */ public static final int TIMEOUT_MS = 30; public static final double ENCODER_ROTATION = 4096.0; - /* ROTATION PID CONSTANTS */ + public static final int APRIL_TAG_CONFIDENCE_FRAMES = 3; + public static final double APRIL_TAG_ROTATION_ZONE = 5; + public static final double ROTATION_ERROR_DEGREES = 5.; } diff --git a/src/main/java/frc/robot/auton/ExtendArms.java b/src/main/java/frc/robot/auton/ExtendArms.java index 72223d6..7cf74be 100644 --- a/src/main/java/frc/robot/auton/ExtendArms.java +++ b/src/main/java/frc/robot/auton/ExtendArms.java @@ -6,11 +6,9 @@ public class ExtendArms extends AutonAction { - private double extensionTime; - @Override public boolean isDone() { - if (Timer.getFPGATimestamp() >= extensionTime) { + if (Robot.motors.getLeftClimb().getEncoderPosition() > Constants.CLIMB_EXTENDED_ENCODER_POSITION) { Robot.motors.getLeftClimb().set(0); Robot.motors.getRightClimb().set(0); return true; @@ -22,6 +20,5 @@ public boolean isDone() { public void initiate() { Robot.motors.getLeftClimb().set(Constants.ARM_EXTENSION_SPEED); Robot.motors.getRightClimb().set(Constants.ARM_EXTENSION_SPEED); - extensionTime = Timer.getFPGATimestamp() + Constants.ARM_EXTENSION_TIME; } } From 21aa4b1e2704bdd7130c4b665e2a701bc9744c2d Mon Sep 17 00:00:00 2001 From: team6101 Date: Mon, 26 Feb 2024 16:49:03 -0500 Subject: [PATCH 3/5] actually saved the retract arms file --- src/main/java/frc/robot/auton/RetractArms.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/auton/RetractArms.java b/src/main/java/frc/robot/auton/RetractArms.java index d8ab569..54ab236 100644 --- a/src/main/java/frc/robot/auton/RetractArms.java +++ b/src/main/java/frc/robot/auton/RetractArms.java @@ -6,11 +6,9 @@ public class RetractArms extends AutonAction { - private double extensionTime; - @Override public boolean isDone() { - if (Timer.getFPGATimestamp() >= extensionTime) { + if (Robot.motors.getLeftClimb().getEncoderPosition() < Constants.CLIMB_RETRACTED_ENCODER_POSITION) { Robot.motors.getLeftClimb().set(0); Robot.motors.getRightClimb().set(0); return true; @@ -22,6 +20,5 @@ public boolean isDone() { public void initiate() { Robot.motors.getLeftClimb().set(Constants.ARM_RETRACTION_SPEED); Robot.motors.getRightClimb().set(Constants.ARM_RETRACTION_SPEED); - extensionTime = Timer.getFPGATimestamp() + Constants.ARM_RETRACTION_TIME; } } From 2b6bfa33e20265b44a0ff9dc581dabb90367aad9 Mon Sep 17 00:00:00 2001 From: team6101 Date: Mon, 26 Feb 2024 16:54:40 -0500 Subject: [PATCH 4/5] put the constants back --- src/main/java/frc/robot/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 92ff99a..7fe0b71 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -35,6 +35,8 @@ public class Constants { public static final int CAMERA_WIDTH = 640; public static final int CAMERA_BIT_CORRECTION_AMOUNT = 0; /**This is the amount of frames in a row that an april tag needs in frame to be to be considered confidently an april tag */ + public static final int APRIL_TAG_CONFIDENCE_FRAMES = 3; + public static final double APRIL_TAG_ROTATION_ZONE = 5; /*PID GAINS *///0.00001, 100 public static final Gains ROTATION_GAINS = new Gains(0.011, 0.0001, 0.000935, 0, 0, 0.3); @@ -55,8 +57,6 @@ public class Constants { public static final int TIMEOUT_MS = 30; public static final double ENCODER_ROTATION = 4096.0; /* ROTATION PID CONSTANTS */ - public static final int APRIL_TAG_CONFIDENCE_FRAMES = 3; - public static final double APRIL_TAG_ROTATION_ZONE = 5; public static final double ROTATION_ERROR_DEGREES = 5.; } From 471241bf3635f358ccf3ea5dbe1f38a1ef420bc3 Mon Sep 17 00:00:00 2001 From: team6101 Date: Mon, 26 Feb 2024 17:15:06 -0500 Subject: [PATCH 5/5] make arm control manual --- src/main/java/frc/robot/Constants.java | 4 ++-- .../java/frc/robot/InputtedCoDriverControls.java | 12 ++++++++++-- src/main/java/frc/robot/auton/ExtendArms.java | 4 ++-- .../java/frc/robot/auton/ParallelActionRunner.java | 7 +++++-- src/main/java/frc/robot/auton/RetractArms.java | 4 ++-- 5 files changed, 21 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7fe0b71..ce44d6e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -21,9 +21,9 @@ public class Constants { public static final double CLIMB_EXTENDED_ENCODER_POSITION = 5; public static final double CLIMB_RETRACTED_ENCODER_POSITION = 0.5; /**the % of max power we give to the arm motors while extending */ - public static final double ARM_EXTENSION_SPEED = .25; + public static final double CLIMB_EXTENSION_SPEED = .25; /**the % of max power we give to the arm motors while retracting */ - public static final double ARM_RETRACTION_SPEED = .25; + public static final double CLIMB_RETRACTION_SPEED = -.25; /*DRIVE TRAIN CONFIG */ public static final double CONTROLLER_DEADZONE = 0.06; diff --git a/src/main/java/frc/robot/InputtedCoDriverControls.java b/src/main/java/frc/robot/InputtedCoDriverControls.java index 76cde03..73fd528 100644 --- a/src/main/java/frc/robot/InputtedCoDriverControls.java +++ b/src/main/java/frc/robot/InputtedCoDriverControls.java @@ -14,6 +14,8 @@ public static void setCoDriverController(XboxController xboxController) { } public static void onEveryFrame() { + Robot.motors.getLeftClimb().stopMotor(); + Robot.motors.getRightClimb().stopMotor(); if (controller.getAButton()) { Robot.motors.getFeeder().set(30); } else { @@ -24,19 +26,25 @@ public static void onEveryFrame() { } doShooterIntake(); if (controller.getStartButton()) { - extendArms(); + //extendArms(); + Robot.motors.getLeftClimb().set(Constants.CLIMB_EXTENSION_SPEED); + Robot.motors.getRightClimb().set(Constants.CLIMB_EXTENSION_SPEED); } if (controller.getBackButton()) { - retractArms(); + //retractArms(); + Robot.motors.getLeftClimb().set(Constants.CLIMB_RETRACTION_SPEED); + Robot.motors.getRightClimb().set(Constants.CLIMB_RETRACTION_SPEED); } } private static void extendArms() { Robot.getTeleopActionRunner().addActionToRun(new ExtendArms()); + Robot.getTeleopActionRunner().removeActionsOfType(RetractArms.class); } private static void retractArms() { Robot.getTeleopActionRunner().addActionToRun(new RetractArms()); + Robot.getTeleopActionRunner().removeActionsOfType(ExtendArms.class); } private static void doShooterIntake() { diff --git a/src/main/java/frc/robot/auton/ExtendArms.java b/src/main/java/frc/robot/auton/ExtendArms.java index 7cf74be..7a28e0a 100644 --- a/src/main/java/frc/robot/auton/ExtendArms.java +++ b/src/main/java/frc/robot/auton/ExtendArms.java @@ -18,7 +18,7 @@ public boolean isDone() { @Override public void initiate() { - Robot.motors.getLeftClimb().set(Constants.ARM_EXTENSION_SPEED); - Robot.motors.getRightClimb().set(Constants.ARM_EXTENSION_SPEED); + Robot.motors.getLeftClimb().set(Constants.CLIMB_EXTENSION_SPEED); + Robot.motors.getRightClimb().set(Constants.CLIMB_EXTENSION_SPEED); } } diff --git a/src/main/java/frc/robot/auton/ParallelActionRunner.java b/src/main/java/frc/robot/auton/ParallelActionRunner.java index 45dd32c..ddfb641 100644 --- a/src/main/java/frc/robot/auton/ParallelActionRunner.java +++ b/src/main/java/frc/robot/auton/ParallelActionRunner.java @@ -25,8 +25,7 @@ public void onEveryFrame() { // This method is important if you have code that could add two of the same action with opposite goals. // That would mean they would fight each other and never finish, which is bad news bears if you ask me. - public void removeActionsOfType(AutonAction action) { - Class clazz = action.getClass(); + public void removeActionsOfType(Class clazz) { Iterator it = actions.iterator(); while (it.hasNext()) { AutonAction auction = it.next(); @@ -35,4 +34,8 @@ public void removeActionsOfType(AutonAction action) { } } } + + public void removeActionsOfType(AutonAction action) { + removeActionsOfType(action.getClass()); + } } diff --git a/src/main/java/frc/robot/auton/RetractArms.java b/src/main/java/frc/robot/auton/RetractArms.java index 54ab236..f3d2115 100644 --- a/src/main/java/frc/robot/auton/RetractArms.java +++ b/src/main/java/frc/robot/auton/RetractArms.java @@ -18,7 +18,7 @@ public boolean isDone() { @Override public void initiate() { - Robot.motors.getLeftClimb().set(Constants.ARM_RETRACTION_SPEED); - Robot.motors.getRightClimb().set(Constants.ARM_RETRACTION_SPEED); + Robot.motors.getLeftClimb().set(Constants.CLIMB_RETRACTION_SPEED); + Robot.motors.getRightClimb().set(Constants.CLIMB_RETRACTION_SPEED); } }