From 0589463c4e2f68e30ef85255b0e92289215f613a Mon Sep 17 00:00:00 2001 From: Tylr-J42 <84476781+Tylr-J42@users.noreply.github.com> Date: Sun, 13 Apr 2025 04:32:46 -0400 Subject: [PATCH] pose, logger, and barge shot tweaks --- .../deploy/pathplanner/autos/1 L4 Center.auto | 6 ++ .../pathplanner/autos/1L4 + Algae Center.auto | 55 ++++++++++++++ .../deploy/pathplanner/paths/H Backup.path | 24 +++---- .../paths/{New Path.path => HG Algae.path} | 26 +++---- .../deploy/pathplanner/paths/HG to Barge.path | 71 +++++++++++++++++++ .../deploy/pathplanner/paths/HP to K.path | 4 +- .../deploy/pathplanner/paths/Start to H.path | 28 +++++--- src/main/deploy/pathplanner/settings.json | 3 +- src/main/java/frc/robot/RobotContainer.java | 18 ++++- .../constants/ManipulatorPivotConstants.java | 4 +- .../frc/robot/constants/VisionConstants.java | 2 +- .../java/frc/robot/subsystems/Drivetrain.java | 27 +++---- .../java/frc/robot/subsystems/Elevator.java | 51 ++----------- .../frc/robot/subsystems/Manipulator.java | 9 +++ .../robot/subsystems/ManipulatorPivot.java | 56 +++++++++++++++ .../java/frc/robot/subsystems/Vision.java | 4 +- 16 files changed, 289 insertions(+), 99 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto rename src/main/deploy/pathplanner/paths/{New Path.path => HG Algae.path} (66%) create mode 100644 src/main/deploy/pathplanner/paths/HG to Barge.path diff --git a/src/main/deploy/pathplanner/autos/1 L4 Center.auto b/src/main/deploy/pathplanner/autos/1 L4 Center.auto index 319353d..b9457fe 100644 --- a/src/main/deploy/pathplanner/autos/1 L4 Center.auto +++ b/src/main/deploy/pathplanner/autos/1 L4 Center.auto @@ -27,6 +27,12 @@ "data": { "pathName": "H Backup" } + }, + { + "type": "named", + "data": { + "name": "HP Pickup" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto b/src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto new file mode 100644 index 0000000..55e6cc7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start to H" + } + }, + { + "type": "named", + "data": { + "name": "Shoot Coral L4" + } + }, + { + "type": "path", + "data": { + "pathName": "H Backup" + } + }, + { + "type": "named", + "data": { + "name": "Pickup Algae L2" + } + }, + { + "type": "path", + "data": { + "pathName": "HG Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "HG to Barge" + } + }, + { + "type": "named", + "data": { + "name": "Shoot Algae" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H Backup.path b/src/main/deploy/pathplanner/paths/H Backup.path index 71b3072..5b3940a 100644 --- a/src/main/deploy/pathplanner/paths/H Backup.path +++ b/src/main/deploy/pathplanner/paths/H Backup.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 5.7, - "y": 4.3 + "x": 5.758260140458621, + "y": 4.193633481772718 }, "prevControl": null, "nextControl": { - "x": 6.053790983606557, - "y": 4.312704918032787 + "x": 6.112051124065178, + "y": 4.206338399805505 }, "isLocked": false, "linkedName": "H" }, { "anchor": { - "x": 6.389446721311475, - "y": 4.3 + "x": 6.5, + "y": 4.021985060730393 }, "prevControl": { - "x": 6.1394487099079695, - "y": 4.300997143065429 + "x": 6.250001988596495, + "y": 4.022982203795822 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Behind HG Face" } ], "rotationTargets": [], @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.5, - "maxAcceleration": 1.75, + "maxAcceleration": 0.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, @@ -45,10 +45,10 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Center", "idealStartingState": { "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/HG Algae.path similarity index 66% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/HG Algae.path index 61690b6..ac1a32a 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/HG Algae.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.786987704919465, - "y": 4.348668032784689 + "x": 6.5, + "y": 4.021985060730393 }, "prevControl": null, "nextControl": { - "x": 9.786987704919456, - "y": 4.348668032784689 + "x": 6.224227344787433, + "y": 4.009974315068493 }, "isLocked": false, - "linkedName": null + "linkedName": "Behind HG Face" }, { "anchor": { - "x": 11.256454918034219, - "y": 4.348668032784689 + "x": 5.872671261177789, + "y": 4.021985060730393 }, "prevControl": { - "x": 7.543546317006037, - "y": 6.492316813064786 + "x": 6.125859753477119, + "y": 4.004837296105254 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "HG Algae" } ], "rotationTargets": [], @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Center", "idealStartingState": { "velocity": 0, - "rotation": 124.695153531234 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HG to Barge.path b/src/main/deploy/pathplanner/paths/HG to Barge.path new file mode 100644 index 0000000..fff5058 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HG to Barge.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.872671261177789, + "y": 4.021985060730393 + }, + "prevControl": null, + "nextControl": { + "x": 7.829105562465259, + "y": 3.8676166530198106 + }, + "isLocked": false, + "linkedName": "HG Algae" + }, + { + "anchor": { + "x": 7.032020547945206, + "y": 4.761258561643835 + }, + "prevControl": { + "x": 7.262336691900294, + "y": 4.359747737103425 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Pre-Barge" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17234468937875755, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Processor Position", + "waypointRelativePos": 0.188095238095238, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Processor Position" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 1.75, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 300.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Center", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP to K.path b/src/main/deploy/pathplanner/paths/HP to K.path index 9d72bd4..92807d1 100644 --- a/src/main/deploy/pathplanner/paths/HP to K.path +++ b/src/main/deploy/pathplanner/paths/HP to K.path @@ -46,7 +46,7 @@ ], "globalConstraints": { "maxVelocity": 3.5, - "maxAcceleration": 1.75, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, @@ -62,5 +62,5 @@ "velocity": 0, "rotation": -53.97262661489646 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start to H.path b/src/main/deploy/pathplanner/paths/Start to H.path index 516ea05..74c29b0 100644 --- a/src/main/deploy/pathplanner/paths/Start to H.path +++ b/src/main/deploy/pathplanner/paths/Start to H.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.7, - "y": 4.3 + "x": 5.758260140458621, + "y": 4.193633481772718 }, "prevControl": { - "x": 6.347336065573771, - "y": 4.2640368852459005 + "x": 6.405596206032391, + "y": 4.157670367018619 }, "nextControl": null, "isLocked": false, @@ -31,10 +31,22 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Lift L4", + "waypointRelativePos": 0.10238095238095252, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Lift L4" + } + } + } + ], "globalConstraints": { "maxVelocity": 3.5, - "maxAcceleration": 1.75, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, @@ -45,10 +57,10 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Center", "idealStartingState": { "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index a1e375e..c23abb7 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,7 +3,8 @@ "robotLength": 0.8763, "holonomicMode": true, "pathFolders": [ - "Left Paths" + "Left Paths", + "Center" ], "autoFolders": [], "defaultMaxVel": 3.5, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6c04ee9..6f291ef 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -351,6 +351,18 @@ public class RobotContainer { ManipulatorPivotConstants.kStartingPosition ) ); + + NamedCommands.registerCommand( + "Shoot Algae", + shootAlgae() + ); + + NamedCommands.registerCommand( + "Processor Position", + moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition) + .alongWith(manipulator.runManipulator(() -> 0.85, false)) + .until(() -> driver.a().getAsBoolean()) + ); } //creates tabs and transforms them on the shuffleboard @@ -607,10 +619,10 @@ public class RobotContainer { } private Command shootAlgae(){ - return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition) - .andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition) + 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.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition) + elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition) .raceWith(elevator.maintainPosition())); } diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index da2bad4..9b66b8a 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -23,7 +23,9 @@ public class ManipulatorPivotConstants { public static final double kPositionalP = 4.5; public static final double kPositionalI = 0; public static final double kPositionalD = 0; - public static final double kPositionalTolerance = Units.degreesToRadians(1.5); + public static final double kPositionalTolerance = Units.degreesToRadians(3); + + public static final double kAlgaeP = 7; public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19 public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41 diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index b3db789..faf3b7c 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.0; + public static final double latencyFudge = 0.03; } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index e7d8348..c1d36a8 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.1, 0.1, Units.degreesToRadians(5)), - VecBuilder.fill(1, 1, Units.degreesToRadians(360)) + VecBuilder.fill(0.07, 0.7, Units.degreesToRadians(0.5)), + VecBuilder.fill(1, 1, Units.degreesToRadians(4000)) ); 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(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(4000))); if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){ if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000))); }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000))); }else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000))); }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000))); } // if the detected tags match your alliances reef tags use their pose estimates @@ -203,17 +203,18 @@ public class Drivetrain extends SubsystemBase { Logger.recordOutput("orange pose", new Pose3d(orangePose2d)); Logger.recordOutput("orange dist", vision.getOrangeDist()); Logger.recordOutput("orange detected", vision.getOrangeTagDetected()); + Logger.recordOutput("orange tag", vision.getOrangeTagDetected()); if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){ if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000))); }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000))); }else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000))); }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000))); } if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){ @@ -227,9 +228,11 @@ public class Drivetrain extends SubsystemBase { } Logger.recordOutput("black pose", new Pose3d(blackPose2d)); Logger.recordOutput("black dist", vision.getBlackDist()); - Logger.recordOutput("orange detected", vision.getBlackTagDetected()); + Logger.recordOutput("black detected", vision.getBlackTagDetected()); + Logger.recordOutput("black tag", vision.getBlackTagDetected()); Logger.recordOutput("drive velocity", getVelocity()); + Logger.recordOutput("closest tag", getClosestTag()); Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition())); if(musicTimer.get()>10){ diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 6700502..60a201a 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -2,6 +2,8 @@ package frc.robot.subsystems; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.PersistMode; @@ -94,6 +96,10 @@ public class Elevator extends SubsystemBase { if (!getBottomLimitSwitch()) { encoder.setPosition(0); } + + Logger.recordOutput("elevator position", getEncoderPosition()); + Logger.recordOutput("elevator up setpoint", pidControllerUp.getSetpoint()); + Logger.recordOutput("elevator down setpoint", pidControllerDown.getSetpoint()); } /** @@ -245,50 +251,7 @@ public class Elevator extends SubsystemBase { }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint()); } - - - /* - elevatorMotor1.setVoltage( - pidController.calculate( - encoder.getPosition(), - clampedSetpoint - ) + feedForward.calculate(0) - ); -*/ - /* - if (!pidController.atSetpoint()) { - elevatorMotor1.setVoltage( - pidController.calculate( - encoder.getPosition(), - clampedSetpoint - ) + feedForward.calculate(0) - ); - } else { - elevatorMotor1.setVoltage( - feedForward.calculate(0) - ); - } - - });*/ - } - - /* - if(encoder.getPosition() >= setpoint.getAsDouble()){ - elevatorMotor1.setVoltage( - pidControllerUp.calculate( - encoder.getPosition(), - clampedSetpoint - ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity) - ); - }else if(encoder.getPosition() <= setpoint.getAsDouble()){ - elevatorMotor1.setVoltage( - pidControllerDown.calculate( - encoder.getPosition(), - clampedSetpoint - ) + feedForward.calculate(pidControllerDown.getSetpoint().velocity) - ); - } - */ + } /** * Returns the current encoder position diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java index d223707..499be35 100644 --- a/src/main/java/frc/robot/subsystems/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/Manipulator.java @@ -2,6 +2,8 @@ package frc.robot.subsystems; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -46,6 +48,13 @@ public class Manipulator extends SubsystemBase { coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID); } + @Override + public void periodic() { + super.periodic(); + + Logger.recordOutput("coral beam break", getCoralBeamBreak()); + } + /** * The default command for the manipulator that either stops the manipulator or slowly * runs the manipulator to retain the algae diff --git a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java index 042ce89..baaee53 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java @@ -4,6 +4,8 @@ import com.revrobotics.spark.SparkMax; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -25,6 +27,8 @@ public class ManipulatorPivot extends SubsystemBase { private PIDController pidController; + private PIDController algaePIDController; + public ManipulatorPivot() { pivotMotor = new SparkMax( ManipulatorPivotConstants.kPivotMotorID, @@ -44,10 +48,22 @@ public class ManipulatorPivot extends SubsystemBase { ManipulatorPivotConstants.kPositionalI, ManipulatorPivotConstants.kPositionalD ); + pidController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance); + pidController.setSetpoint(0); pidController.enableContinuousInput(0, 280); + algaePIDController = new PIDController( + ManipulatorPivotConstants.kAlgaeP, + 0, + 0); + algaePIDController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance); + + algaePIDController.setSetpoint(0); + + algaePIDController.enableContinuousInput(0, 280); + feedForward = new ArmFeedforward( ManipulatorPivotConstants.kFeedForwardS, ManipulatorPivotConstants.kFeedForwardG, @@ -55,6 +71,14 @@ public class ManipulatorPivot extends SubsystemBase { ); } + @Override + public void periodic() { + super.periodic(); + + Logger.recordOutput("manipulator position", getEncoderPosition()); + Logger.recordOutput("manipulator setpoint", pidController.getSetpoint()); + } + /** * Returns whether or not the motion is safe relative to the encoder's current position * and the arm safe stow position @@ -125,6 +149,38 @@ public class ManipulatorPivot extends SubsystemBase { }).until(() -> pidController.atSetpoint()); } + public Command goToSetpointAlgae(DoubleSupplier setpoint) { + return startRun(() -> { + + algaePIDController.setSetpoint(setpoint.getAsDouble()); + algaePIDController.reset(); + pidController.setSetpoint(setpoint.getAsDouble()); + pidController.reset(); + }, + () -> { + /* + if (!pidController.atSetpoint()) { + pivotMotor.setVoltage( + pidController.calculate( + encoder.getPosition(), + setpoint.getAsDouble() + ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0) + ); + } else { + pivotMotor.setVoltage( + -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0) + ); + } + */ + pivotMotor.setVoltage( + algaePIDController.calculate( + encoder.getPosition(), + setpoint.getAsDouble() + ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0) + ); + }).until(() -> algaePIDController.atSetpoint()); + } + public Command maintainPosition() { return startRun(() -> { diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 6e33690..c5a8871 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -177,11 +177,11 @@ public class Vision{ } public boolean isBlackConnected(){ - return Timer.getFPGATimestamp()-black_tx.getLastChange() > 2.0; + return Timer.getFPGATimestamp()-blackFramerate.getLastChange() > 3.0; } public boolean isOrangeConnected(){ - return Timer.getFPGATimestamp()-orange_tx.getLastChange() > 2.0; + return Timer.getFPGATimestamp()-orangeFramerate.getLastChange() > 3.0; } }