diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bebe34e..a84846f 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; @@ -102,17 +103,17 @@ public class RobotContainer { () -> true ) ); - + /* elevator.setDefaultCommand( elevator.goToSetpoint( () -> 0 ) ); - + */ manipulatorPivot.setDefaultCommand( manipulatorPivot.runManualPivot( - () -> 0 + () -> operator.getRightY() * 0.5 ) ); @@ -140,28 +141,28 @@ 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) + manipulatorPivot.goToSetpoint(() -> 0) ); operator.povDown().whileTrue( - elevator.goToSetpoint(() -> 0) - ); - + 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( @@ -267,6 +268,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() { @@ -317,7 +324,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); @@ -327,7 +334,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)); } } @@ -341,23 +348,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 @@ -368,15 +376,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)); } } @@ -391,7 +399,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/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index 726e761..e289db3 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -16,25 +16,25 @@ 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 = 2.5; 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); @@ -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..0094d89 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -141,23 +141,40 @@ public class Elevator extends SubsystemBase { * @return Sets motor voltage to achieve the target destination */ public Command goToSetpoint(DoubleSupplier setpoint) { - double clampedSetpoint = MathUtil.clamp( - setpoint.getAsDouble(), - 0, - ElevatorConstants.kMaxHeight - ); + return startRun(() -> { + + + pidController.setSetpoint(setpoint.getAsDouble()); pidController.reset(); - - return run(() -> { + }, + () -> { + if (!pidController.atSetpoint()) { + elevatorMotor1.setVoltage( + pidController.calculate( + encoder.getPosition(), + setpoint.getAsDouble() + ) + feedForward.calculate(0) + ); + } else { + elevatorMotor1.setVoltage( + feedForward.calculate(0) + ); + } + }); + + + + + /* elevatorMotor1.setVoltage( pidController.calculate( encoder.getPosition(), clampedSetpoint ) + feedForward.calculate(0) ); - - /* +*/ + /* if (!pidController.atSetpoint()) { elevatorMotor1.setVoltage( pidController.calculate( @@ -170,8 +187,8 @@ public class Elevator extends SubsystemBase { 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..11b651d 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,8 @@ public class ManipulatorPivot extends SubsystemBase { ManipulatorPivotConstants.kPositionalI, ManipulatorPivotConstants.kPositionalD ); + + pidController.disableContinuousInput(); feedForward = new ArmFeedforward( ManipulatorPivotConstants.kFeedForwardS, @@ -92,24 +95,27 @@ 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 - ); - - return run(() -> { - pivotMotor.setVoltage( - pidController.calculate( - encoder.getPosition(), - clampedSetpoint - ) + feedForward.calculate( - encoder.getPosition(), - 0 - ) - ); - }); + 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) + ); + } + }); } /** @@ -118,7 +124,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 +132,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(); } }