diff --git a/src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto b/src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto index 55e6cc7..4ea1cdb 100644 --- a/src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto +++ b/src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto @@ -45,6 +45,25 @@ "data": { "name": "Shoot Algae" } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "HP Pickup" + } + }, + { + "type": "path", + "data": { + "pathName": "Post-Barge Backup" + } + } + ] + } } ] } diff --git a/src/main/deploy/pathplanner/paths/30 Right to HP.path b/src/main/deploy/pathplanner/paths/30 Right to HP.path index 60710c8..e277f5a 100644 --- a/src/main/deploy/pathplanner/paths/30 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/30 Right to HP.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.2587090163934425, - "y": 7.08186475409836 + "x": 1.1268442622950818, + "y": 7.201741803278688 }, "prevControl": { - "x": 2.4191352739726026, - "y": 6.654494863013699 + "x": 2.287270519874242, + "y": 6.774371912194027 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP to 330 Right.path b/src/main/deploy/pathplanner/paths/HP to 330 Right.path index d9f7713..6428ad8 100644 --- a/src/main/deploy/pathplanner/paths/HP to 330 Right.path +++ b/src/main/deploy/pathplanner/paths/HP to 330 Right.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.2587090163934425, - "y": 7.08186475409836 + "x": 1.1268442622950818, + "y": 7.201741803278688 }, "prevControl": null, "nextControl": { - "x": 2.550917920503032, - "y": 6.856479480125757 + "x": 2.0019467213114757, + "y": 6.434528688524591 }, "isLocked": false, "linkedName": "HP Left Position" diff --git a/src/main/deploy/pathplanner/paths/HP to K.path b/src/main/deploy/pathplanner/paths/HP to K.path index d8c1147..622acc1 100644 --- a/src/main/deploy/pathplanner/paths/HP to K.path +++ b/src/main/deploy/pathplanner/paths/HP to K.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.2587090163934425, - "y": 7.08186475409836 + "x": 1.1268442622950818, + "y": 7.201741803278688 }, "prevControl": null, "nextControl": { - "x": 2.058518835616438, - "y": 6.624443493150685 + "x": 1.9266540815180775, + "y": 6.744320542331013 }, "isLocked": false, "linkedName": "HP Left Position" diff --git a/src/main/deploy/pathplanner/paths/L Backup.path b/src/main/deploy/pathplanner/paths/L Backup.path index 50ea2f9..3cdebd2 100644 --- a/src/main/deploy/pathplanner/paths/L Backup.path +++ b/src/main/deploy/pathplanner/paths/L Backup.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.2587090163934425, - "y": 7.08186475409836 + "x": 1.1268442622950818, + "y": 7.201741803278688 }, "prevControl": { - "x": 1.4625000000000001, - "y": 6.8061475409836065 + "x": 1.3306352459016395, + "y": 6.926024590163935 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Post-Barge Backup.path b/src/main/deploy/pathplanner/paths/Post-Barge Backup.path new file mode 100644 index 0000000..ba1979b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Post-Barge Backup.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.032020547945206, + "y": 4.761258561643835 + }, + "prevControl": null, + "nextControl": { + "x": 6.71311475409836, + "y": 4.9480532786885245 + }, + "isLocked": false, + "linkedName": "Pre-Barge" + }, + { + "anchor": { + "x": 5.933913934426228, + "y": 5.247745901639345 + }, + "prevControl": { + "x": 6.641188524590164, + "y": 5.043954918032786 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 400.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Center", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 71cac96..adb4b01 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -339,7 +339,7 @@ public class RobotContainer { ), manipulatorPivot.maintainPosition() .withTimeout( - 0.01 + 0.1 ) ) ); @@ -354,7 +354,7 @@ public class RobotContainer { NamedCommands.registerCommand( "Shoot Algae", - shootAlgae() + shootAlgae().withTimeout(2) ); NamedCommands.registerCommand( @@ -367,6 +367,11 @@ public class RobotContainer { "Pickup Algae L2", moveWithAlgae(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition) .raceWith(manipulator.runManipulator(() -> 0.85, false)) + .andThen( + elevator.maintainPosition() + .alongWith(manipulatorPivot.maintainPosition())).withTimeout(0.1) + + //Dont you need a holdPosition call? ); } @@ -625,9 +630,9 @@ public class RobotContainer { private Command shootAlgae(){ return manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition) - .andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition) - .raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>43/* 44*/).andThen(manipulator.runManipulator(() -> -1, false), - elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition) + .andThen(elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition) + .raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>36/* 44*/).andThen(manipulator.runManipulator(() -> -1, false), + elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition) .raceWith(elevator.maintainPosition())); } diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 8fa220b..d66e731 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -56,6 +56,8 @@ public class ElevatorConstants { public static final double kVoltageLimit = 7; + public static final double kVoltageLimitAlgae = 9; + // 1, 7, 10 are the defaults for these, change as necessary public static final double kSysIDRampRate = .25; public static final double kSysIDStepVolts = 3; diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 60a201a..e7d8948 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -253,6 +253,64 @@ public class Elevator extends SubsystemBase { } } + public Command goToSetpointAlgae(DoubleSupplier setpoint) { + + if (setpoint.getAsDouble() == 0) { + return startRun(() -> { + + pidControllerUp.reset(); + pidControllerDown.reset(); + pidControllerUp.setSetpoint(setpoint.getAsDouble()); + pidControllerDown.setSetpoint(setpoint.getAsDouble()); + + }, + () -> { + double upOutput = pidControllerUp.calculate(getEncoderPosition()); + double downOutput = pidControllerDown.calculate(getEncoderPosition()); + + if(setpoint.getAsDouble()>encoder.getPosition()) + elevatorMotor1.setVoltage( MathUtil.clamp( + upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae) + ); + else{ + elevatorMotor1.setVoltage( + MathUtil.clamp( + downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae) + ); + } + + }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint()) + .andThen(runManualElevator(() -> -.5) + .until(() -> encoder.getPosition() == 0)); + + } else { + return startRun(() -> { + + pidControllerUp.reset(); + pidControllerDown.reset(); + pidControllerUp.setSetpoint(setpoint.getAsDouble()); + pidControllerDown.setSetpoint(setpoint.getAsDouble()); + + }, + () -> { + double upOutput = pidControllerUp.calculate(getEncoderPosition()); + double downOutput = pidControllerDown.calculate(getEncoderPosition()); + + if(setpoint.getAsDouble()>encoder.getPosition()) + elevatorMotor1.setVoltage( MathUtil.clamp( + upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae) + ); + else{ + elevatorMotor1.setVoltage( + MathUtil.clamp( + downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae) + ); + } + + }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint()); + } + } + /** * Returns the current encoder position *