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

Adjusting moveManipulator so it's not so...rough

Browse Source
This commit is contained in:
Bradley Bickford 2025-01-16 17:59:12 -05:00
parent d1d577f52f
commit 5325920b42
3 changed files with 78 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@ -184,25 +185,41 @@ public class RobotContainer {
}
private Command moveManipulator(double elevatorPosition, double armPosition) {
if ((elevator.getEncoderPosition() > ElevatorConstants.kElevatorBracePosition && elevatorPosition > ElevatorConstants.kElevatorBracePosition) || (arm.getEncoderPosition() > ArmConstants.kArmSafeStowPosition && armPosition > ArmConstants.kArmSafeStowPosition)) { //if the elevator and target are above the brace, or the arm and target is in front of the brace, move together
return arm.goToSetpoint(armPosition, 2)
.alongWith(elevator.goToSetpoint(elevatorPosition, 2));
} else if (armPosition < ArmConstants.kArmSafeStowPosition) { //if the target is behind the brace, move the elevator first
if (arm.getEncoderPosition() < ArmConstants.kArmSafeStowPosition) { //if the arm is behind the brace at start, move the arm to a safe position, then elevator, then arm
return arm.goToSetpoint(ArmConstants.kArmSafeStowPosition, 2)
.andThen(elevator.goToSetpoint(elevatorPosition, 2))
.andThen(arm.goToSetpoint(armPosition, 2));
} else { //if the arm is in front of the brace, move elevator, then arm
return elevator.goToSetpoint(elevatorPosition, 2)
.andThen(arm.goToSetpoint(armPosition, 2));
}
} else if (arm.getEncoderPosition() < ArmConstants.kArmSafeStowPosition) { //if the arm is behind the brace, move to target, then move the elevator
return arm.goToSetpoint(armPosition, 2)
.andThen(elevator.goToSetpoint(elevatorPosition, 2));
} else { //catch all that will always safely move the arm in a safe way
return arm.goToSetpoint(ArmConstants.kArmSafeStowPosition, 2)
.andThen(elevator.goToSetpoint(elevatorPosition, 2))
.andThen(arm.goToSetpoint(armPosition, 2));
// 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))) {
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));
// 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()) {
return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
// If the arm is behind the brace, move the arm first, then the elevator
} else if (!arm.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));
}
}
private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
return Commands.either(
Commands.either(
elevator.goToSetpoint(elevatorPosition, 2).andThen(arm.goToSetpoint(armPosition, 2)),
elevator.goToSetpoint(elevatorPosition, 2).alongWith(arm.goToSetpoint(armPosition, 2)),
() -> sequential
),
Commands.either(
arm.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)),
arm.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)),
() -> sequential
),
() -> elevatorFirst
);
}
}

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

@ -31,6 +31,27 @@ public class Arm extends SubsystemBase {
canCoder = new CANcoder(ArmConstants.kCANcoderID);
}
/**
* Returns whether or not the motion is safe relative to the encoder's current position
* and the arm safe stow position
*
* @return Is the motion safe
*/
public boolean isMotionSafe() {
return isMotionSafe(getEncoderPosition());
}
/**
* Returns whether or not the motion is safe relative to some target position and the
* arm safe stow position
*
* @param motionTarget The target position to determine the safety of
* @return Is the motion safe
*/
public boolean isMotionSafe(double motionTarget) {
return motionTarget > ArmConstants.kArmSafeStowPosition;
}
//manual command that keeps ouput speed consistent no matter the direction
public Command runArm(DoubleSupplier speed) {
return run(() -> {

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

@ -81,6 +81,27 @@ public class Elevator extends SubsystemBase {
);
}
/**
* Returns whether or not the motion is safe relative to the encoder's current position
* and the elevator brace position
*
* @return Is the motion safe
*/
public boolean isMotionSafe() {
return isMotionSafe(getEncoderPosition());
}
/**
* Returns whether or not the motion is safe relative to some target position and the elevator
* brace position
*
* @param motionTarget The target position to determine the safety of
* @return Is the motion safe
*/
public boolean isMotionSafe(double motionTarget) {
return motionTarget > ElevatorConstants.kElevatorBracePosition;
}
//manual command that keeps ouput speed consistent no matter the direction
public Command runElevator(DoubleSupplier speed) {
return run(() -> {