From 87e7eb49749821650b27d786459694e01904d01f Mon Sep 17 00:00:00 2001 From: Team 2648 Date: Sat, 22 Feb 2025 13:22:00 -0500 Subject: [PATCH] elevator pid work, but crash --- src/main/java/frc/robot/RobotContainer.java | 24 +++++++++++-------- .../robot/constants/ElevatorConstants.java | 4 ++-- .../constants/ManipulatorPivotConstants.java | 4 ++-- .../java/frc/robot/subsystems/Elevator.java | 9 ++++--- 4 files changed, 24 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 606efd5..6420ef4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -71,6 +71,7 @@ public class RobotContainer { configureButtonBindings(); //elevatorSysIDBindings(); + //elevatorOnlyBindings(); configureNamedCommands(); @@ -86,6 +87,13 @@ public class RobotContainer { operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse)); }*/ + private void elevatorOnlyBindings(){ + elevator.setDefaultCommand(elevator.maintainPosition()); + manipulatorPivot.setDefaultCommand(manipulatorPivot.maintainPosition()); + + driver.a().onTrue(safeMoveManipulator(ElevatorConstants.kL2Position, ManipulatorPivotConstants.kL2Position)); + } + private void configureButtonBindings() { //Default commands climberPivot.setDefaultCommand( @@ -139,7 +147,7 @@ public class RobotContainer { startingConfig() ); - driver.y().onTrue(drivetrain.zeroHeading()); + driver.y().whileTrue(drivetrain.zeroHeading()); driver.povDown().whileTrue(climberPivot.runPivot(-0.5)); driver.povUp().whileTrue(climberPivot.runPivot(0.5)); @@ -177,19 +185,15 @@ public class RobotContainer { ); operator.a().onTrue( - coralIntakeRoutine() + safeMoveManipulator(ElevatorConstants.kL1Position, 0.0) ); operator.x().onTrue( - algaeIntakeRoutine(true) + safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition) ); operator.b().onTrue( - algaeIntakeRoutine(false) - ); - - operator.y().whileTrue( - elevator.goToSetpoint(() -> ElevatorConstants.kL2Position) + safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition) ); } @@ -398,8 +402,8 @@ public class RobotContainer { .deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition), elevator.maintainPosition());*/ return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition) - .andThen(elevator.goToSetpoint(() -> elevatorPosition)) - .andThen(manipulatorPivot.goToSetpoint(() -> armPosition)); + .andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition) + .raceWith(elevator.maintainPosition())); } @SuppressWarnings("unused") diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index c345ffe..f531601 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -43,14 +43,14 @@ public class ElevatorConstants { public static final double kL1Position = 0; public static final double kL2Position = 14.5; public static final double kL3Position = 29.0; - public static final double kL4Position = 52.0; + public static final double kL4Position = 49.0; public static final double kL4TransitionPosition = 40.0; public static final double kL2AlgaePosition = 22.0; 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 = 0; - public static final double kMaxHeight = 52.0; //actual is 52 + public static final double kMaxHeight = 51.0; //actual is 51 // 1, 7, 10 are the defaults for these, change as necessary public static final double kSysIDRampRate = .25; diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index 8e3a96b..d4e27e0 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -34,7 +34,7 @@ public class ManipulatorPivotConstants { public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100 public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168 - public static final double kEncoderOffset = 0.7815; + public static final double kEncoderOffset = 0.781; public static final double kCoralIntakePosition = Units.degreesToRadians(175.0); public static final double kL1Position = Units.degreesToRadians(0.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(67.0); + public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.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 635816d..9059e80 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -141,7 +141,7 @@ public class Elevator extends SubsystemBase { public Command maintainPosition() { return startRun(() -> { - + System.out.println("maintain position"); }, () -> { @@ -184,10 +184,13 @@ public class Elevator extends SubsystemBase { return startRun(() -> { - pidControllerUp.setSetpoint(setpoint.getAsDouble()); - pidControllerDown.setSetpoint(setpoint.getAsDouble()); pidControllerUp.reset(); pidControllerDown.reset(); + pidControllerUp.setSetpoint(setpoint.getAsDouble()); + pidControllerDown.setSetpoint(setpoint.getAsDouble()); + + + System.out.println("go to setpoint"); }, () -> { double upOutput = pidControllerUp.calculate(getEncoderPosition());