From d2fbdb40f73e268d1456fe8fc1691f074c9258d0 Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 15 Apr 2025 08:59:23 -0700 Subject: [PATCH 01/19] Adjust auto routines --- src/main/deploy/pathplanner/autos/0 - L.auto | 42 +++++++++++-------- src/main/deploy/pathplanner/autos/0 - M.auto | 42 +++++++++++-------- src/main/deploy/pathplanner/autos/0 - R.auto | 42 +++++++++++-------- src/main/deploy/pathplanner/autos/1 - L.auto | 12 ++++++ .../deploy/pathplanner/autos/1 - M PoleL.auto | 12 ++++++ .../deploy/pathplanner/autos/1 - M PoleR.auto | 12 ++++++ src/main/deploy/pathplanner/autos/1 - R.auto | 12 ++++++ src/main/deploy/pathplanner/autos/2 - L.auto | 12 ++++++ src/main/deploy/pathplanner/autos/2 - R.auto | 12 ++++++ .../deploy/pathplanner/autos/Pit Test.auto | 31 ++++++++++++++ 10 files changed, 175 insertions(+), 54 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Pit Test.auto diff --git a/src/main/deploy/pathplanner/autos/0 - L.auto b/src/main/deploy/pathplanner/autos/0 - L.auto index 8344b96c..302d99cd 100644 --- a/src/main/deploy/pathplanner/autos/0 - L.auto +++ b/src/main/deploy/pathplanner/autos/0 - L.auto @@ -1,19 +1,25 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Mobility Left" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": true +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ZeroWrist" + } + }, + { + "type": "path", + "data": { + "pathName": "Mobility Left" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/0 - M.auto b/src/main/deploy/pathplanner/autos/0 - M.auto index bed85134..9f6ed8ee 100644 --- a/src/main/deploy/pathplanner/autos/0 - M.auto +++ b/src/main/deploy/pathplanner/autos/0 - M.auto @@ -1,19 +1,25 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Mobility Center" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": true +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ZeroWrist" + } + }, + { + "type": "path", + "data": { + "pathName": "Mobility Center" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/0 - R.auto b/src/main/deploy/pathplanner/autos/0 - R.auto index 7c0348d1..69ad8c43 100644 --- a/src/main/deploy/pathplanner/autos/0 - R.auto +++ b/src/main/deploy/pathplanner/autos/0 - R.auto @@ -1,19 +1,25 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Mobility Right" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": true +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ZeroWrist" + } + }, + { + "type": "path", + "data": { + "pathName": "Mobility Right" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 - L.auto b/src/main/deploy/pathplanner/autos/1 - L.auto index 66e56376..29030662 100644 --- a/src/main/deploy/pathplanner/autos/1 - L.auto +++ b/src/main/deploy/pathplanner/autos/1 - L.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "ZeroWrist" + } + }, { "type": "path", "data": { @@ -15,6 +21,12 @@ "data": { "name": "AutoPlaceRight4" } + }, + { + "type": "named", + "data": { + "name": "ArmToStow" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/1 - M PoleL.auto b/src/main/deploy/pathplanner/autos/1 - M PoleL.auto index 47a07022..24064ebf 100644 --- a/src/main/deploy/pathplanner/autos/1 - M PoleL.auto +++ b/src/main/deploy/pathplanner/autos/1 - M PoleL.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "ZeroWrist" + } + }, { "type": "path", "data": { @@ -15,6 +21,12 @@ "data": { "name": "AutoPlaceLeft4" } + }, + { + "type": "named", + "data": { + "name": "ArmToStow" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/1 - M PoleR.auto b/src/main/deploy/pathplanner/autos/1 - M PoleR.auto index fc820102..fb689980 100644 --- a/src/main/deploy/pathplanner/autos/1 - M PoleR.auto +++ b/src/main/deploy/pathplanner/autos/1 - M PoleR.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "ZeroWrist" + } + }, { "type": "path", "data": { @@ -15,6 +21,12 @@ "data": { "name": "AutoPlaceRight4" } + }, + { + "type": "named", + "data": { + "name": "ArmToStow" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/1 - R.auto b/src/main/deploy/pathplanner/autos/1 - R.auto index 608b22a6..e8fb34e0 100644 --- a/src/main/deploy/pathplanner/autos/1 - R.auto +++ b/src/main/deploy/pathplanner/autos/1 - R.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "ZeroWrist" + } + }, { "type": "path", "data": { @@ -15,6 +21,12 @@ "data": { "name": "AutoPlaceLeft4" } + }, + { + "type": "named", + "data": { + "name": "ArmToStow" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/2 - L.auto b/src/main/deploy/pathplanner/autos/2 - L.auto index 8fc675f8..3b57d36a 100644 --- a/src/main/deploy/pathplanner/autos/2 - L.auto +++ b/src/main/deploy/pathplanner/autos/2 - L.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "ZeroWrist" + } + }, { "type": "path", "data": { @@ -52,6 +58,12 @@ "data": { "name": "AutoPlaceLeft4" } + }, + { + "type": "named", + "data": { + "name": "ArmToStow" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/2 - R.auto b/src/main/deploy/pathplanner/autos/2 - R.auto index e9633060..7685b962 100644 --- a/src/main/deploy/pathplanner/autos/2 - R.auto +++ b/src/main/deploy/pathplanner/autos/2 - R.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "ZeroWrist" + } + }, { "type": "path", "data": { @@ -64,6 +70,12 @@ "data": { "name": "AutoPlaceRight4" } + }, + { + "type": "named", + "data": { + "name": "ArmToStow" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Pit Test.auto b/src/main/deploy/pathplanner/autos/Pit Test.auto new file mode 100644 index 00000000..42dd0274 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Pit Test.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "TestPlaceReefImmobile" + } + }, + { + "type": "named", + "data": { + "name": "ArmToStow" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": true +} \ No newline at end of file From 5e379401b92f1ae9f9be2075d34b4a29e259a098 Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 15 Apr 2025 09:00:19 -0700 Subject: [PATCH 02/19] Remove problematic redundant logger init --- src/main/java/frc/robot/Robot.java | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b7ba2f74..f83ce0ab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,12 +23,6 @@ public class Robot extends LoggedRobot { @Override public void robotInit() { - - Logger.addDataReceiver(new WPILOGWriter("U/logs")); - Logger.addDataReceiver(new NT4Publisher()); - - Logger.start(); - m_robotContainer = new RobotContainer(); // Set up data receivers & replay source From c7e73f87790962c0447136f068d1c89f062842c9 Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 15 Apr 2025 09:00:56 -0700 Subject: [PATCH 03/19] Remove arm zeroing from teleopInit --- src/main/java/frc/robot/Robot.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f83ce0ab..59ab4ecc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -87,8 +87,6 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - - m_robotContainer.zeroArm().schedule(); } @Override From 6847fa46e263ff30fe42ecf04b57cc0e22633188 Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 15 Apr 2025 09:04:58 -0700 Subject: [PATCH 04/19] Zero arm after shooting --- src/main/java/frc/robot/RobotContainer.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6096f632..adad4337 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -198,8 +198,9 @@ private void configureBindings() { .onFalse(CompoundCommands.armToStowSafely()); controller .rightTrigger(0.25) - .whileTrue(intake.ejectCoralCommand()) - .onFalse(intake.stopCommand().alongWith(CompoundCommands.armToStow())); + .whileTrue(intake.intakeCoralCommand()) + .onFalse( + intake.stopCommand().alongWith(CompoundCommands.armToStowSafely()).andThen(zeroArm())); controller .rightStick() From 70dbaea0203343ec631b18359d14d69e41bf592e Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 15 Apr 2025 09:05:52 -0700 Subject: [PATCH 05/19] Adjust positions --- .../robot/constants/PositionConstants.java | 39 +++++++++++-------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/constants/PositionConstants.java b/src/main/java/frc/robot/constants/PositionConstants.java index 919c5f64..d85c4e88 100644 --- a/src/main/java/frc/robot/constants/PositionConstants.java +++ b/src/main/java/frc/robot/constants/PositionConstants.java @@ -77,32 +77,35 @@ public static Pose2d getSourcePose(boolean isOnRight) { } // L1 Arm Setpoint Values - public static final double L1WristAngle = 42.199999999999996; - public static final double L1PivotAngle = 33.10199999999992; + public static final double L1WristAngle = 63.11; + public static final double L1PivotAngle = 22.81; public static final double L1ElevatorExtension = 0; // L2 Arm Setpoint Values - public static final double L2WristAngle = 6.799999999999816; - public static final double L2PivotAngle = 72.79999999999995; + public static final double L2WristAngle = 39.109999999999985; + public static final double L2PivotAngle = 51.82; public static final double L2ElevatorExtension = 0; // L3 Arm Setpoint Values - public static final double L3WristAngle = -5.199999999999999; - public static final double L3PivotAngle = 76.2; - public static final double L3ElevatorExtension = 0.46500000000000263; + public static final double L3WristAngle = 22.31; + public static final double L3PivotAngle = 61.62; + public static final double L3ElevatorExtension = 0.26; // L4 Arm Setpoint Values - public static final double L4WristAngle = 59.94921875000001; + public static final double L4WristAngleAuto = 52.74921875000003; + public static final double L4PivotAngleAuto = 74.399999999999781; + + public static final double L4WristAngle = 54.349; public static final double L4PivotAngle = 74.72634881514921; public static final double L4ElevatorExtension = 0.9051441865808824; // Source Arm Setpoint Values - public static final double sourceWristAngle = 108.275; + public static final double sourceWristAngle = 111.475; public static final double sourcePivotAngle = 65.88248059405254; public static final double sourceElevatorExtension = 0; // Climb Arm Setpoint Values - public static final double climbWristAngle = -26.745; + public static final double climbWristAngle = 12.4; public static final double climbPivotAngle = 24; public static final double climbElevatorExtension = 0; @@ -111,15 +114,15 @@ public static Pose2d getSourcePose(boolean isOnRight) { public static final double stowPivotAngle = 81.81999599249039; public static final double stowElevatorExtension = 0; - public static final double safePivotPosition = 80.40000000000006; + public static final double safePivotPosition = 90.0; - public static final double lowerAlgaeRemovalPivotAngle = 32.33384351834553; - public static final double lowerAlgaeRemovalWristAngle = 90; + public static final double lowerAlgaeRemovalPivotAngle = 54.9999999999995; + public static final double lowerAlgaeRemovalWristAngle = 143.11; public static final double lowerAlgaeRemovalElevatorHeight = 0; - public static final double upperAlgaeRemovalPivotAngle = 58.533843518345904; - public static final double upperAlgaeRemovalWristAngle = 71.60000000000007; - public static final double upperAlgaeRemovalElevatorHeight = 0.15000000000000005; + public static final double upperAlgaeRemovalPivotAngle = 78.51970529998351; + public static final double upperAlgaeRemovalWristAngle = 159.2; + public static final double upperAlgaeRemovalElevatorHeight = 0.31000000000000016; public static final double[][] reefArmPositions = { {L1WristAngle, L1PivotAngle, L1ElevatorExtension}, @@ -127,4 +130,8 @@ public static Pose2d getSourcePose(boolean isOnRight) { {L3WristAngle, L3PivotAngle, L3ElevatorExtension}, {L4WristAngle, L4PivotAngle, L4ElevatorExtension} }; + + public static final double NetWristAngle = 139.11; + public static final double NetPivotAngle = 87.81; + public static final double NetElevatorExtension = 1.15; } From 3fc0e352729a5a3460631d280da0622f24087b77 Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 15 Apr 2025 09:07:18 -0700 Subject: [PATCH 06/19] Add barge ability --- src/main/java/frc/robot/RobotContainer.java | 6 ++++++ src/main/java/frc/robot/util/CompoundCommands.java | 4 ++++ 2 files changed, 10 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index adad4337..62c128b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -164,6 +164,12 @@ private void configureBindings() { backupController.rightBumper().whileTrue(intake.intakeCoralCommand()); + backupController.start().whileTrue(CompoundCommands.armToNet()); + + backupController.x().whileTrue(intake.intakeAlgaeCommand()); + + backupController.y().whileTrue(intake.ejectAlgaeCommand()); + backupController .povUp() .onTrue( diff --git a/src/main/java/frc/robot/util/CompoundCommands.java b/src/main/java/frc/robot/util/CompoundCommands.java index 58d9df62..1156dfa2 100644 --- a/src/main/java/frc/robot/util/CompoundCommands.java +++ b/src/main/java/frc/robot/util/CompoundCommands.java @@ -138,6 +138,10 @@ public static Command armToReef(int reefLevel) { reefArmPositions[reefLevel - 1][2]); } + public static Command armToNet() { + return armToGoal(NetWristAngle, NetPivotAngle, NetElevatorExtension); + } + public static Command armToAlgae(boolean upper) { if (upper) { return armToGoal( From c37d4552eb41b4c339223b6a3ff27f833939aa00 Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 15 Apr 2025 09:11:09 -0700 Subject: [PATCH 07/19] Debounce wrist current, log when rezeroing --- .../frc/robot/constants/WristConstants.java | 3 +++ .../frc/robot/subsystems/wrist/Wrist.java | 19 ++++++++++++++++--- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/constants/WristConstants.java b/src/main/java/frc/robot/constants/WristConstants.java index 1d964211..52b66db2 100644 --- a/src/main/java/frc/robot/constants/WristConstants.java +++ b/src/main/java/frc/robot/constants/WristConstants.java @@ -80,6 +80,9 @@ public class WristConstants { /** */ public static final double ZEROING_VOLTAGE = 1; // TODO tune + /** Rising debounce time for amperage at max */ + public static final double ZEROING_DEBOUNCE_TIME = 0.5; + /** */ public static final double WRIST_ZEROING_MAX_AMPS = 20; // TODO tune diff --git a/src/main/java/frc/robot/subsystems/wrist/Wrist.java b/src/main/java/frc/robot/subsystems/wrist/Wrist.java index c78fdbfe..07f19a4a 100644 --- a/src/main/java/frc/robot/subsystems/wrist/Wrist.java +++ b/src/main/java/frc/robot/subsystems/wrist/Wrist.java @@ -4,8 +4,11 @@ import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.wpilibj2.command.Commands.waitUntil; import static frc.robot.constants.GeneralConstants.UPDATE_PERIOD; +import static frc.robot.constants.PositionConstants.stowWristAngle; import static frc.robot.constants.WristConstants.*; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,6 +23,9 @@ public class Wrist extends SubsystemBase { private final WristIOInputsAutoLogged inputs = new WristIOInputsAutoLogged(); private final SysIdRoutine sysId; + private final Debouncer currentDebouncer = + new Debouncer(ZEROING_DEBOUNCE_TIME, DebounceType.kRising); + public Wrist(WristIO io) { this.io = io; @@ -117,9 +123,16 @@ public Command changeGoalPosition(double velocityDegPerSec) { /** */ public Command zeroWrist() { - return run(() -> setVoltage(ZEROING_VOLTAGE)) - .until(() -> inputs.wristCurrentAmps > WRIST_ZEROING_MAX_AMPS) + return runOnce(() -> Logger.recordOutput("Wrist/rezeroing", true)) + .andThen(run(() -> setVoltage(ZEROING_VOLTAGE))) + .until(() -> currentDebouncer.calculate(inputs.wristCurrentAmps > WRIST_ZEROING_MAX_AMPS)) .andThen(runOnce(() -> io.setMotorZero())) - .andThen(runOnce(() -> stopWrist())); + .andThen(runOnce(() -> stopWrist())) + .finallyDo( + () -> { + Logger.recordOutput("Wrist/rezeroing", false); + currentDebouncer.calculate(false); + }) + .andThen(setGoalPosition(() -> stowWristAngle)); } } From 616676200c26220a2bf1a7e98bc2b33678392b7c Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 15 Apr 2025 09:35:54 -0700 Subject: [PATCH 08/19] Adjust arm speeds --- src/main/java/frc/robot/constants/ElevatorConstants.java | 3 +++ src/main/java/frc/robot/constants/PivotConstants.java | 4 ++++ src/main/java/frc/robot/constants/WristConstants.java | 3 +++ .../java/frc/robot/subsystems/elevator/ElevatorIOReal.java | 4 ++-- src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java | 4 ++-- src/main/java/frc/robot/subsystems/wrist/WristIOReal.java | 4 ++-- 6 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index fcf38af0..a8bc18cc 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -93,6 +93,9 @@ public class ElevatorConstants { public static final double moveVoltage = 5.0; + public static final double MAX_SPEED = 3.6 + public static final double MAX_ACCELERATION = 4.0 + /* Device IDs */ public static final int motorID = 12; diff --git a/src/main/java/frc/robot/constants/PivotConstants.java b/src/main/java/frc/robot/constants/PivotConstants.java index e13821ee..b9efcc43 100644 --- a/src/main/java/frc/robot/constants/PivotConstants.java +++ b/src/main/java/frc/robot/constants/PivotConstants.java @@ -46,6 +46,10 @@ public class PivotConstants { /** */ public static final double GEAR_RATIO = 378; + public static final double MAX_SPEED = 0.525; + + public static final double MAX_ACCELERATION = 0.225; + /** */ public static final ArmFeedforward FF_MODEL = new ArmFeedforward(0.11164, 0.0090459, 0.11954, 0.0090459); diff --git a/src/main/java/frc/robot/constants/WristConstants.java b/src/main/java/frc/robot/constants/WristConstants.java index 52b66db2..488a81d4 100644 --- a/src/main/java/frc/robot/constants/WristConstants.java +++ b/src/main/java/frc/robot/constants/WristConstants.java @@ -22,6 +22,9 @@ public class WristConstants { /** ID of the wrist sparkmax */ public static final int MOTOR_ID = 14; + public static final double MAX_SPEED = 0.5 + public static final double MAX_ACCELERATION = 1.0 + /** */ public static final boolean MOTOR_INVERTED = false; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 487cd635..7b660edf 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -45,8 +45,8 @@ public ElevatorIOReal() { leftMotorConfig.Slot0.kI = 0; leftMotorConfig.Slot0.kD = 0; - leftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 4 / rotToMetMultFactor; - leftMotorConfig.MotionMagic.MotionMagicAcceleration = 4 / rotToMetMultFactor; + leftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = MAX_SPEED / rotToMetMultFactor; + leftMotorConfig.MotionMagic.MotionMagicAcceleration = MAX_ACCELERATION / rotToMetMultFactor; leftMotorConfig.MotionMagic.MotionMagicJerk = 0 / rotToMetMultFactor; leadingMotor.getConfigurator().apply(leftMotorConfig); diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java index 98eb8c01..e438435a 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java @@ -93,8 +93,8 @@ public PivotIOReal() { leftMotorConfig.Slot0.kI = 0; leftMotorConfig.Slot0.kD = 0; - leftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = GEAR_RATIO * 0.75; - leftMotorConfig.MotionMagic.MotionMagicAcceleration = GEAR_RATIO * 0.75; + leftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = GEAR_RATIO * MAX_SPEED; + leftMotorConfig.MotionMagic.MotionMagicAcceleration = GEAR_RATIO * MAX_ACCELERATION; leftMotorConfig.MotionMagic.MotionMagicJerk = 0; leadingMotor.getConfigurator().apply(leftMotorConfig); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java index b76888bd..dadc8cd5 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java @@ -39,8 +39,8 @@ public WristIOReal() { motorConfig.Slot0.kI = 0; motorConfig.Slot0.kD = 0; - motorConfig.MotionMagic.MotionMagicCruiseVelocity = 2500 / MOTOR_RATIO; - motorConfig.MotionMagic.MotionMagicAcceleration = 5000 / MOTOR_RATIO; + motorConfig.MotionMagic.MotionMagicCruiseVelocity = MAX_SPEED * MOTOR_RATIO; + motorConfig.MotionMagic.MotionMagicAcceleration = MAX_ACCELERATION * MOTOR_RATIO; motorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; From a386970a192f9e14e4cf16d821202794a6a89c69 Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 15 Apr 2025 09:39:54 -0700 Subject: [PATCH 09/19] Move all arm mechanisms simultaneously --- .../java/frc/robot/util/CompoundCommands.java | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/util/CompoundCommands.java b/src/main/java/frc/robot/util/CompoundCommands.java index 1156dfa2..e2c3fc6d 100644 --- a/src/main/java/frc/robot/util/CompoundCommands.java +++ b/src/main/java/frc/robot/util/CompoundCommands.java @@ -282,6 +282,21 @@ public static DeferredCommand deferDrive(Supplier command) { return new DeferredCommand(command, Set.of(drive)); } + /** + * Generates a {@link Command} to move the arm to the specified position, moving all mechanisms simultaneously. + * + * @param wristSetPos Angle in degrees to set the wrist + * @param pivotSetPos Angle in degrees to set the pivot + * @param elevatorSetPos Height in meters to set the elevator + * @return A {@link Command} to set the arm goal position to the specified position + */ + public static Command armToGoal(double wristSetPos, double pivotSetPos, double elevatorSetPos) { + return pivot + .setPosition(() -> pivotSetPos) + .alongWith(wrist.setGoalPosition(() -> wristSetPos)) + .alongWith(elevator.setPosition((() -> elevatorSetPos))) + } + /** * Generates a {@link Command} to move the arm to the specified position, ensuring to always stay * in frame perimeter. If the goal position needs the elevator to extend, we first move pivot, @@ -293,8 +308,7 @@ public static DeferredCommand deferDrive(Supplier command) { * @param elevatorSetPos Height in meters to set the elevator * @return A {@link Command} to set the arm goal position to the specified position */ - public static Command armToGoal(double wristSetPos, double pivotSetPos, double elevatorSetPos) { - + public static Command armToGoalSafely(double wristSetPos, double pivotSetPos, double elevatorSetPos) { if (elevatorSetPos > elevator.getPosition().getAsDouble()) { return elevator .setPosition(() -> elevatorSetPos) From 59322571c91cfb1887128a5e2b741aa56e992d6b Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 15 Apr 2025 09:40:43 -0700 Subject: [PATCH 10/19] Don't launch coral away from reef in auto --- src/main/java/frc/robot/util/CompoundCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/CompoundCommands.java b/src/main/java/frc/robot/util/CompoundCommands.java index e2c3fc6d..66da456b 100644 --- a/src/main/java/frc/robot/util/CompoundCommands.java +++ b/src/main/java/frc/robot/util/CompoundCommands.java @@ -109,7 +109,7 @@ public static Command placeReef(boolean isOnRight, int level) { new ParallelCommandGroup(driveCommand, armToReefSafely(level)).withTimeout(5.0); // Small wait to ensure arm is stable before shooting - return alignCommand.andThen(waitSeconds(0.5)).andThen(ejectCoral()); + return alignCommand.andThen(waitSeconds(0.5)).andThen(intakeCoral()); } public static Command intakeSource(boolean isOnRight) { From 2c0d73d693ec26cacbb46d1d4da31ea87e15137a Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 15 Apr 2025 09:44:17 -0700 Subject: [PATCH 11/19] Avoid algae when placing reef, add immobile pit test auto place --- .../java/frc/robot/util/CompoundCommands.java | 26 +++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/util/CompoundCommands.java b/src/main/java/frc/robot/util/CompoundCommands.java index 66da456b..8e48d01d 100644 --- a/src/main/java/frc/robot/util/CompoundCommands.java +++ b/src/main/java/frc/robot/util/CompoundCommands.java @@ -60,7 +60,6 @@ public static void configure( } private static void configureNamedCommands() { - // TODO: Change auto routines to work with different NamedCommands NamedCommands.registerCommand("DriveReefL", driveToLeftReef(1)); NamedCommands.registerCommand("DriveReefR", driveToRightReef(1)); @@ -79,6 +78,8 @@ private static void configureNamedCommands() { NamedCommands.registerCommand("AutoPlaceRight2", placeReef(true, 2)); NamedCommands.registerCommand("AutoPlaceRight1", placeReef(true, 1)); + NamedCommands.registerCommand("TestPlaceReefImmobile", immobilePlaceReef(false, 4)); + NamedCommands.registerCommand("Eject", ejectCoral()); NamedCommands.registerCommand("DriveSourceLeft", driveToLeftSource()); @@ -87,7 +88,9 @@ private static void configureNamedCommands() { NamedCommands.registerCommand("AutoIntakeSourceLeft", intakeSource(false)); NamedCommands.registerCommand("AutoIntakeSourceRight", intakeSource(true)); - NamedCommands.registerCommand("ArmStow", armToStow()); + NamedCommands.registerCommand("ArmToStow", armToStow()); + + NamedCommands.registerCommand("ZeroWrist", wrist.zeroWrist()); } /** @@ -105,6 +108,25 @@ public static Command placeReef(boolean isOnRight, int level) { driveCommand = driveToLeftReef(level); } + Command alignCommand = + new ParallelCommandGroup(driveCommand, armToReefAvoidAlgae(level)).withTimeout(5.0); + + // Small wait to ensure arm is stable before shooting + return alignCommand.andThen(waitSeconds(0.5)).andThen(intakeCoral()); + } + + public static Command armToReefAvoidAlgae(int reefLevel) { + return pivot + .setPosition(() -> safePivotPosition) + .andThen(armToGoal(L4WristAngleAuto, safePivotPosition, reefArmPositions[reefLevel - 1][2])) + .andThen(pivot.setPosition(() -> reefArmPositions[reefLevel - 1][1])); + } + + /** placeReef but it doesn't drive. For pit testing */ + public static Command immobilePlaceReef(boolean isOnRight, int level) { + Command driveCommand; + driveCommand = waitSeconds(3); + Command alignCommand = new ParallelCommandGroup(driveCommand, armToReefSafely(level)).withTimeout(5.0); From 5bb8e5e742850700c5bfe4035fda9df5c838fc2b Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 15 Apr 2025 09:48:27 -0700 Subject: [PATCH 12/19] Semicolons (My IDE's lang server isn't working .-. ) Also spotless --- .../java/frc/robot/constants/ElevatorConstants.java | 4 ++-- .../java/frc/robot/constants/WristConstants.java | 4 ++-- src/main/java/frc/robot/util/CompoundCommands.java | 12 +++++++----- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index a8bc18cc..a3ddfbb6 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -93,8 +93,8 @@ public class ElevatorConstants { public static final double moveVoltage = 5.0; - public static final double MAX_SPEED = 3.6 - public static final double MAX_ACCELERATION = 4.0 + public static final double MAX_SPEED = 3.6; + public static final double MAX_ACCELERATION = 4.0; /* Device IDs */ public static final int motorID = 12; diff --git a/src/main/java/frc/robot/constants/WristConstants.java b/src/main/java/frc/robot/constants/WristConstants.java index 488a81d4..c1270ede 100644 --- a/src/main/java/frc/robot/constants/WristConstants.java +++ b/src/main/java/frc/robot/constants/WristConstants.java @@ -22,8 +22,8 @@ public class WristConstants { /** ID of the wrist sparkmax */ public static final int MOTOR_ID = 14; - public static final double MAX_SPEED = 0.5 - public static final double MAX_ACCELERATION = 1.0 + public static final double MAX_SPEED = 0.5; + public static final double MAX_ACCELERATION = 1.0; /** */ public static final boolean MOTOR_INVERTED = false; diff --git a/src/main/java/frc/robot/util/CompoundCommands.java b/src/main/java/frc/robot/util/CompoundCommands.java index 8e48d01d..968eab68 100644 --- a/src/main/java/frc/robot/util/CompoundCommands.java +++ b/src/main/java/frc/robot/util/CompoundCommands.java @@ -305,7 +305,8 @@ public static DeferredCommand deferDrive(Supplier command) { } /** - * Generates a {@link Command} to move the arm to the specified position, moving all mechanisms simultaneously. + * Generates a {@link Command} to move the arm to the specified position, moving all mechanisms + * simultaneously. * * @param wristSetPos Angle in degrees to set the wrist * @param pivotSetPos Angle in degrees to set the pivot @@ -314,9 +315,9 @@ public static DeferredCommand deferDrive(Supplier command) { */ public static Command armToGoal(double wristSetPos, double pivotSetPos, double elevatorSetPos) { return pivot - .setPosition(() -> pivotSetPos) - .alongWith(wrist.setGoalPosition(() -> wristSetPos)) - .alongWith(elevator.setPosition((() -> elevatorSetPos))) + .setPosition(() -> pivotSetPos) + .alongWith(wrist.setGoalPosition(() -> wristSetPos)) + .alongWith(elevator.setPosition((() -> elevatorSetPos))); } /** @@ -330,7 +331,8 @@ public static Command armToGoal(double wristSetPos, double pivotSetPos, double e * @param elevatorSetPos Height in meters to set the elevator * @return A {@link Command} to set the arm goal position to the specified position */ - public static Command armToGoalSafely(double wristSetPos, double pivotSetPos, double elevatorSetPos) { + public static Command armToGoalSafely( + double wristSetPos, double pivotSetPos, double elevatorSetPos) { if (elevatorSetPos > elevator.getPosition().getAsDouble()) { return elevator .setPosition(() -> elevatorSetPos) From 8bf84326e763ec908b7bcc1f92ae4865a75cd77f Mon Sep 17 00:00:00 2001 From: FlameFish Date: Thu, 17 Apr 2025 08:38:17 -0700 Subject: [PATCH 13/19] Adjust coral ejection logic --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/constants/IntakeConstants.java | 6 ++++- .../frc/robot/subsystems/intake/Intake.java | 27 ++++++++++++++++--- .../java/frc/robot/util/CompoundCommands.java | 2 +- 4 files changed, 31 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 62c128b1..249701f0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -160,7 +160,7 @@ private void configureBindings() { backupController.povDown().onTrue(zeroArm()); - backupController.leftBumper().whileTrue(intake.ejectCoralCommand()); + backupController.leftBumper().whileTrue(intake.backEjectCoralCommand()); backupController.rightBumper().whileTrue(intake.intakeCoralCommand()); diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 198ca31d..828eff73 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -8,7 +8,11 @@ public class IntakeConstants { public static final double INTAKE_CORAL_TOP_MOTOR_VOLTAGE = -3; public static final double INTAKE_CORAL_LOW_MOTOR_VOLTAGE = 3; - public static final double EJECT_CORAL_TOP_MOTOR_VOLTAGE = 4; + + public static final double BACKWARD_EJECT_CORAL_TOP_MOTOR_VOLTAGE = 3; + public static final double FOREWARD_EJECT_CORAL_TOP_MOTOR_VOLTAGE = -3; + public static final double BACKWARD_EJECT_CORAL_LOW_MOTOR_VOLTAGE = -3; + public static final double FOREWARD_EJECT_CORAL_LOW_MOTOR_VOLTAGE = 3; public static final double INTAKE_ALGAE_LOW_MOTOR_VOLTAGE = -4; public static final double EJECT_ALGAE_LOW_MOTOR_VOLTAGE = 3; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 95edbc42..0e4b37e9 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -67,14 +67,35 @@ public Command intakeCoralCommand() { } /** - * Runs the top intake motor at eject voltage specified in {@link IntakeConstants}. + * Runs the top intake motor at backwards eject voltage specified in {@link IntakeConstants}. + * + * @return Command that runs the intake until we no longer sense a coral, then continues for a + * small time. + */ + public Command backEjectCoralCommand() { + return runBothMotorsCommand( + () -> BACKWARD_EJECT_CORAL_TOP_MOTOR_VOLTAGE, + () -> BACKWARD_EJECT_CORAL_LOW_MOTOR_VOLTAGE) + .until(() -> !getHasGamePiece().getAsBoolean()) + .andThen(waitSeconds(EJECT_RELEASE_DELAY)) + .finallyDo( + () -> { + intake.setLowMotorVoltage(0); + intake.setTopMotorVoltage(0); + }); + } + + /** + * Runs the top intake motor at forward eject voltage specified in {@link IntakeConstants}. * * @return Command that runs the intake until we no longer sense a coral, then continues for a * small time. */ public Command ejectCoralCommand() { - return runTopMotorCommand(() -> EJECT_CORAL_TOP_MOTOR_VOLTAGE) - .until(getHasGamePiece()) + return runBothMotorsCommand( + () -> FOREWARD_EJECT_CORAL_TOP_MOTOR_VOLTAGE, + () -> FOREWARD_EJECT_CORAL_LOW_MOTOR_VOLTAGE) + .until(() -> !getHasGamePiece().getAsBoolean()) .andThen(waitSeconds(EJECT_RELEASE_DELAY)) .finallyDo( () -> { diff --git a/src/main/java/frc/robot/util/CompoundCommands.java b/src/main/java/frc/robot/util/CompoundCommands.java index 968eab68..ff61f744 100644 --- a/src/main/java/frc/robot/util/CompoundCommands.java +++ b/src/main/java/frc/robot/util/CompoundCommands.java @@ -112,7 +112,7 @@ public static Command placeReef(boolean isOnRight, int level) { new ParallelCommandGroup(driveCommand, armToReefAvoidAlgae(level)).withTimeout(5.0); // Small wait to ensure arm is stable before shooting - return alignCommand.andThen(waitSeconds(0.5)).andThen(intakeCoral()); + return alignCommand.andThen(waitSeconds(0.5)).andThen(ejectCoral()); } public static Command armToReefAvoidAlgae(int reefLevel) { From f4472536c8ab4e74136823dc594e85aa057a56c3 Mon Sep 17 00:00:00 2001 From: FlameFish Date: Mon, 21 Apr 2025 13:41:16 -0700 Subject: [PATCH 14/19] Use the modified eject command in place reef immobile --- src/main/java/frc/robot/util/CompoundCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/CompoundCommands.java b/src/main/java/frc/robot/util/CompoundCommands.java index ff61f744..3f6b57e2 100644 --- a/src/main/java/frc/robot/util/CompoundCommands.java +++ b/src/main/java/frc/robot/util/CompoundCommands.java @@ -131,7 +131,7 @@ public static Command immobilePlaceReef(boolean isOnRight, int level) { new ParallelCommandGroup(driveCommand, armToReefSafely(level)).withTimeout(5.0); // Small wait to ensure arm is stable before shooting - return alignCommand.andThen(waitSeconds(0.5)).andThen(intakeCoral()); + return alignCommand.andThen(waitSeconds(0.5)).andThen(ejectCoral()); } public static Command intakeSource(boolean isOnRight) { From d0ddd724b6f5193a5bdcaf3eab4a0dd4b82803f2 Mon Sep 17 00:00:00 2001 From: FlameFish Date: Wed, 23 Apr 2025 11:20:37 -0700 Subject: [PATCH 15/19] Change max speed and accel constants for unit documentation and consistency --- src/main/java/frc/robot/constants/ElevatorConstants.java | 3 +++ src/main/java/frc/robot/constants/PivotConstants.java | 6 ++++-- src/main/java/frc/robot/constants/WristConstants.java | 7 +++++-- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index a3ddfbb6..1159d1c1 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -93,7 +93,10 @@ public class ElevatorConstants { public static final double moveVoltage = 5.0; + /** Maximum speed of elevator in m/s */ public static final double MAX_SPEED = 3.6; + + /** Maximum acceleration of elevator in m/s² */ public static final double MAX_ACCELERATION = 4.0; /* Device IDs */ diff --git a/src/main/java/frc/robot/constants/PivotConstants.java b/src/main/java/frc/robot/constants/PivotConstants.java index b9efcc43..6b44fb34 100644 --- a/src/main/java/frc/robot/constants/PivotConstants.java +++ b/src/main/java/frc/robot/constants/PivotConstants.java @@ -46,9 +46,11 @@ public class PivotConstants { /** */ public static final double GEAR_RATIO = 378; - public static final double MAX_SPEED = 0.525; + /** Maximum speed of pivot in rot per sec */ + public static final double MAX_SPEED = Units.degreesToRotations(189); - public static final double MAX_ACCELERATION = 0.225; +/** Maximum acceleration of pivot in rot per sec² */ + public static final double MAX_ACCELERATION = Units.degreesToRotations(81); /** */ public static final ArmFeedforward FF_MODEL = diff --git a/src/main/java/frc/robot/constants/WristConstants.java b/src/main/java/frc/robot/constants/WristConstants.java index c1270ede..9334d533 100644 --- a/src/main/java/frc/robot/constants/WristConstants.java +++ b/src/main/java/frc/robot/constants/WristConstants.java @@ -22,8 +22,11 @@ public class WristConstants { /** ID of the wrist sparkmax */ public static final int MOTOR_ID = 14; - public static final double MAX_SPEED = 0.5; - public static final double MAX_ACCELERATION = 1.0; + /** The maximum speed of the wrist in rot per sec */ + public static final double MAX_SPEED = Units.degreesToRotations(180); + + /** The maximum acceleration of the wrist in rot per sec² */ + public static final double MAX_ACCELERATION = Units.degreesToRotations(360); /** */ public static final boolean MOTOR_INVERTED = false; From 721c48a3692e26737b3f574c3f18686e95b7ccea Mon Sep 17 00:00:00 2001 From: FlameFish Date: Wed, 23 Apr 2025 11:21:05 -0700 Subject: [PATCH 16/19] Spotless --- src/main/java/frc/robot/constants/PivotConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/PivotConstants.java b/src/main/java/frc/robot/constants/PivotConstants.java index 6b44fb34..71d44ff0 100644 --- a/src/main/java/frc/robot/constants/PivotConstants.java +++ b/src/main/java/frc/robot/constants/PivotConstants.java @@ -49,7 +49,7 @@ public class PivotConstants { /** Maximum speed of pivot in rot per sec */ public static final double MAX_SPEED = Units.degreesToRotations(189); -/** Maximum acceleration of pivot in rot per sec² */ + /** Maximum acceleration of pivot in rot per sec² */ public static final double MAX_ACCELERATION = Units.degreesToRotations(81); /** */ From 31ac7e698fb69a817c65306faecf558aa18e112c Mon Sep 17 00:00:00 2001 From: FlameFish Date: Wed, 23 Apr 2025 11:27:03 -0700 Subject: [PATCH 17/19] Remove unneeded delay in pit test auto --- src/main/deploy/pathplanner/autos/Pit Test.auto | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Pit Test.auto b/src/main/deploy/pathplanner/autos/Pit Test.auto index 42dd0274..d158e8a9 100644 --- a/src/main/deploy/pathplanner/autos/Pit Test.auto +++ b/src/main/deploy/pathplanner/autos/Pit Test.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, { "type": "named", "data": { From d211f140009379b9f8cca92e58e6eca6e2501c43 Mon Sep 17 00:00:00 2001 From: FlameFish Date: Thu, 24 Apr 2025 16:42:35 -0700 Subject: [PATCH 18/19] Adjust eject function as req by driver, documentation --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../java/frc/robot/subsystems/intake/Intake.java | 16 ++++++++++++++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 249701f0..f169cfae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -160,7 +160,7 @@ private void configureBindings() { backupController.povDown().onTrue(zeroArm()); - backupController.leftBumper().whileTrue(intake.backEjectCoralCommand()); + backupController.leftBumper().whileTrue(intake.runEjectCommand()); backupController.rightBumper().whileTrue(intake.intakeCoralCommand()); @@ -204,7 +204,7 @@ private void configureBindings() { .onFalse(CompoundCommands.armToStowSafely()); controller .rightTrigger(0.25) - .whileTrue(intake.intakeCoralCommand()) + .whileTrue(intake.runEjectCommand()) .onFalse( intake.stopCommand().alongWith(CompoundCommands.armToStowSafely()).andThen(zeroArm())); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 0e4b37e9..5fdf0a22 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -104,6 +104,22 @@ public Command ejectCoralCommand() { }); } + /** + * Runs both motors at forward eject voltage specified in {@link IntakeConstants}. + * + * @return Command that runs the intake until we terminate. + */ + public Command runEjectCommand() { + return runBothMotorsCommand( + () -> FOREWARD_EJECT_CORAL_TOP_MOTOR_VOLTAGE, + () -> FOREWARD_EJECT_CORAL_LOW_MOTOR_VOLTAGE) + .finallyDo( + () -> { + intake.setLowMotorVoltage(0); + intake.setTopMotorVoltage(0); + }); + } + /** * MUST BE TERMINATED EXTERNALLY * From 2dac39a3fa869299f97647a94a90fffa8e0d58fb Mon Sep 17 00:00:00 2001 From: FlameFish Date: Tue, 29 Apr 2025 16:20:39 -0700 Subject: [PATCH 19/19] Fix auto test --- src/main/deploy/pathplanner/autos/Pit Test.auto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/autos/Pit Test.auto b/src/main/deploy/pathplanner/autos/Pit Test.auto index d158e8a9..3c3d12ba 100644 --- a/src/main/deploy/pathplanner/autos/Pit Test.auto +++ b/src/main/deploy/pathplanner/autos/Pit Test.auto @@ -19,7 +19,7 @@ ] } }, - "resetOdom": true, + "resetOdom": false, "folder": null, "choreoAuto": true } \ No newline at end of file