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

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

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

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

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