Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

pid maintain position for elevator

Browse Source
This commit is contained in:
Tylr-J42 2025-02-28 07:46:43 -05:00
parent d61314fc01
commit 92206fa252
7 changed files with 75 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/deploy/pathplanner/autos/One Coral Left.auto
View File

@ -15,6 +15,12 @@
"data": {
"name": "Shoot Coral L4"
}
},
{
"type": "path",
"data": {
"pathName": "J Backup"
}
}
]
}

54
src/main/deploy/pathplanner/paths/J Backup.path Normal file
View File

@ -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
}

2
src/main/deploy/pathplanner/paths/Start to 30 Right.path
View File

@ -25,7 +25,7 @@
},
"nextControl": null,
"isLocked": false,
"linkedName": null
"linkedName": "J"
}
],
"rotationTargets": [],

2
src/main/java/frc/robot/RobotContainer.java
View File

@ -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));

2
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@ -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;

2
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -161,7 +161,7 @@ public class Drivetrain extends SubsystemBase {
}
*/
if(musicTimer.get()>4){
if(musicTimer.get()>20){
if (m_orchestra.isPlaying()) {
m_orchestra.stop();
}

20
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -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)