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..3c3d12ba --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Pit Test.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "TestPlaceReefImmobile" + } + }, + { + "type": "named", + "data": { + "name": "ArmToStow" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b7ba2f74..59ab4ecc 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 @@ -93,8 +87,6 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - - m_robotContainer.zeroArm().schedule(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6096f632..f169cfae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -160,10 +160,16 @@ private void configureBindings() { backupController.povDown().onTrue(zeroArm()); - backupController.leftBumper().whileTrue(intake.ejectCoralCommand()); + backupController.leftBumper().whileTrue(intake.runEjectCommand()); backupController.rightBumper().whileTrue(intake.intakeCoralCommand()); + backupController.start().whileTrue(CompoundCommands.armToNet()); + + backupController.x().whileTrue(intake.intakeAlgaeCommand()); + + backupController.y().whileTrue(intake.ejectAlgaeCommand()); + backupController .povUp() .onTrue( @@ -198,8 +204,9 @@ private void configureBindings() { .onFalse(CompoundCommands.armToStowSafely()); controller .rightTrigger(0.25) - .whileTrue(intake.ejectCoralCommand()) - .onFalse(intake.stopCommand().alongWith(CompoundCommands.armToStow())); + .whileTrue(intake.runEjectCommand()) + .onFalse( + intake.stopCommand().alongWith(CompoundCommands.armToStowSafely()).andThen(zeroArm())); controller .rightStick() diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index fcf38af0..1159d1c1 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -93,6 +93,12 @@ 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 */ public static final int motorID = 12; 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/constants/PivotConstants.java b/src/main/java/frc/robot/constants/PivotConstants.java index e13821ee..71d44ff0 100644 --- a/src/main/java/frc/robot/constants/PivotConstants.java +++ b/src/main/java/frc/robot/constants/PivotConstants.java @@ -46,6 +46,12 @@ public class PivotConstants { /** */ public static final double GEAR_RATIO = 378; + /** 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² */ + public static final double MAX_ACCELERATION = Units.degreesToRotations(81); + /** */ 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/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; } diff --git a/src/main/java/frc/robot/constants/WristConstants.java b/src/main/java/frc/robot/constants/WristConstants.java index 1d964211..9334d533 100644 --- a/src/main/java/frc/robot/constants/WristConstants.java +++ b/src/main/java/frc/robot/constants/WristConstants.java @@ -22,6 +22,12 @@ public class WristConstants { /** ID of the wrist sparkmax */ public static final int MOTOR_ID = 14; + /** 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; @@ -80,6 +86,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/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/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 95edbc42..5fdf0a22 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( () -> { @@ -83,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 * 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/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)); } } 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; diff --git a/src/main/java/frc/robot/util/CompoundCommands.java b/src/main/java/frc/robot/util/CompoundCommands.java index 58d9df62..3f6b57e2 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(ejectCoral()); + } + + 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); @@ -138,6 +160,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( @@ -278,6 +304,22 @@ 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, @@ -289,8 +331,8 @@ 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)