Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1,000 comments, reworked the climber pivot, removed indexer, added clamps on goToSetpoint methods

Browse Source
This commit is contained in:
NoahLacks63 2025-01-21 03:56:00 +00:00
parent edb95da65c
commit 9ab7ffad84
11 changed files with 155 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
src/main/java/frc/robot/RobotContainer.java
View File

@ -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) {

3
src/main/java/frc/robot/constants/ArmConstants.java
View File

@ -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;

2
src/main/java/frc/robot/constants/ClimberPivotConstants.java
View File

@ -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;
}

1
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@ -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;

6
src/main/java/frc/robot/constants/IndexerConstants.java
View File

@ -1,6 +0,0 @@
package frc.robot.constants;
public class IndexerConstants {
public static final int kIndexerMotorID = 0;
public static final int kIndexerBeamBreakID = 0;
}

43
src/main/java/frc/robot/subsystems/Arm.java
View File

@ -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());
}

31
src/main/java/frc/robot/subsystems/ClimberPivot.java
View File

@ -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();
}

6
src/main/java/frc/robot/subsystems/ClimberRollers.java
View File

@ -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);

40
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -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);

36
src/main/java/frc/robot/subsystems/Indexer.java
View File

@ -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);
}
}

20
src/main/java/frc/robot/subsystems/Manipulator.java
View File

@ -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);