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