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

Renamed things for consistency, added a few methods

Browse Source
This commit is contained in:
NoahLacks63 2025-01-26 19:28:49 +00:00
parent dff4d0e04f
commit cef200a864
9 changed files with 156 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -4,10 +4,11 @@
package frc.robot;
import frc.robot.constants.ArmConstants;
import frc.robot.constants.ManipulatorPivotConstants;
import frc.robot.constants.ClimberPivotConstants;
import frc.robot.constants.ElevatorConstants;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.ManipulatorPivot;
import frc.robot.subsystems.ClimberPivot;
import frc.robot.subsystems.ClimberRollers;
import frc.robot.subsystems.Drivetrain;
@ -27,8 +28,6 @@ import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
public class RobotContainer {
private Arm arm;
private ClimberPivot climberPivot;
private ClimberRollers climberRollers;
@ -39,14 +38,14 @@ public class RobotContainer {
private Manipulator manipulator;
private ManipulatorPivot manipulatorPivot;
private CommandXboxController driver;
private CommandXboxController operator;
private SendableChooser<Command> autoChooser;
public RobotContainer() {
arm = new Arm();
climberPivot = new ClimberPivot();
climberRollers = new ClimberRollers();
@ -57,6 +56,8 @@ public class RobotContainer {
manipulator = new Manipulator();
manipulatorPivot = new ManipulatorPivot();
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
@ -71,10 +72,6 @@ public class RobotContainer {
private void configureButtonBindings() {
//Default commands
arm.setDefaultCommand(
arm.goToSetpoint(0, 1)
);
climberPivot.setDefaultCommand(
climberPivot.runPivot(0)
);
@ -97,7 +94,11 @@ public class RobotContainer {
);
manipulator.setDefaultCommand(
manipulator.runManipulator(() -> 0, true)
manipulator.defaultCommand()
);
manipulatorPivot.setDefaultCommand(
manipulatorPivot.runAssistedArm(operator::getRightY)
);
//Driver inputs
@ -109,32 +110,36 @@ public class RobotContainer {
manipulator.runManipulator(() -> 1, true)
);
driver.start().and(driver.back()).onTrue(
startingConfig()
);
//Operator inputs
operator.povUp().onTrue(
moveManipulator(
ElevatorConstants.kElevatorL4Position,
ArmConstants.kArmL4Position
ElevatorConstants.kL4Position,
ManipulatorPivotConstants.kL4Position
)
);
operator.povRight().onTrue(
moveManipulator(
ElevatorConstants.kElevatorL3Position,
ArmConstants.kArmL3Position
ElevatorConstants.kL3Position,
ManipulatorPivotConstants.kL3Position
)
);
operator.povLeft().onTrue(
moveManipulator(
ElevatorConstants.kElevatorL2Position,
ArmConstants.kArmL2Position
ElevatorConstants.kL2Position,
ManipulatorPivotConstants.kL2Position
)
);
operator.povDown().onTrue(
moveManipulator(
ElevatorConstants.kElevatorL1Position,
ArmConstants.kArmL1Position
ElevatorConstants.kL1Position,
ManipulatorPivotConstants.kL1Position
)
);
@ -172,7 +177,7 @@ public class RobotContainer {
.withPosition(0, 0)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("ArmPosition", arm::getEncoderPosition)
sensorTab.addDouble("ArmPosition", manipulatorPivot::getEncoderPosition)
.withSize(2, 1)
.withPosition(2, 0)
.withWidget(BuiltInWidgets.kTextView);
@ -188,8 +193,8 @@ public class RobotContainer {
*/
private Command coralIntakeRoutine() {
return moveManipulator(
ElevatorConstants.kElevatorCoralIntakePosition,
ArmConstants.kArmCoralIntakePosition
ElevatorConstants.kCoralIntakePosition,
ManipulatorPivotConstants.kCoralIntakePosition
)
.andThen(manipulator.runUntilCollected(1, true));
}
@ -202,8 +207,8 @@ public class RobotContainer {
*/
private Command algaeIntakeRoutine(boolean l2) {
return moveManipulator(
l2 ? ElevatorConstants.kElevatorL2AlgaePosition : ElevatorConstants.kElevatorL3AlgaePosition,
l2 ? ArmConstants.kArmL2AlgaePosition : ArmConstants.kArmL3AlgaePosition
l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition,
l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
)
.andThen(manipulator.runUntilCollected(1, false));
}
@ -218,23 +223,23 @@ public class RobotContainer {
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
if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (arm.isMotionSafe() && arm.isMotionSafe(armPosition))) {
if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (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
} else if (!arm.isMotionSafe(armPosition) && !arm.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
.andThen(arm.goToSetpoint(armPosition, 2));
} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition, 2));
// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
} else if (!arm.isMotionSafe(armPosition) && arm.isMotionSafe()) {
} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
// If the arm is behind the brace, move the arm first, then the elevator
} else if (!arm.isMotionSafe()) {
} else if (!manipulatorPivot.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
// Catch all command that's safe regardless of arm and elevator positions
} else {
return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
.andThen(arm.goToSetpoint(armPosition, 2));
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition, 2));
}
}
@ -248,23 +253,23 @@ 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.kElevatorBracePosition || elevatorPosition == 0) {
if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
armPosition = MathUtil.clamp(
armPosition,
0,
ArmConstants.kArmRotationLimit
ManipulatorPivotConstants.kRotationLimit
);
}
return Commands.either(
Commands.either(
elevator.goToSetpoint(elevatorPosition, 2).andThen(arm.goToSetpoint(armPosition, 2)),
elevator.goToSetpoint(elevatorPosition, 2).alongWith(arm.goToSetpoint(armPosition, 2)),
elevator.goToSetpoint(elevatorPosition, 2).andThen(manipulatorPivot.goToSetpoint(armPosition, 2)),
elevator.goToSetpoint(elevatorPosition, 2).alongWith(manipulatorPivot.goToSetpoint(armPosition, 2)),
() -> sequential
),
Commands.either(
arm.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)),
arm.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)),
manipulatorPivot.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)),
manipulatorPivot.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)),
() -> sequential
),
() -> elevatorFirst
@ -280,7 +285,12 @@ public class RobotContainer {
*/
@SuppressWarnings("unused")
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
.andThen(arm.goToSetpoint(armPosition, 2));
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition, 2));
}
private Command startingConfig() {
return moveManipulatorUtil(0, 0, false, true)
.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));
}
}

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

@ -14,6 +14,7 @@ public class ClimberPivotConstants {
public static final double kPIDControllerD = 0;
public static final double kClimberClimbPosition = 0;
public static final double kClimberStartingPosition = 0;
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
}

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

@ -33,18 +33,18 @@ public class ElevatorConstants {
public static final double kFeedForwardG = 0;
public static final double kFeedForwardV = 0;
public static final double kElevatorMaxVelocity = 0;
public static final double kMaxVelocity = 0;
public static final double kElevatorCoralIntakePosition = 0;
public static final double kElevatorL1Position = 0;
public static final double kElevatorL2Position = 0;
public static final double kElevatorL3Position = 0;
public static final double kElevatorL4Position = 0;
public static final double kElevatorL2AlgaePosition = 0;
public static final double kElevatorL3AlgaePosition = 0;
public static final double kCoralIntakePosition = 0;
public static final double kL1Position = 0;
public static final double kL2Position = 0;
public static final double kL3Position = 0;
public static final double kL4Position = 0;
public static final double kL2AlgaePosition = 0;
public static final double kL3AlgaePosition = 0;
/**The position of the top of the elevator brace */
public static final double kElevatorBracePosition = 0;
public static final double kElevatorMaxHeight = 0;
public static final double kBracePosition = 0;
public static final double kMaxHeight = 0;
// 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = 1;

20
src/main/java/frc/robot/constants/ArmConstants.java → src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
View File

@ -13,13 +13,13 @@ import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
public class ArmConstants {
public class ManipulatorPivotConstants {
public static final int kArmMotorID = 0;
public static final int kCANcoderID = 0;
public static final int kMotorAmpsMax = 0;
public static final double kArmMaxVelocity = 0;
public static final double kPivotMaxVelocity = 0;
public static final double kPositionalP = 0;
public static final double kPositionalI = 0;
@ -32,17 +32,17 @@ public class ArmConstants {
// TODO Is this reasonable?
public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60;
public static final double kArmCoralIntakePosition = 0;
public static final double kArmL1Position = 0;
public static final double kArmL2Position = 0;
public static final double kArmL3Position = 0;
public static final double kArmL4Position = 0;
public static final double kArmL2AlgaePosition = 0;
public static final double kArmL3AlgaePosition = 0;
public static final double kCoralIntakePosition = 0;
public static final double kL1Position = 0;
public static final double kL2Position = 0;
public static final double kL3Position = 0;
public static final double kL4Position = 0;
public static final double kL2AlgaePosition = 0;
public static final double kL3AlgaePosition = 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 kRotationLimit = 0;
public static final double kMagnetOffset = 0.0;
public static final double kAbsoluteSensorDiscontinuityPoint = 0.0;

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

@ -42,15 +42,16 @@ public class ClimberPivot extends SubsystemBase {
}
/**
* Runs the climber until it is at a defined setpoint
* Runs the climber until it is at setpoint
*
* @param speed The speed at which the pivot runs
* @return Sets the motor speed until at the climbed position
* @param setpoint The target position of the climber
* @return Sets the motor speed until at the target position
*/
public Command climb(double speed) {
public Command climb(double setpoint, double speed) {
return run(() -> {
pivotMotor.set(speed);
}).until(() -> neoEncoder.getPosition() >= ClimberPivotConstants.kClimberClimbPosition);
}).until(() -> neoEncoder.getPosition() >= setpoint);
}
/**

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

@ -13,6 +13,7 @@ import edu.wpi.first.math.controller.ElevatorFeedforward;
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.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ElevatorConstants;
@ -102,7 +103,7 @@ public class Elevator extends SubsystemBase {
* @return Is the motion safe
*/
public boolean isMotionSafe(double motionTarget) {
return motionTarget > ElevatorConstants.kElevatorBracePosition;
return motionTarget > ElevatorConstants.kBracePosition;
}
/**
@ -125,7 +126,7 @@ public class Elevator extends SubsystemBase {
*/
public Command runAssistedElevator(DoubleSupplier speed) {
return run(() -> {
double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kMaxVelocity;
double voltsOut = velocityController.calculate(
encoder.getVelocity(),
@ -134,11 +135,12 @@ public class Elevator extends SubsystemBase {
elevatorMotor1.setVoltage(voltsOut);
}).until(
() -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kElevatorMaxHeight);
() -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight);
}
/**
* Moves the elevator to a target destination (setpoint)
* Moves the elevator to a target destination (setpoint).
* If the setpoint is 0, the elevator will creep down to hit the limit switch
*
* @param setpoint Target destination of the subsystem
* @param timeout Time to achieve the setpoint before quitting
@ -148,19 +150,37 @@ public class Elevator extends SubsystemBase {
double clampedSetpoint = MathUtil.clamp(
setpoint,
0,
ElevatorConstants.kElevatorMaxHeight
ElevatorConstants.kMaxHeight
);
return run(() -> {
double voltsOut = positionController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(0);
elevatorMotor1.setVoltage(voltsOut);
}).until(
() -> positionController.atSetpoint() || bottomLimitSwitch.get()
).withTimeout(timeout);
if (clampedSetpoint == 0) {
return run(() -> {
double voltsOut = positionController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(0);
elevatorMotor1.setVoltage(voltsOut);
}).until(
() -> positionController.atSetpoint() || bottomLimitSwitch.get()
).withTimeout(timeout)
.andThen(Commands.either(
runAssistedElevator(() -> 0),
runAssistedElevator(() -> -.2),
bottomLimitSwitch::get
)).withTimeout(timeout);
} else {
return run(() -> {
double voltsOut = positionController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(0);
elevatorMotor1.setVoltage(voltsOut);
}).until(
() -> positionController.atSetpoint() || bottomLimitSwitch.get()
).withTimeout(timeout);
}
}
/**

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

@ -16,7 +16,7 @@ public class Manipulator extends SubsystemBase {
private SparkMax manipulatorMotor;
private DigitalInput coralBeamBreak;
private DigitalInput algaeBeamBreak;
private DigitalInput algaeBeamBreak;
public Manipulator() {
manipulatorMotor = new SparkMax(
@ -34,16 +34,32 @@ public class Manipulator extends SubsystemBase {
algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
}
/**
* The default command for the manipulator that either stops the manipulator or slowly
* runs the manipulator to retain the algae
*
* @return Returns a command that sets the speed of the motor
*/
public Command defaultCommand() {
return run(() -> {
manipulatorMotor.set(
algaeBeamBreak.get() ? 0.1 : 0
);
});
}
/**
* 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
* @return Returns a command that sets the speed of the motor
*/
public Command runManipulator(DoubleSupplier speed, boolean coral) {
return run(() -> {
manipulatorMotor.set(coral ? speed.getAsDouble() : speed.getAsDouble() * -1);
manipulatorMotor.set(
coral ? speed.getAsDouble() : speed.getAsDouble() * -1
);
});
}
@ -52,11 +68,13 @@ public class Manipulator extends SubsystemBase {
*
* @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
* @return Returns a command that sets the speed of the motor
*/
public Command runUntilCollected(double speed, boolean coral) {
return run(() -> {
manipulatorMotor.set(coral ? speed : speed * -1);
manipulatorMotor.set(
coral ? speed : speed * -1
);
}).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
}
}

36
src/main/java/frc/robot/subsystems/Arm.java → src/main/java/frc/robot/subsystems/ManipulatorPivot.java
View File

@ -14,9 +14,9 @@ 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.ArmConstants;
import frc.robot.constants.ManipulatorPivotConstants;
public class Arm extends SubsystemBase {
public class ManipulatorPivot extends SubsystemBase {
protected SparkMax armMotor;
private CANcoder canCoder;
@ -26,38 +26,38 @@ public class Arm extends SubsystemBase {
private ArmFeedforward feedForward;
public Arm() {
public ManipulatorPivot() {
armMotor = new SparkMax(
ArmConstants.kArmMotorID,
ManipulatorPivotConstants.kArmMotorID,
MotorType.kBrushless
);
armMotor.configure(
ArmConstants.motorConfig,
ManipulatorPivotConstants.motorConfig,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
positionController = new PIDController(
ArmConstants.kPositionalP,
ArmConstants.kPositionalI,
ArmConstants.kPositionalD
ManipulatorPivotConstants.kPositionalP,
ManipulatorPivotConstants.kPositionalI,
ManipulatorPivotConstants.kPositionalD
);
// TODO: Generate constants for continuous input range based on CANcoder configuration?
positionController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(179));
positionController.setTolerance(ArmConstants.kPositionalTolerance);
positionController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
velocityController = new PIDController(
ArmConstants.kVelocityP,
ArmConstants.kVelocityI,
ArmConstants.kVelocityD
ManipulatorPivotConstants.kVelocityP,
ManipulatorPivotConstants.kVelocityI,
ManipulatorPivotConstants.kVelocityD
);
velocityController.setTolerance(ArmConstants.kVelocityTolerance);
velocityController.setTolerance(ManipulatorPivotConstants.kVelocityTolerance);
canCoder = new CANcoder(ArmConstants.kCANcoderID);
canCoder.getConfigurator().apply(ArmConstants.canCoderConfig);
canCoder = new CANcoder(ManipulatorPivotConstants.kCANcoderID);
canCoder.getConfigurator().apply(ManipulatorPivotConstants.canCoderConfig);
}
/**
@ -78,7 +78,7 @@ public class Arm extends SubsystemBase {
* @return Is the motion safe
*/
public boolean isMotionSafe(double motionTarget) {
return motionTarget > ArmConstants.kArmSafeStowPosition;
return motionTarget > ManipulatorPivotConstants.kArmSafeStowPosition;
}
/**
@ -95,7 +95,7 @@ public class Arm extends SubsystemBase {
);
return run(() -> {
double realSpeedTarget = clampedSpeed * ArmConstants.kArmMaxVelocity;
double realSpeedTarget = clampedSpeed * ManipulatorPivotConstants.kPivotMaxVelocity;
double voltsOut = velocityController.calculate(
getEncoderVelocity(),
@ -120,7 +120,7 @@ public class Arm extends SubsystemBase {
double clampedSetpoint = MathUtil.clamp(
setpoint,
0,
ArmConstants.kArmRotationLimit
ManipulatorPivotConstants.kRotationLimit
);
return run(() -> {

8
src/main/java/frc/robot/subsystems/sysid/ArmSysID.java
View File

@ -10,10 +10,10 @@ import edu.wpi.first.units.measure.MutVoltage;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.constants.ArmConstants;
import frc.robot.subsystems.Arm;
import frc.robot.constants.ManipulatorPivotConstants;
import frc.robot.subsystems.ManipulatorPivot;
public class ArmSysID extends Arm {
public class ArmSysID extends ManipulatorPivot {
private MutVoltage appliedVoltage;
private MutAngle armPosition;
@ -32,7 +32,7 @@ public class ArmSysID extends Arm {
armVelocity = RadiansPerSecond.mutable(0);
routine = new SysIdRoutine(
ArmConstants.kSysIDConfig,
ManipulatorPivotConstants.kSysIDConfig,
new SysIdRoutine.Mechanism(
armMotor::setVoltage,
(log) -> {