From 771bcc89e195e2b557b6f8f0ccb57af78f3c8bb2 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Fri, 21 Feb 2025 20:39:03 -0500 Subject: [PATCH] Rework done, only a proof of concept, shouldn't be used without a lot of free time --- src/main/java/frc/robot/RobotContainer.java | 64 +++--- .../robot/constants/ElevatorConstants.java | 41 +++- .../robot/constants/ManipulatorConstants.java | 2 + .../java/frc/robot/subsystems/Elevator.java | 193 ++++++------------ .../frc/robot/subsystems/Manipulator.java | 6 + .../robot/subsystems/sysid/ElevatorSysID.java | 9 +- 6 files changed, 143 insertions(+), 172 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4ec082b..06cd0e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,6 +8,7 @@ import frc.robot.constants.ManipulatorPivotConstants; import frc.robot.constants.ClimberPivotConstants; import frc.robot.constants.ElevatorConstants; import frc.robot.constants.OIConstants; +import frc.robot.constants.ElevatorConstants.ElevatorPositions; import frc.robot.subsystems.ManipulatorPivot; import frc.robot.subsystems.Vision; import frc.robot.subsystems.ClimberPivot; @@ -26,6 +27,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class RobotContainer { @@ -53,14 +55,14 @@ public class RobotContainer { climberRollers = new ClimberRollers(); + manipulator = new Manipulator(); + //vision = new Vision(drivetrain::getGyroValue); drivetrain = new Drivetrain(); - elevator = new Elevator(); + elevator = new Elevator(manipulator::getAlgaePhotoswitch); //elevator = new ElevatorSysID(); - manipulator = new Manipulator(); - manipulatorPivot = new ManipulatorPivot(); driver = new CommandXboxController(OIConstants.kDriverControllerPort); @@ -104,11 +106,6 @@ public class RobotContainer { ) ); - elevator.setDefaultCommand( - elevator.maintainPosition() - ); - - manipulatorPivot.setDefaultCommand( manipulatorPivot.maintainPosition() ); @@ -147,28 +144,28 @@ public class RobotContainer { //Operator inputs operator.povUp().onTrue( safeMoveManipulator( - ElevatorConstants.kL4Position, + ElevatorPositions.kL4, ManipulatorPivotConstants.kL4Position ) ); operator.povRight().onTrue( safeMoveManipulator( - ElevatorConstants.kL3Position, + ElevatorPositions.kL3, ManipulatorPivotConstants.kL3Position ) ); operator.povLeft().onTrue( safeMoveManipulator( - ElevatorConstants.kL2Position, + ElevatorPositions.kL2, ManipulatorPivotConstants.kL2Position ) ); operator.povDown().onTrue( safeMoveManipulator( - ElevatorConstants.kL1Position, + ElevatorPositions.kL1, ManipulatorPivotConstants.kL1Position ) ); @@ -185,8 +182,8 @@ public class RobotContainer { algaeIntakeRoutine(false) ); - operator.y().whileTrue( - elevator.goToSetpoint(() -> ElevatorConstants.kL2Position) + operator.y().onTrue( + elevator.setTargetPosition(ElevatorPositions.kL2) ); @@ -272,7 +269,7 @@ public class RobotContainer { @SuppressWarnings("unused") private Command coralIntakeRoutine() { return moveManipulator( - ElevatorConstants.kCoralIntakePosition, + ElevatorPositions.kCoralIntake, ManipulatorPivotConstants.kCoralIntakePosition ) .andThen(manipulator.runUntilCollected(() -> .5)); @@ -287,7 +284,7 @@ public class RobotContainer { @SuppressWarnings("unused") private Command algaeIntakeRoutine(boolean l2) { return moveManipulator( - l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, + l2 ? ElevatorPositions.kL2Algae : ElevatorPositions.kL3Algae, l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition ) .andThen(manipulator.runUntilCollected(() -> 1)); @@ -300,10 +297,10 @@ public class RobotContainer { * @param armPosition The target rotation of the arm * @return Moves the elevator and arm to the setpoints using the most efficient path */ - private Command moveManipulator(double elevatorPosition, double armPosition) { + private Command moveManipulator(ElevatorPositions elevatorPosition, double armPosition) { // If the elevator current and target positions are above the brace, or the arm current and target position is in // front of the brace, move together - if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) { + if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition.getPosition())) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) { return moveManipulatorUtil(elevatorPosition, armPosition, false, false); // If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first, // then the elevator, then the arm again @@ -332,7 +329,7 @@ public class RobotContainer { * @param sequential Does the elevator and arm move separately? (True = .andThen, False = .alongWith) * @return Moves the elevator and arm to the setpoints */ - private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) { + private Command moveManipulatorUtil(ElevatorPositions elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) { /*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) { armPosition = MathUtil.clamp( @@ -344,13 +341,16 @@ public class RobotContainer { return Commands.either( Commands.either( - elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)), - elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)), + elevator.setTargetPosition(elevatorPosition).andThen( + new WaitUntilCommand(elevator::atSetpoint), + manipulatorPivot.goToSetpoint(() -> armPosition) + ), + elevator.setTargetPosition(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.setTargetPosition(elevatorPosition)), + manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.setTargetPosition(elevatorPosition)), () -> sequential ), () -> elevatorFirst @@ -358,18 +358,24 @@ public class RobotContainer { } @SuppressWarnings("unused") - private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){ + private Command manipulatorSafeTravel(ElevatorPositions elevatorPosition, double armPosition, boolean isL4){ if(!isL4){ return Commands.sequence( manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition), - elevator.goToSetpoint(() -> elevatorPosition), + elevator.setTargetPosition(elevatorPosition), + new WaitUntilCommand(elevator::atSetpoint), manipulatorPivot.goToSetpoint(() -> armPosition)); }else{ return Commands.sequence( manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition), - elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition), - Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition)); + elevator.setTargetPosition(elevatorPosition), + new WaitUntilCommand(() -> elevator.getEncoderPosition() > ElevatorPositions.kL4Transition.getPosition()), + Commands.parallel( + manipulatorPivot.goToSetpoint(() -> armPosition), + elevator.setTargetPosition(elevatorPosition).andThen(new WaitUntilCommand(elevator::atSetpoint)) + ) + ); } } @@ -382,14 +388,14 @@ public class RobotContainer { * @return Moves the elevator and arm to the setpoints */ @SuppressWarnings("unused") - private Command safeMoveManipulator(double elevatorPosition, double armPosition) { + private Command safeMoveManipulator(ElevatorPositions elevatorPosition, double armPosition) { return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true) .andThen(manipulatorPivot.goToSetpoint(() -> armPosition)); } @SuppressWarnings("unused") private Command startingConfig() { - return moveManipulatorUtil(0, 0, false, true) + return moveManipulatorUtil(ElevatorPositions.kStartingPosition, 0, false, true) .alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index c7bd9f7..8e737a3 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -11,6 +11,34 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; public class ElevatorConstants { + public enum ElevatorControlMode { + kPID, + kManualMaintain + } + + public enum ElevatorPositions { + kStartingPosition(0), + kCoralIntake(0), + kL1(0), + kL2(14.5), + kL2Algae(22), + kL3(29), + kL3Algae(39), + kL4(53), + kL4Transition(40), + kProcessor(4); + + private double position; + + private ElevatorPositions(double position) { + this.position = position; + } + + public double getPosition() { + return position; + } + } + public static final int kElevatorMotor1ID = 8; public static final int kElevatorMotor2ID = 6; @@ -36,18 +64,13 @@ public class ElevatorConstants { public static final double kFeedForwardG = (0.95 + 0.2)/2; /* kG too high + kG too low / 2 */ // calculated value 0.6 public static final double kFeedForwardV = 0.12; // calculated value 0.12 + public static final double kFeedForwardAlgaeHeldS = 0; + public static final double kFeedForwardAlgaeHeldG = 0; + public static final double kFeedForwardAlgaeHeldV = 0; + public static final double kMaxVelocity = 150.0; // 120 inches per second (COOKING) calculated max is 184 in/s public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2 - public static final double kCoralIntakePosition = 0; - 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 = 53.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 = 47.5; //actual is 53 diff --git a/src/main/java/frc/robot/constants/ManipulatorConstants.java b/src/main/java/frc/robot/constants/ManipulatorConstants.java index 5e4a580..ab6204f 100644 --- a/src/main/java/frc/robot/constants/ManipulatorConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorConstants.java @@ -4,7 +4,9 @@ import com.revrobotics.spark.config.SparkMaxConfig; public class ManipulatorConstants { public static final int kManipulatorMotorID = 12; + public static final int kCoralBeamBreakID = 2; + public static final int kAlgaePhotoswitchID = 3; public static final SparkMaxConfig motorConfig = new SparkMaxConfig(); } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 45bbe98..db8175c 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import com.revrobotics.RelativeEncoder; @@ -15,6 +16,8 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.ElevatorConstants.ElevatorControlMode; +import frc.robot.constants.ElevatorConstants.ElevatorPositions; public class Elevator extends SubsystemBase { protected SparkMax elevatorMotor1; @@ -22,13 +25,20 @@ public class Elevator extends SubsystemBase { protected RelativeEncoder encoder; + private BooleanSupplier hasAlgae; + private DigitalInput bottomLimitSwitch; private PIDController pidController; private ElevatorFeedforward feedForward; + private ElevatorFeedforward algaeHeldFeedForward; + + private ElevatorControlMode mode; + + private ElevatorPositions currentTargetPosition; - public Elevator() { + public Elevator(BooleanSupplier hasAlgae) { elevatorMotor1 = new SparkMax( ElevatorConstants.kElevatorMotor1ID, MotorType.kBrushless @@ -53,6 +63,8 @@ public class Elevator extends SubsystemBase { encoder = elevatorMotor1.getEncoder(); + this.hasAlgae = hasAlgae; + bottomLimitSwitch = new DigitalInput( ElevatorConstants.kBottomLimitSwitchID ); @@ -71,6 +83,16 @@ public class Elevator extends SubsystemBase { ElevatorConstants.kFeedForwardG, ElevatorConstants.kFeedForwardV ); + + algaeHeldFeedForward = new ElevatorFeedforward( + ElevatorConstants.kFeedForwardAlgaeHeldS, + ElevatorConstants.kFeedForwardAlgaeHeldG, + ElevatorConstants.kFeedForwardAlgaeHeldV + ); + + mode = ElevatorControlMode.kPID; + + currentTargetPosition = ElevatorPositions.kCoralIntake; } @Override @@ -78,6 +100,27 @@ public class Elevator extends SubsystemBase { if (!getBottomLimitSwitch()) { encoder.setPosition(0); } + + if(mode == ElevatorControlMode.kPID) { + if (!pidController.atSetpoint()) { + elevatorMotor1.setVoltage( + pidController.calculate( + encoder.getPosition(), + currentTargetPosition.getPosition() + ) + (hasAlgae.getAsBoolean() ? algaeHeldFeedForward.calculate(0) : feedForward.calculate(0)) + ); + } else { + if (hasAlgae.getAsBoolean()) { + elevatorMotor1.setVoltage( + algaeHeldFeedForward.calculate(0) + ); + } else { + elevatorMotor1.setVoltage( + feedForward.calculate(0) + ); + } + } + } } /** @@ -116,145 +159,33 @@ public class Elevator extends SubsystemBase { speed.getAsDouble() ); } else { - elevatorMotor1.setVoltage(feedForward.calculate(0)); + if (hasAlgae.getAsBoolean()) { + elevatorMotor1.setVoltage( + algaeHeldFeedForward.calculate(0) + ); + } else { + elevatorMotor1.setVoltage( + feedForward.calculate(0) + ); + } } - }); } - /** - * A command that will use the feed forward to hold up the elevator. - * Used for feed forward tuning. - * - * @return Sets motor voltage based on feed forward calculation. - */ - public Command maintainPosition() { + public Command setTargetPosition(ElevatorPositions position) { + return runOnce(() -> { + if (mode == ElevatorControlMode.kManualMaintain) { + pidController.reset(); + mode = ElevatorControlMode.kPID; + } - 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) - ); + currentTargetPosition = position; }); - - - - - /* - 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) - ); - } - - });*/ } - /** - * 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) - ); - } - - });*/ + public boolean atSetpoint() { + return pidController.atSetpoint(); } - - /* - if(encoder.getPosition() >= setpoint.getAsDouble()){ - elevatorMotor1.setVoltage( - pidControllerUp.calculate( - encoder.getPosition(), - clampedSetpoint - ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity) - ); - }else if(encoder.getPosition() <= setpoint.getAsDouble()){ - elevatorMotor1.setVoltage( - pidControllerDown.calculate( - encoder.getPosition(), - clampedSetpoint - ) + feedForward.calculate(pidControllerDown.getSetpoint().velocity) - ); - } - */ /** * Returns the current encoder position diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java index d7ebed8..37e6240 100644 --- a/src/main/java/frc/robot/subsystems/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/Manipulator.java @@ -16,6 +16,7 @@ public class Manipulator extends SubsystemBase { private SparkMax manipulatorMotor; private DigitalInput coralBeamBreak; + private DigitalInput algaePhotoswitch; public Manipulator() { manipulatorMotor = new SparkMax( @@ -30,6 +31,7 @@ public class Manipulator extends SubsystemBase { ); coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID); + algaePhotoswitch = new DigitalInput(ManipulatorConstants.kAlgaePhotoswitchID); } /** @@ -90,4 +92,8 @@ public class Manipulator extends SubsystemBase { public boolean getCoralBeamBreak() { return coralBeamBreak.get(); } + + public boolean getAlgaePhotoswitch() { + return algaePhotoswitch.get(); + } } diff --git a/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java b/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java index c650930..d928c32 100644 --- a/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java +++ b/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java @@ -2,6 +2,9 @@ package frc.robot.subsystems.sysid; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Volts; + +import java.util.function.BooleanSupplier; + import static edu.wpi.first.units.Units.InchesPerSecond; import edu.wpi.first.units.measure.MutDistance; @@ -22,8 +25,8 @@ public class ElevatorSysID extends Elevator { private SysIdRoutine routine; - public ElevatorSysID() { - super(); + public ElevatorSysID(BooleanSupplier hasAlgae) { + super(hasAlgae); appliedVoltage = Volts.mutable(0); @@ -38,7 +41,7 @@ public class ElevatorSysID extends Elevator { (log) -> { log.motor("elevatorMotor1") .voltage(appliedVoltage.mut_replace( - elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts + getMotor1(), Volts )) .linearPosition(elevatorPosition.mut_replace( encoder.getPosition(), Inches