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

testing elevator

Browse Source
This commit is contained in:
Team 2648 2025-02-22 10:15:10 -05:00
parent a145c290fd
commit 44a036f420
2 changed files with 30 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -246,12 +246,22 @@ public class RobotContainer {
sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
.withWidget(BuiltInWidgets.kGraph);
sensorTab.addDouble("Elevator setpoint", elevator::getPIDUpSetpoint)
sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
.withSize(1, 1)
.withPosition(5, 0)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("Elevator error", elevator::getPIDUpError)
sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
.withSize(1, 1)
.withPosition(5, 1)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
.withSize(1, 1)
.withPosition(5, 0)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
.withSize(1, 1)
.withPosition(5, 1)
.withWidget(BuiltInWidgets.kTextView);
@ -384,7 +394,11 @@ public class RobotContainer {
* @return Moves the elevator and arm to the setpoints
*/
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
elevator.maintainPosition());*/
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
.andThen(elevator.goToSetpoint(() -> elevatorPosition))
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
}

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

@ -145,6 +145,7 @@ public class Elevator extends SubsystemBase {
},
() -> {
/*
if (!pidControllerUp.atSetpoint()) {
if(encoder.getPosition()>pidControllerUp.getSetpoint()){
elevatorMotor1.setVoltage(
@ -163,7 +164,11 @@ public class Elevator extends SubsystemBase {
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
}
}*/
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
});
}
@ -185,21 +190,20 @@ public class Elevator extends SubsystemBase {
pidControllerDown.reset();
},
() -> {
double upOutput = pidControllerUp.calculate(getEncoderPosition());
double downOutput = pidControllerDown.calculate(getEncoderPosition());
if(setpoint.getAsDouble()>encoder.getPosition())
elevatorMotor1.setVoltage(
pidControllerUp.calculate(
encoder.getPosition()
) + feedForward.calculate(0)
upOutput + feedForward.calculate(0)
);
else{
elevatorMotor1.setVoltage(
pidControllerDown.calculate(
encoder.getPosition()
) + feedForward.calculate(0)
downOutput + feedForward.calculate(0)
);
}
}).until(() -> pidControllerUp.atSetpoint());
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
/*