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

fixing pid maintain position

Browse Source
This commit is contained in:
Tylr-J42 2025-02-28 10:37:14 -05:00
parent e0d0a121ba
commit c52a9ead0f
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -80,6 +80,8 @@ public class Elevator extends SubsystemBase {
maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0); maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
maintainPID.setTolerance(ElevatorConstants.kAllowedError);
feedForward = new ElevatorFeedforward( feedForward = new ElevatorFeedforward(
ElevatorConstants.kFeedForwardS, ElevatorConstants.kFeedForwardS,
ElevatorConstants.kFeedForwardG, ElevatorConstants.kFeedForwardG,
@ -145,27 +147,28 @@ public class Elevator extends SubsystemBase {
public Command maintainPosition() { public Command maintainPosition() {
return startRun(() -> { return startRun(() -> {
maintainPID.reset();
maintainPID.setSetpoint(pidControllerUp.getSetpoint());
}, },
() -> { () -> {
double maintainOutput = maintainPID.calculate(getEncoderPosition()); double maintainOutput = maintainPID.calculate(getEncoderPosition());
if(pidControllerUp.getSetpoint()>encoder.getPosition()) if(!maintainPID.atSetpoint())
elevatorMotor1.setVoltage( MathUtil.clamp( elevatorMotor1.setVoltage( MathUtil.clamp(
maintainOutput + feedForward.calculate(0), -1, 1) maintainOutput + feedForward.calculate(0), -2, 2)
); );
else{ else{
elevatorMotor1.setVoltage( elevatorMotor1.setVoltage(
MathUtil.clamp( feedForward.calculate(0)
maintainOutput + feedForward.calculate(0), -1, 1)
); );
} }
/*
elevatorMotor1.setVoltage( elevatorMotor1.setVoltage(
feedForward.calculate(0) feedForward.calculate(0)
); );
*/
}); });