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 54def77..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.115173057489333, - "y": 6.135246603413428 + "x": 2.287270519874242, + "y": 6.774371912194027 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, - "maxAcceleration": 2.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/330 Right to HP.path b/src/main/deploy/pathplanner/paths/330 Right to HP.path index 549fc93..e7fe252 100644 --- a/src/main/deploy/pathplanner/paths/330 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/330 Right to HP.path @@ -46,7 +46,7 @@ ], "globalConstraints": { "maxVelocity": 5.0, - "maxAcceleration": 2.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, 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 096023c..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" @@ -46,7 +46,7 @@ ], "globalConstraints": { "maxVelocity": 3.5, - "maxAcceleration": 1.5, + "maxAcceleration": 1.25, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP to K.path b/src/main/deploy/pathplanner/paths/HP to K.path index 92807d1..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": 1.7741803278688524, - "y": 6.530430327868851 + "x": 1.9266540815180775, + "y": 6.744320542331013 }, "isLocked": false, "linkedName": "HP Left Position" @@ -20,8 +20,8 @@ "y": 5.211782786885246 }, "prevControl": { - "x": 3.2246926229508195, - "y": 6.230737704918033 + "x": 2.975085616438356, + "y": 6.023416095890411 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.5, + "maxVelocity": 5.0, "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, diff --git a/src/main/deploy/pathplanner/paths/L Backup.path b/src/main/deploy/pathplanner/paths/L Backup.path index 2c7a178..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, @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, - "maxAcceleration": 2.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..4b70c88 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.756421232876712, + "y": 5.227054794520548 + }, + "prevControl": { + "x": 2.756421232876712, + "y": 5.227054794520548 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 1.75, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 400.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.18537788806707 + }, + "reversed": false, + "folder": "Left Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file 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/deploy/pathplanner/paths/Start to 30 Right.path b/src/main/deploy/pathplanner/paths/Start to 30 Right.path index a5b20af..f3d34aa 100644 --- a/src/main/deploy/pathplanner/paths/Start to 30 Right.path +++ b/src/main/deploy/pathplanner/paths/Start to 30 Right.path @@ -46,7 +46,7 @@ ], "globalConstraints": { "maxVelocity": 3.5, - "maxAcceleration": 1.5, + "maxAcceleration": 1.25, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6f291ef..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,14 +354,24 @@ public class RobotContainer { NamedCommands.registerCommand( "Shoot Algae", - shootAlgae() + shootAlgae().withTimeout(2) ); NamedCommands.registerCommand( "Processor Position", moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition) - .alongWith(manipulator.runManipulator(() -> 0.85, false)) - .until(() -> driver.a().getAsBoolean()) + .raceWith(manipulator.runManipulator(() -> 0.85, false)) + ); + + NamedCommands.registerCommand( + "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? ); } @@ -620,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/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index faf3b7c..b3db789 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -85,6 +85,6 @@ public class VisionConstants { {5.322, 2.511}//22 }; - public static final double latencyFudge = 0.03; + public static final double latencyFudge = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index c1d36a8..6b6ff78 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -111,8 +111,8 @@ public class Drivetrain extends SubsystemBase { m_rearRight.getPosition() }, new Pose2d(), - VecBuilder.fill(0.07, 0.7, Units.degreesToRadians(0.5)), - VecBuilder.fill(1, 1, Units.degreesToRadians(4000)) + VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)), + VecBuilder.fill(1, 1, Units.degreesToRadians(360)) ); pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints); @@ -177,17 +177,17 @@ public class Drivetrain extends SubsystemBase { gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees()); - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360))); if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){ if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360))); }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); }else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); } // if the detected tags match your alliances reef tags use their pose estimates @@ -204,17 +204,18 @@ public class Drivetrain extends SubsystemBase { Logger.recordOutput("orange dist", vision.getOrangeDist()); Logger.recordOutput("orange detected", vision.getOrangeTagDetected()); Logger.recordOutput("orange tag", vision.getOrangeTagDetected()); + Logger.recordOutput("orange FPS", vision.getOrangeFPS()); if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){ if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360))); }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); }else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); } if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){ @@ -230,6 +231,7 @@ public class Drivetrain extends SubsystemBase { Logger.recordOutput("black dist", vision.getBlackDist()); Logger.recordOutput("black detected", vision.getBlackTagDetected()); Logger.recordOutput("black tag", vision.getBlackTagDetected()); + Logger.recordOutput("black FPS", vision.getBlackFPS()); Logger.recordOutput("drive velocity", getVelocity()); Logger.recordOutput("closest tag", getClosestTag()); 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 *