From 92206fa25299167e19b0f675c709142c33d0e41e Mon Sep 17 00:00:00 2001 From: Tylr-J42 Date: Fri, 28 Feb 2025 07:46:43 -0500 Subject: [PATCH] pid maintain position for elevator --- .../pathplanner/autos/One Coral Left.auto | 6 +++ .../deploy/pathplanner/paths/J Backup.path | 54 +++++++++++++++++++ .../pathplanner/paths/Start to 30 Right.path | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 + .../robot/constants/ElevatorConstants.java | 2 + .../java/frc/robot/subsystems/Drivetrain.java | 2 +- .../java/frc/robot/subsystems/Elevator.java | 20 ++++--- 7 files changed, 75 insertions(+), 13 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/J Backup.path diff --git a/src/main/deploy/pathplanner/autos/One Coral Left.auto b/src/main/deploy/pathplanner/autos/One Coral Left.auto index e537c2b..d1e9060 100644 --- a/src/main/deploy/pathplanner/autos/One Coral Left.auto +++ b/src/main/deploy/pathplanner/autos/One Coral Left.auto @@ -15,6 +15,12 @@ "data": { "name": "Shoot Coral L4" } + }, + { + "type": "path", + "data": { + "pathName": "J Backup" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/J Backup.path b/src/main/deploy/pathplanner/paths/J Backup.path new file mode 100644 index 0000000..58e6778 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/J Backup.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.988527397260274, + "y": 5.227054794520548 + }, + "prevControl": null, + "nextControl": { + "x": 5.1321837955756875, + "y": 5.49468698033176 + }, + "isLocked": false, + "linkedName": "J" + }, + { + "anchor": { + "x": 5.442044107776481, + "y": 6.005045141603656 + }, + "prevControl": { + "x": 5.268886874487802, + "y": 5.749866060967707 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 400.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -118.30075576600632 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -121.60750224624898 + }, + "useDefaultConstraints": true +} \ 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 3fdb040..e2e5358 100644 --- a/src/main/deploy/pathplanner/paths/Start to 30 Right.path +++ b/src/main/deploy/pathplanner/paths/Start to 30 Right.path @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "J" } ], "rotationTargets": [], diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9ff1e10..103ad2d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -228,6 +228,8 @@ public class RobotContainer { private void configureNamedCommands() { new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); + new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); + NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand()); NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2)); NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35)); diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 2758076..5960e9e 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -29,6 +29,8 @@ public class ElevatorConstants { public static final double kDownControllerP = 5.6;//7; // public static final double kDownControllerI = 0; public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35 + + public static final double kMaintainP = 3; public static final double kAllowedError = 1; diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 28c60d0..b6527b2 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -161,7 +161,7 @@ public class Drivetrain extends SubsystemBase { } */ - if(musicTimer.get()>4){ + if(musicTimer.get()>20){ if (m_orchestra.isPlaying()) { m_orchestra.stop(); } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index aa89493..1732f27 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -27,6 +27,8 @@ public class Elevator extends SubsystemBase { private PIDController pidControllerUp; private PIDController pidControllerDown; + private PIDController maintainPID; + private ElevatorFeedforward feedForward; public Elevator() { @@ -76,7 +78,7 @@ public class Elevator extends SubsystemBase { pidControllerUp.setTolerance(ElevatorConstants.kAllowedError); - + maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0); feedForward = new ElevatorFeedforward( ElevatorConstants.kFeedForwardS, @@ -143,27 +145,23 @@ public class Elevator extends SubsystemBase { public Command maintainPosition() { return startRun(() -> { -/* - pidControllerUp.reset(); - pidControllerDown.reset(); - */ + }, () -> { -/* - double upOutput = pidControllerUp.calculate(getEncoderPosition()); - double downOutput = pidControllerDown.calculate(getEncoderPosition()); + + double maintainOutput = maintainPID.calculate(getEncoderPosition()); if(pidControllerUp.getSetpoint()>encoder.getPosition()) elevatorMotor1.setVoltage( MathUtil.clamp( - upOutput + feedForward.calculate(0), -1, 1) + maintainOutput + feedForward.calculate(0), -1, 1) ); else{ elevatorMotor1.setVoltage( MathUtil.clamp( - downOutput + feedForward.calculate(0), -1, 1) + maintainOutput + feedForward.calculate(0), -1, 1) ); } - */ + elevatorMotor1.setVoltage( feedForward.calculate(0)