From 9ab7ffad84fcdeea8ce860526b7decd0cce18faa Mon Sep 17 00:00:00 2001 From: NoahLacks63 Date: Tue, 21 Jan 2025 03:56:00 +0000 Subject: [PATCH] 1,000 comments, reworked the climber pivot, removed indexer, added clamps on goToSetpoint methods --- src/main/java/frc/robot/RobotContainer.java | 61 ++++++++++++++----- .../frc/robot/constants/ArmConstants.java | 3 + .../constants/ClimberPivotConstants.java | 2 + .../robot/constants/ElevatorConstants.java | 1 + .../frc/robot/constants/IndexerConstants.java | 6 -- src/main/java/frc/robot/subsystems/Arm.java | 43 +++++++++++-- .../frc/robot/subsystems/ClimberPivot.java | 31 +++++----- .../frc/robot/subsystems/ClimberRollers.java | 6 ++ .../java/frc/robot/subsystems/Elevator.java | 40 ++++++++---- .../java/frc/robot/subsystems/Indexer.java | 36 ----------- .../frc/robot/subsystems/Manipulator.java | 20 +++++- 11 files changed, 155 insertions(+), 94 deletions(-) delete mode 100644 src/main/java/frc/robot/constants/IndexerConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/Indexer.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 47a5b8f..3d57018 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,12 +12,12 @@ import frc.robot.subsystems.ClimberPivot; import frc.robot.subsystems.ClimberRollers; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Indexer; import frc.robot.subsystems.Manipulator; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -37,8 +37,6 @@ public class RobotContainer { private Elevator elevator; - private Indexer indexer; - private Manipulator manipulator; private CommandXboxController driver; @@ -57,8 +55,6 @@ public class RobotContainer { elevator = new Elevator(); - indexer = new Indexer(); - manipulator = new Manipulator(); driver = new CommandXboxController(OIConstants.kDriverControllerPort); @@ -74,12 +70,13 @@ public class RobotContainer { } private void configureButtonBindings() { + //Default commands arm.setDefaultCommand( arm.goToSetpoint(0, 1) ); climberPivot.setDefaultCommand( - climberPivot.goToAngle(0, 1) + climberPivot.runPivot(0) ); climberRollers.setDefaultCommand( @@ -99,12 +96,8 @@ public class RobotContainer { elevator.runAssistedElevator(operator::getLeftY) ); - indexer.setDefaultCommand( - indexer.runIndexer(0) - ); - manipulator.setDefaultCommand( - manipulator.runManipulator(0) + manipulator.runManipulator(() -> 0, true) ); //Driver inputs @@ -113,7 +106,7 @@ public class RobotContainer { ); driver.rightTrigger().whileTrue( - manipulator.runManipulator(1) + manipulator.runManipulator(() -> 1, true) ); //Operator inputs @@ -189,7 +182,10 @@ public class RobotContainer { return autoChooser.getSelected(); } - //teleop routines + /** + * Moves the elevator and arm to the coral intake position, then runs the manipulator until collected + * @return Moves the elevator and arm, then intakes coral + */ private Command coralIntakeRoutine() { return moveManipulator( ElevatorConstants.kElevatorCoralIntakePosition, @@ -198,6 +194,12 @@ public class RobotContainer { .andThen(manipulator.runUntilCollected(1, true)); } + /** + * Moves the elevator and arm to the constant setpoints and runs the manipulator until collected + * + * @param l2 Is the algae on L2? (True = L2, False = L3) + * @return Moves the elevator and arm then intakes algae + */ private Command algaeIntakeRoutine(boolean l2) { return moveManipulator( l2 ? ElevatorConstants.kElevatorL2AlgaePosition : ElevatorConstants.kElevatorL3AlgaePosition, @@ -206,6 +208,13 @@ public class RobotContainer { .andThen(manipulator.runUntilCollected(1, false)); } + /** + * Moves the elevator and arm in different order based on target positions + * + * @param elevatorPosition The target position of the elevator + * @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) { // 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 @@ -229,7 +238,24 @@ public class RobotContainer { } } + /** + * Moves the elevator and arm in customizeable ways + * + * @param elevatorPosition The target elevator position + * @param armPosition The target arm position + * @param elevatorFirst Does the elevator move first? (True = Elevator first, False = Arm first) + * @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) { + if (elevatorPosition <= ElevatorConstants.kElevatorBracePosition || elevatorPosition == 0) { + armPosition = MathUtil.clamp( + armPosition, + 0, + ArmConstants.kArmRotationLimit + ); + } + return Commands.either( Commands.either( elevator.goToSetpoint(elevatorPosition, 2).andThen(arm.goToSetpoint(armPosition, 2)), @@ -245,9 +271,12 @@ public class RobotContainer { ); } - /* - * A moveManipulator method that will guarantee a safe movement. - * Here in case we need want to skip moveManipulator debugging + /** + * Moves the arm and elevator in a safe way. + * + * @param elevatorPosition The target position of the elevator + * @param armPosition The target rotation of the arm + * @return Moves the elevator and arm to the setpoints */ @SuppressWarnings("unused") private Command safeMoveManipulator(double elevatorPosition, double armPosition) { diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index 3ecc1fb..48be67a 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -39,7 +39,10 @@ public class ArmConstants { public static final double kArmL4Position = 0; public static final double kArmL2AlgaePosition = 0; public static final double kArmL3AlgaePosition = 0; + /**The closest position to the elevator brace without hitting it */ public static final double kArmSafeStowPosition = 0; + /**The forward rotation limit of the arm */ + public static final double kArmRotationLimit = 0; public static final double kMagnetOffset = 0.0; public static final double kAbsoluteSensorDiscontinuityPoint = 0.0; diff --git a/src/main/java/frc/robot/constants/ClimberPivotConstants.java b/src/main/java/frc/robot/constants/ClimberPivotConstants.java index 0138ad2..b83b040 100644 --- a/src/main/java/frc/robot/constants/ClimberPivotConstants.java +++ b/src/main/java/frc/robot/constants/ClimberPivotConstants.java @@ -10,4 +10,6 @@ public class ClimberPivotConstants { public static final double kPIDControllerP = 0; public static final double kPIDControllerI = 0; public static final double kPIDControllerD = 0; + + public static final double kClimberClimbPosition = 0; } diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 2367a0b..a70e003 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -42,6 +42,7 @@ public class ElevatorConstants { public static final double kElevatorL4Position = 0; public static final double kElevatorL2AlgaePosition = 0; public static final double kElevatorL3AlgaePosition = 0; + /**The position of the top of the elevator brace */ public static final double kElevatorBracePosition = 0; public static final double kElevatorMaxHeight = 0; diff --git a/src/main/java/frc/robot/constants/IndexerConstants.java b/src/main/java/frc/robot/constants/IndexerConstants.java deleted file mode 100644 index cdec2fe..0000000 --- a/src/main/java/frc/robot/constants/IndexerConstants.java +++ /dev/null @@ -1,6 +0,0 @@ -package frc.robot.constants; - -public class IndexerConstants { - public static final int kIndexerMotorID = 0; - public static final int kIndexerBeamBreakID = 0; -} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 99fca13..943e43c 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -8,6 +8,7 @@ import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; 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; @@ -76,10 +77,21 @@ public class Arm extends SubsystemBase { return motionTarget > ArmConstants.kArmSafeStowPosition; } - //manual command that keeps ouput speed consistent no matter the direction - public Command runArm(DoubleSupplier speed) { + /** + * A manual rotation command that will move the elevator using a consistent velocity disregarding direction + * + * @param speed The velocity at which the arm rotates + * @return Sets motor voltage to achieve the target velocity + */ + public Command runAssistedArm(DoubleSupplier speed) { + double clampedSpeed = MathUtil.clamp( + speed.getAsDouble(), + -1, + 1 + ); + return run(() -> { - double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity; + double realSpeedTarget = clampedSpeed * ArmConstants.kArmMaxVelocity; double voltsOut = velocityController.calculate( getEncoderVelocity(), @@ -93,11 +105,24 @@ public class Arm extends SubsystemBase { }); } + /** + * Moves the arm 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(double setpoint, double timeout) { + double clampedSetpoint = MathUtil.clamp( + setpoint, + 0, + ArmConstants.kArmRotationLimit + ); + return run(() -> { double voltsOut = positionController.calculate( getEncoderPosition(), - setpoint + clampedSetpoint ) + feedForward.calculate( getEncoderPosition(), getEncoderVelocity() @@ -107,10 +132,20 @@ public class Arm extends SubsystemBase { }).until(positionController::atSetpoint).withTimeout(timeout); } + /** + * Returns the CANCoder's position in radians + * + * @return CANCoder's position in radians + */ public double getEncoderPosition() { return Units.rotationsToRadians(canCoder.getAbsolutePosition().getValueAsDouble()); } + /** + * Returns the CANCoder's velocity in radians per second + * + * @return CANCoder's velocity in radians per second + */ public double getEncoderVelocity() { return Units.rotationsToRadians(canCoder.getVelocity().getValueAsDouble()); } diff --git a/src/main/java/frc/robot/subsystems/ClimberPivot.java b/src/main/java/frc/robot/subsystems/ClimberPivot.java index 791b6a6..f57a5b7 100644 --- a/src/main/java/frc/robot/subsystems/ClimberPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimberPivot.java @@ -4,7 +4,6 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -17,8 +16,6 @@ public class ClimberPivot extends SubsystemBase { private DigitalInput cageLimitSwitch; - private PIDController pidController; - public ClimberPivot() { pivotMotor = new SparkMax( ClimberPivotConstants.kPivotMotorID, @@ -28,12 +25,6 @@ public class ClimberPivot extends SubsystemBase { neoEncoder = pivotMotor.getEncoder(); cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID); - - pidController = new PIDController( - ClimberPivotConstants.kPIDControllerP, - ClimberPivotConstants.kPIDControllerI, - ClimberPivotConstants.kPIDControllerD - ); } public Command runPivot(double speed) { @@ -42,17 +33,23 @@ public class ClimberPivot extends SubsystemBase { }); } - public Command goToAngle(double setpoint, double timeout) { + /** + * Runs the climber until it is at a defined setpoint + * + * @param speed The speed at which the pivot runs + * @return Sets the motor speed until at the climbed position + */ + public Command climb(double speed) { return run(() -> { - pivotMotor.set( - pidController.calculate( - neoEncoder.getPosition(), - setpoint - ) - ); - }).withTimeout(timeout); + pivotMotor.set(speed); + }).until(() -> neoEncoder.getPosition() >= ClimberPivotConstants.kClimberClimbPosition); } + /** + * Returns the limit switch attached to the climber. Detects if the cage is present + * + * @return Is the cage in the climber + */ public boolean getCageLimitSwitch() { return cageLimitSwitch.get(); } diff --git a/src/main/java/frc/robot/subsystems/ClimberRollers.java b/src/main/java/frc/robot/subsystems/ClimberRollers.java index 61aa276..5ecdba6 100644 --- a/src/main/java/frc/robot/subsystems/ClimberRollers.java +++ b/src/main/java/frc/robot/subsystems/ClimberRollers.java @@ -17,6 +17,12 @@ public class ClimberRollers extends SubsystemBase { ); } + /** + * Runs the rollers at a set speed + * + * @param speed The speed in which the roller runs + * @return Runs the rollers at a set speed + */ public Command runRoller(double speed) { return run(() -> { rollerMotor.set(speed); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 4c7f2b3..71319ba 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -8,6 +8,7 @@ import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DigitalInput; @@ -76,6 +77,13 @@ public class Elevator extends SubsystemBase { ); } + @Override + public void periodic() { + if (getBottomLimitSwitch()) { + encoder.setPosition(0); + } + } + /** * Returns whether or not the motion is safe relative to the encoder's current position * and the elevator brace position @@ -96,6 +104,18 @@ public class Elevator extends SubsystemBase { public boolean isMotionSafe(double motionTarget) { return motionTarget > ElevatorConstants.kElevatorBracePosition; } + + /** + * A manual translation command that uses feed forward calculation to maintain position + * + * @param speed The speed at which the elevator translates + * @return Sets motor voltage to translate the elevator and maintain position + */ + public Command runManualElevator(double speed) { + return run(() -> { + elevatorMotor1.set(speed); + }); + } /** * A manual translation command that will move the elevator using a consistent velocity disregarding direction @@ -116,18 +136,6 @@ public class Elevator extends SubsystemBase { }).until( () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kElevatorMaxHeight); } - - /** - * A manual translation command that uses feed forward calculation to maintain position - * - * @param speed The speed at which the elevator translates - * @return Sets motor voltage to translate the elevator and maintain position - */ - public Command runManualElevator(double speed) { - return run(() -> { - elevatorMotor1.set(speed); - }); - } /** * Moves the elevator to a target destination (setpoint) @@ -137,10 +145,16 @@ public class Elevator extends SubsystemBase { * @return Sets motor voltage to achieve the target destination */ public Command goToSetpoint(double setpoint, double timeout) { + double clampedSetpoint = MathUtil.clamp( + setpoint, + 0, + ElevatorConstants.kElevatorMaxHeight + ); + return run(() -> { double voltsOut = positionController.calculate( encoder.getPosition(), - setpoint + clampedSetpoint ) + feedForward.calculate(0); elevatorMotor1.setVoltage(voltsOut); diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java deleted file mode 100644 index c163749..0000000 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.subsystems; - -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkLowLevel.MotorType; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.IndexerConstants; - -public class Indexer extends SubsystemBase { - private SparkMax indexerMotor; - - private DigitalInput indexerBeamBreak; - - public Indexer() { - indexerMotor = new SparkMax( - IndexerConstants.kIndexerMotorID, - MotorType.kBrushless - ); - - indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakID); - } - - public Command runIndexer(double speed) { - return run(() -> { - indexerMotor.set(speed); - }); - } - - public Command indexCoral(double speed) { - return run(() -> { - indexerMotor.set(speed); - }).until(indexerBeamBreak::get); - } -} diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java index 8cb3f65..7916cb9 100644 --- a/src/main/java/frc/robot/subsystems/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/Manipulator.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -24,12 +26,26 @@ public class Manipulator extends SubsystemBase { algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID); } - public Command runManipulator(double speed) { + /** + * Runs the manipulator at a set speed with the direction based on the coral parameter + * + * @param speed The speed at which the manipulator runs + * @param coral Is the manipulator manipulating a coral? (True = Coral, False = Algae) + * @return Runs manipulator at speed + */ + public Command runManipulator(DoubleSupplier speed, boolean coral) { return run(() -> { - manipulatorMotor.set(speed); + manipulatorMotor.set(coral ? speed.getAsDouble() : speed.getAsDouble() * -1); }); } + /** + * Runs the manipulator until either the algae or coral beam break reads true + * + * @param speed The speed at which the manipulator is run + * @param coral Is the object a coral? (True = Coral, False = Algae) + * @return Runs the manipulator until an object is present + */ public Command runUntilCollected(double speed, boolean coral) { return run(() -> { manipulatorMotor.set(coral ? speed : speed * -1);