diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4ec5db5..6de4d05 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -103,18 +104,14 @@ public class RobotContainer { () -> true ) ); - + elevator.setDefaultCommand( - elevator.goToSetpoint( - () -> 0 - ) + elevator.maintainPosition() ); - + manipulatorPivot.setDefaultCommand( - manipulatorPivot.runManualPivot( - () -> 0 - ) + manipulatorPivot.maintainPosition() ); @@ -125,7 +122,7 @@ public class RobotContainer { ); //Driver inputs - /* + driver.start().whileTrue( drivetrain.setXCommand() ); @@ -141,52 +138,37 @@ public class RobotContainer { driver.start().and(driver.back()).onTrue( startingConfig() ); - */ + driver.povDown().whileTrue(climberPivot.runPivot(-0.5)); driver.povUp().whileTrue(climberPivot.runPivot(0.5)); driver.povLeft().whileTrue(climberRollers.runRoller(0.5)); driver.povRight().whileTrue(climberRollers.runRoller(-0.5)); - - operator.povUp().whileTrue( - elevator.goToSetpoint(() -> 50) - ); - - operator.povDown().whileTrue( - elevator.goToSetpoint(() -> 0) - ); - - /* - 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 +185,11 @@ public class RobotContainer { operator.b().onTrue( algaeIntakeRoutine(false) ); - */ + + operator.y().whileTrue( + elevator.goToSetpoint(() -> ElevatorConstants.kL2Position) + ); + } @@ -268,6 +254,12 @@ public class RobotContainer { .withSize(1, 1) .withPosition(5, 1) .withWidget(BuiltInWidgets.kTextView); + + sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput); + + sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition); + + } public Command getAutonomousCommand() { @@ -318,7 +310,7 @@ public class RobotContainer { // then the elevator, then the arm again } else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true) - .andThen(manipulatorPivot.goToSetpoint(armPosition)); + .andThen(manipulatorPivot.goToSetpoint(() -> armPosition)); // If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm } else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) { return moveManipulatorUtil(elevatorPosition, armPosition, true, true); @@ -328,7 +320,7 @@ public class RobotContainer { // Catch all command that's safe regardless of arm and elevator positions } else { return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true) - .andThen(manipulatorPivot.goToSetpoint(armPosition)); + .andThen(manipulatorPivot.goToSetpoint(() -> armPosition)); } } @@ -342,23 +334,24 @@ public class RobotContainer { * @return Moves the elevator and arm to the setpoints */ private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) { - if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) { + + /*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) { armPosition = MathUtil.clamp( armPosition, 0, ManipulatorPivotConstants.kRotationLimit ); - } + }*/ return Commands.either( Commands.either( - elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(armPosition)), - elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(armPosition)), + elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)), + elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)), () -> sequential ), Commands.either( - manipulatorPivot.goToSetpoint(armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)), - manipulatorPivot.goToSetpoint(armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)), + manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)), + manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)), () -> sequential ), () -> elevatorFirst @@ -369,15 +362,15 @@ public class RobotContainer { private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){ if(!isL4){ return Commands.sequence( - manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition), + manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition), elevator.goToSetpoint(() -> elevatorPosition), - manipulatorPivot.goToSetpoint(armPosition)); + manipulatorPivot.goToSetpoint(() -> armPosition)); }else{ return Commands.sequence( - manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition), + manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition), elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition), - Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(() -> elevatorPosition)); + Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition)); } } @@ -392,7 +385,7 @@ public class RobotContainer { @SuppressWarnings("unused") private Command safeMoveManipulator(double elevatorPosition, double armPosition) { return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true) - .andThen(manipulatorPivot.goToSetpoint(armPosition)); + .andThen(manipulatorPivot.goToSetpoint(() -> armPosition)); } @SuppressWarnings("unused") 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 726e761..8e3a96b 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -16,29 +16,29 @@ public class ManipulatorPivotConstants { public static final int kMotorCurrentMax = 40; - public static final double kPivotConversion = 12.0/60.0 * 20.0/60.0 * 12.0/28.0; + public static final double kPivotConversion = 2 * Math.PI; - public static final double kPivotMaxVelocity = 0; + public static final double kPivotMaxVelocity = 2 * Math.PI; - public static final double kPositionalP = 0; + 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); - public static final double kFeedForwardS = 0; - public static final double kFeedForwardG = 0.41; // calculated value 0.41 + public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19 + public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41 public static final double kFeedForwardV = 0.68; //calculated value 0.68 - public static final double kFFGravityOffset = Units.degreesToRadians(-135.0); + public static final double kFFGravityOffset = Units.degreesToRadians(135.0); 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; + public static final double kEncoderOffset = 0.7815; 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); @@ -69,7 +69,8 @@ public class ManipulatorPivotConstants { static { motorConfig .smartCurrentLimit(kMotorCurrentMax) - .idleMode(kIdleMode); + .idleMode(kIdleMode) + .inverted(true); motorConfig.absoluteEncoder .positionConversionFactor(kPivotConversion) .inverted(false) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index d9aa681..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,36 +129,42 @@ 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) + ); }); - } - - /** - * Moves the elevator to a target destination (setpoint). - * - * @param setpoint Target destination of the subsystem - * @param timeout Time to achieve the setpoint before quitting - * @return Sets motor voltage to achieve the target destination - */ - public Command goToSetpoint(DoubleSupplier setpoint) { - double clampedSetpoint = MathUtil.clamp( - setpoint.getAsDouble(), - 0, - ElevatorConstants.kMaxHeight - ); - - pidController.reset(); - - return run(() -> { + + + + + /* elevatorMotor1.setVoltage( pidController.calculate( encoder.getPosition(), clampedSetpoint ) + feedForward.calculate(0) ); - - /* +*/ + /* if (!pidController.atSetpoint()) { elevatorMotor1.setVoltage( pidController.calculate( @@ -170,8 +177,65 @@ public class Elevator extends SubsystemBase { feedForward.calculate(0) ); } - */ - }); + + });*/ + } + + /** + * Moves the elevator to a target destination (setpoint). + * + * @param setpoint Target destination of the subsystem + * @param timeout Time to achieve the setpoint before quitting + * @return Sets motor voltage to achieve the target destination + */ + public Command goToSetpoint(DoubleSupplier setpoint) { + + return startRun(() -> { + + pidController.setSetpoint(setpoint.getAsDouble()); + pidController.reset(); + }, + () -> { + if (!pidController.atSetpoint()) { + elevatorMotor1.setVoltage( + pidController.calculate( + encoder.getPosition(), + setpoint.getAsDouble() + ) + feedForward.calculate(0) + ); + } else { + elevatorMotor1.setVoltage( + feedForward.calculate(0) + ); + } + }).until(() -> pidController.atSetpoint()); + + + + + /* + 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) + ); + } + + });*/ } /* diff --git a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java index 7c41404..f4872b7 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java @@ -12,6 +12,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ManipulatorPivotConstants; @@ -44,6 +45,9 @@ public class ManipulatorPivot extends SubsystemBase { ManipulatorPivotConstants.kPositionalI, ManipulatorPivotConstants.kPositionalD ); + pidController.setSetpoint(0); + + pidController.disableContinuousInput(); feedForward = new ArmFeedforward( ManipulatorPivotConstants.kFeedForwardS, @@ -92,24 +96,64 @@ public class ManipulatorPivot extends SubsystemBase { * @param timeout Time to achieve the setpoint before quitting * @return Sets motor voltage to achieve the target destination */ - public Command goToSetpoint(double setpoint) { - double clampedSetpoint = MathUtil.clamp( - setpoint, - 0, - ManipulatorPivotConstants.kRotationLimit - ); + public Command goToSetpoint(DoubleSupplier setpoint) { + return startRun(() -> { + + pidController.setSetpoint(setpoint.getAsDouble()); + 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(), + setpoint.getAsDouble() + ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0) + ); + }).until(() -> pidController.atSetpoint()); + } - return run(() -> { - pivotMotor.setVoltage( - pidController.calculate( - encoder.getPosition(), - clampedSetpoint - ) + feedForward.calculate( - encoder.getPosition(), - 0 - ) - ); - }); + 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) + ); + }); } /** @@ -118,7 +162,7 @@ public class ManipulatorPivot extends SubsystemBase { * @return Encoder's position in radians */ public double getEncoderPosition() { - return encoder.getPosition(); + return Units.radiansToDegrees( encoder.getPosition()); } /** * Returns the encoder's velocity in radians per second @@ -126,6 +170,14 @@ public class ManipulatorPivot extends SubsystemBase { * @return Encoder's velocity in radians per second */ public double getEncoderVelocity() { - return encoder.getVelocity(); + return Units.radiansToDegrees(encoder.getVelocity()); + } + + public double getCGPosition(){ + return Units.radiansToDegrees(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset); + } + + public double getPivotOutput(){ + return pivotMotor.getAppliedOutput() * pivotMotor.getBusVoltage(); } }