Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #33 from Team4388/driver-preference-controls

Browse Source 
Created Driver Controls
This commit is contained in:
C4llSqin
2025-02-26 17:26:16 -07:00
committed by GitHub
parent 46674cc213 a24b7ea0e7
commit 13e4d363b7
6 changed files with 244 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+9 -9
View File
@@ -237,7 +237,7 @@ public final class Constants {
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, 1, 0);
public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0);
public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
public static final int LIDAR_DIO_CHANNEL = 7;
@@ -430,21 +430,21 @@ public final class Constants {
public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
//Max for endefector = 60%
public static final double L2_SCORE_ENDEFECTOR = -19;
public static final double L2_SCORE_ENDEFFECTOR = -19;
public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
public static final double DEALGAE_L2_EENDEFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
public static final double PRIMED_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
public static final double COMPLETLY_TOP_ENDEFECTOR = 0.51 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
public static final double DEALGAE_L2_ENDEFFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
.withKP(1)
.withKI(0)
.withKD(0);
public static final Slot0Configs ENDEFECTOR_PID = new Slot0Configs()
public static final Slot0Configs ENDEFFECTOR_PID = new Slot0Configs()
.withKP(1)
.withKI(0)
.withKD(0);
src/main/java/frc4388/robot/RobotContainer.java
+93 -5
View File
@@ -19,6 +19,7 @@ import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.utility.controller.XboxController;
@@ -87,7 +88,7 @@ public class RobotContainer {
// private final LED m_robotLED = new LED();
public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
public final Lidar m_lidar = new Lidar();
public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endefectorLimitSwitch);
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
@@ -396,7 +397,12 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
boolean driverPreferenceControls = false;
// ? /* Driver Buttons */
if (!driverPreferenceControls) {
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
@@ -434,12 +440,17 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
} else {
}
// ? /* Operator Buttons */
if (!driverPreferenceControls) {
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar));
@@ -470,11 +481,11 @@ public class RobotContainer {
.onTrue(AprilLidarAlignL2Right);
// Lower coral removal
// Lower algae removal
new JoystickButton(getButtonBox(), ButtonBox.Eight)
.onTrue(leftAlgaeRemove);
// Upper coral removal
// Upper algae removal
new JoystickButton(getButtonBox(), ButtonBox.Four)
.onTrue(rightAlgaeRemove);
@@ -483,7 +494,7 @@ public class RobotContainer {
new JoystickButton(getButtonBox(), ButtonBox.White)
.onTrue(new InstantCommand(() -> {
m_robotElevator.elevatorStop();
m_robotElevator.endefectorStop();
m_robotElevator.endeffectorStop();
m_robotSwerveDrive.endSlowPeriod();
}, m_robotElevator));
@@ -505,12 +516,89 @@ public class RobotContainer {
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
.onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
.onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270)
.onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown()));
} else {
// States: ready/waiting
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator));
//Controller Coral Scoring
new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8)
.onTrue(AprilLidarAlignL4Left);
new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8)
.onTrue(AprilLidarAlignL4Right);
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(AprilLidarAlignL3Left);
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(AprilLidarAlignL3Right);
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(AprilLidarAlignL2Left);
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(AprilLidarAlignL2Right);
//Controller Lower Algae Removal
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
.onTrue(leftAlgaeRemove);
//Controller Upper Algae Removal
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
.onTrue(rightAlgaeRemove);
//Button Box Coral Scoring
new JoystickButton(getButtonBox(), ButtonBox.Five)
.onTrue(AprilLidarAlignL4Left);
new JoystickButton(getButtonBox(), ButtonBox.One)
.onTrue(AprilLidarAlignL4Right);
new JoystickButton(getButtonBox(), ButtonBox.Six)
.onTrue(AprilLidarAlignL3Left);
new JoystickButton(getButtonBox(), ButtonBox.Two)
.onTrue(AprilLidarAlignL3Right);
new JoystickButton(getButtonBox(), ButtonBox.Seven)
.onTrue(AprilLidarAlignL2Left);
new JoystickButton(getButtonBox(), ButtonBox.Three)
.onTrue(AprilLidarAlignL2Right);
// Button Box Lower algae removal
new JoystickButton(getButtonBox(), ButtonBox.Eight)
.onTrue(leftAlgaeRemove);
// Button Box Upper algae removal
new JoystickButton(getButtonBox(), ButtonBox.Four)
.onTrue(rightAlgaeRemove);
// Cancel button
new JoystickButton(getButtonBox(), ButtonBox.White)
.onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endeffectorStop();}, m_robotElevator));
//Manual Controls
m_robotElevator.setDefaultCommand(new RunCommand(() -> {
m_robotElevator.manualEndeffectorVel(getDeadbandedOperatorController().getLeftY());
m_robotElevator.manualElevatorVel(getDeadbandedOperatorController().getRightY());
}, m_robotElevator)
.withName("Default Manual Controls"));
}
// ? /* Programer Buttons (Controller 3)*/
// * /* Auto Recording */
src/main/java/frc4388/robot/RobotMap.java
+1 -1
View File
@@ -57,7 +57,7 @@ public class RobotMap {
public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
public final DigitalInput endefectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
void configureDriveMotorControllers() {}
src/main/java/frc4388/robot/commands/waitElevatorRefrence.java
+1 -1
View File
@@ -31,6 +31,6 @@ public class waitElevatorRefrence extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return elevator.elevatorAtRefrence();
return elevator.elevatorAtReference();
}
}
src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java
+1 -1
View File
@@ -31,6 +31,6 @@ public class waitEndefectorRefrence extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return elevator.endefectorAtRefrence();
return elevator.endeffectorAtReference();
}
}
src/main/java/frc4388/robot/subsystems/Elevator.java
+59 -36
View File
@@ -24,20 +24,23 @@ import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
public class Elevator extends SubsystemBase {
/** Creates a new Elevator. */
private TalonFX elevatorMotor;
private TalonFX endefectorMotor;
private TalonFX endeffectorMotor;
private long wait = 0;
private long maxWait = 1000;
private double elevatorRefrence = 0;
private double endefectorRefrence = 0;
private double endeffectorRefrence = 0;
private boolean elevatorManualStop = true;
private boolean endefectorManualStop = true;
private DigitalInput basinBeamBreak;
private DigitalInput endefectorLimitSwitch;
private DigitalInput endeffectorLimitSwitch;
public enum CoordinationState {
Waiting, // for coral into the though
WatingBeamTriped, //once the beam break trips
WatingBeamTripped, //once the beam break trips
Ready, // Has coral in endefector
Hovering, // Has coral in endefector
L2Score,
@@ -53,18 +56,18 @@ public class Elevator extends SubsystemBase {
private CoordinationState currentState;
public Elevator(TalonFX elevatorTalonFX, TalonFX endefectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endefectorLimitSwitch) {
public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch) {
elevatorMotor = elevatorTalonFX;
endefectorMotor = endefectorTalonFX;
endeffectorMotor = endeffectorTalonFX;
this.basinBeamBreak = basinLimitSwitch;
this.endefectorLimitSwitch = endefectorLimitSwitch;
this.endeffectorLimitSwitch = endeffectorLimitSwitch;
elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
endefectorMotor.setNeutralMode(NeutralModeValue.Brake);
endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
endefectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFECTOR_PID);
endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
currentState = CoordinationState.Ready;
}
@@ -72,7 +75,7 @@ public class Elevator extends SubsystemBase {
private void PIDPosition(TalonFX motor, double position) {
if (motor == elevatorMotor) elevatorRefrence = position;
else endefectorRefrence = position;
else endeffectorRefrence = position;
var request = new PositionDutyCycle(position);
motor.setControl(request);
@@ -82,8 +85,8 @@ public class Elevator extends SubsystemBase {
elevatorMotor.set(0);
}
public void endefectorStop() {
endefectorMotor.set(0);
public void endeffectorStop() {
endeffectorMotor.set(0);
}
public void transitionState(CoordinationState state) {
@@ -92,79 +95,79 @@ public class Elevator extends SubsystemBase {
case Waiting: {
wait = System.currentTimeMillis() + maxWait;
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
break;
}
case WatingBeamTriped: {
case WatingBeamTripped: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
break;
}
case Ready: {
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
break;
}
case Hovering: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
break;
}
case L2Score: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.L2_SCORE_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
case PrimedFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR);
break;
}
case ScoringFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
case PrimedThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
break;
}
case ScoringThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
case BallRemoverL2Primed: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
break;
}
case BallRemoverL2Go: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
case BallRemoverL3Primed: {
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
break;
}
case BallRemoverL3Go: {
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
@@ -175,7 +178,7 @@ public class Elevator extends SubsystemBase {
}
public boolean elevatorAtRefrence() {
public boolean elevatorAtReference() {
// double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
double diffrence = elevatorRefrence - elevatorPosition;
@@ -187,14 +190,14 @@ public class Elevator extends SubsystemBase {
return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
}
public boolean endefectorAtRefrence() {
public boolean endeffectorAtReference() {
// double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble();
double diffrence = endefectorRefrence - endefectorPosition;
double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
double diffrence = endeffectorRefrence - endeffectorPosition;
boolean headedUp = diffrence < 0;
boolean forwardLimit = endefectorMotor.getForwardLimit().asSupplier().get().value == 0;
boolean reverseLimit = endefectorMotor.getReverseLimit().asSupplier().get().value == 0;
boolean forwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0;
boolean reverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0;
return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
}
@@ -215,12 +218,32 @@ public class Elevator extends SubsystemBase {
// }
private void periodicReady() {
if (elevatorMotor.getForwardLimit().asSupplier().get().value == 0)
if (elevatorAtReference())
transitionState(CoordinationState.Hovering);
}
private void periodicScoring() {
if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
if (!endeffectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
}
public void manualElevatorVel(double velocity) {
if (Math.abs(velocity) > 0.1) {
elevatorMotor.set(velocity);
}
if (!elevatorManualStop) {
elevatorManualStop = true;
elevatorMotor.set(0);
}
}
public void manualEndeffectorVel(double velocity) {
if (Math.abs(velocity) > 0.1) {
endeffectorMotor.set(velocity);
}
if (!endefectorManualStop) {
endefectorManualStop = true;
endeffectorMotor.set(0);
}
}
@Override
@@ -228,12 +251,12 @@ public class Elevator extends SubsystemBase {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0);
SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
SmartDashboard.putString("State", currentState.toString());
if (currentState == CoordinationState.Waiting) {
periodicWaiting();
} else if (currentState == CoordinationState.WatingBeamTriped) {
} else if (currentState == CoordinationState.WatingBeamTripped) {
// periodicWaitingTripped();
} else if (currentState == CoordinationState.Ready) {
periodicReady();