diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a84846f..4ec082b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -103,18 +103,14 @@ public class RobotContainer { () -> true ) ); - /* + elevator.setDefaultCommand( - elevator.goToSetpoint( - () -> 0 - ) + elevator.maintainPosition() ); - */ + manipulatorPivot.setDefaultCommand( - manipulatorPivot.runManualPivot( - () -> operator.getRightY() * 0.5 - ) + manipulatorPivot.maintainPosition() ); @@ -125,7 +121,7 @@ public class RobotContainer { ); //Driver inputs - /* + driver.start().whileTrue( drivetrain.setXCommand() ); @@ -147,46 +143,31 @@ public class RobotContainer { driver.povLeft().whileTrue(climberRollers.runRoller(0.5)); driver.povRight().whileTrue(climberRollers.runRoller(-0.5)); - */ - - operator.povUp().whileTrue( - manipulatorPivot.goToSetpoint(() -> 0) - ); - - operator.povDown().whileTrue( - manipulatorPivot.goToSetpoint(() -> (Units.degreesToRadians(110)) - )); - - /* - operator.a().whileTrue(elevator.runManualElevator(() -> 0.2)); - - operator.b().whileTrue(elevator.runManualElevator(() -> -0.2)); - //Operator inputs operator.povUp().onTrue( - moveManipulator( + safeMoveManipulator( ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position ) ); operator.povRight().onTrue( - moveManipulator( + safeMoveManipulator( ElevatorConstants.kL3Position, ManipulatorPivotConstants.kL3Position ) ); operator.povLeft().onTrue( - moveManipulator( + safeMoveManipulator( ElevatorConstants.kL2Position, ManipulatorPivotConstants.kL2Position ) ); operator.povDown().onTrue( - moveManipulator( + safeMoveManipulator( ElevatorConstants.kL1Position, ManipulatorPivotConstants.kL1Position ) @@ -203,7 +184,11 @@ public class RobotContainer { operator.b().onTrue( algaeIntakeRoutine(false) ); - */ + + operator.y().whileTrue( + elevator.goToSetpoint(() -> ElevatorConstants.kL2Position) + ); + } diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 792ba1a..c7bd9f7 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -49,7 +49,7 @@ public class ElevatorConstants { public static final double kL3AlgaePosition = 39.0; public static final double kProcessorPosition = 4.0; /**The position of the top of the elevator brace */ - public static final double kBracePosition = 5.5; + public static final double kBracePosition = 0; public static final double kMaxHeight = 47.5; //actual is 53 // 1, 7, 10 are the defaults for these, change as necessary diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index e289db3..8e3a96b 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -20,7 +20,7 @@ public class ManipulatorPivotConstants { public static final double kPivotMaxVelocity = 2 * Math.PI; - public static final double kPositionalP = 2.5; + public static final double kPositionalP = 4; public static final double kPositionalI = 0; public static final double kPositionalD = 0; public static final double kPositionalTolerance = Units.degreesToRadians(3.0); @@ -38,7 +38,7 @@ public class ManipulatorPivotConstants { public static final double kCoralIntakePosition = Units.degreesToRadians(175.0); public static final double kL1Position = Units.degreesToRadians(0.0); - public static final double kL2Position = Units.degreesToRadians(25.0); + public static final double kL2Position = Units.degreesToRadians(60.0); public static final double kL3Position = Units.degreesToRadians(60.0); public static final double kL4Position = Units.degreesToRadians(45.0); public static final double kL2AlgaePosition = Units.degreesToRadians(175.0); @@ -46,7 +46,7 @@ public class ManipulatorPivotConstants { public static final double kProcesserPosition = Units.degreesToRadians(175.0); public static final double kNetPosition = Units.degreesToRadians(175.0); /**The closest position to the elevator brace without hitting it */ - public static final double kPivotSafeStowPosition = Units.degreesToRadians(60.0); + public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0); /**The forward rotation limit of the pivot */ public static final double kRotationLimit = Units.degreesToRadians(175.0); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 0094d89..45bbe98 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -62,6 +62,7 @@ public class Elevator extends SubsystemBase { ElevatorConstants.kDownControllerI, ElevatorConstants.kDownControllerD ); + pidController.setSetpoint(0); pidController.setTolerance(ElevatorConstants.kAllowedError); @@ -128,9 +129,56 @@ public class Elevator extends SubsystemBase { * @return Sets motor voltage based on feed forward calculation. */ public Command maintainPosition() { - return run(() -> { - elevatorMotor1.setVoltage(feedForward.calculate(0)); + + return startRun(() -> { + + //pidController.reset(); + // pidController.setSetpoint(encoder.getPosition()); + }, + () -> { + /*if (!pidController.atSetpoint()) { + elevatorMotor1.setVoltage( + pidController.calculate( + encoder.getPosition() + ) + feedForward.calculate(0) + ); + } else { + elevatorMotor1.setVoltage( + feedForward.calculate(0) + ); + }*/ + + elevatorMotor1.setVoltage( + feedForward.calculate(0) + ); }); + + + + + /* + elevatorMotor1.setVoltage( + pidController.calculate( + encoder.getPosition(), + clampedSetpoint + ) + feedForward.calculate(0) + ); +*/ + /* + if (!pidController.atSetpoint()) { + elevatorMotor1.setVoltage( + pidController.calculate( + encoder.getPosition(), + clampedSetpoint + ) + feedForward.calculate(0) + ); + } else { + elevatorMotor1.setVoltage( + feedForward.calculate(0) + ); + } + + });*/ } /** @@ -143,7 +191,6 @@ public class Elevator extends SubsystemBase { public Command goToSetpoint(DoubleSupplier setpoint) { return startRun(() -> { - pidController.setSetpoint(setpoint.getAsDouble()); pidController.reset(); @@ -161,7 +208,7 @@ public class Elevator extends SubsystemBase { feedForward.calculate(0) ); } - }); + }).until(() -> pidController.atSetpoint()); diff --git a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java index 11b651d..f4872b7 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java @@ -45,6 +45,7 @@ public class ManipulatorPivot extends SubsystemBase { ManipulatorPivotConstants.kPositionalI, ManipulatorPivotConstants.kPositionalD ); + pidController.setSetpoint(0); pidController.disableContinuousInput(); @@ -98,11 +99,11 @@ public class ManipulatorPivot extends SubsystemBase { public Command goToSetpoint(DoubleSupplier setpoint) { return startRun(() -> { - pidController.setSetpoint(setpoint.getAsDouble()); pidController.reset(); }, () -> { + /* if (!pidController.atSetpoint()) { pivotMotor.setVoltage( pidController.calculate( @@ -115,6 +116,43 @@ public class ManipulatorPivot extends SubsystemBase { -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0) ); } + */ + pivotMotor.setVoltage( + pidController.calculate( + encoder.getPosition(), + setpoint.getAsDouble() + ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0) + ); + }).until(() -> pidController.atSetpoint()); + } + + public Command maintainPosition() { + return startRun(() -> { + + + pidController.reset(); + }, + () -> { + /* + if (!pidController.atSetpoint()) { + pivotMotor.setVoltage( + pidController.calculate( + encoder.getPosition(), + setpoint.getAsDouble() + ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0) + ); + } else { + pivotMotor.setVoltage( + -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0) + ); + } + */ + pivotMotor.setVoltage( + pidController.calculate( + encoder.getPosition(), + pidController.getSetpoint() + ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0) + ); }); }