diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ac14107..606efd5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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,8 +394,12 @@ 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) - .andThen(manipulatorPivot.goToSetpoint(() -> armPosition)); + /*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)); } @SuppressWarnings("unused") diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 2de584f..635816d 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -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()); /*