mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-08 16:28:04 -06:00
Merge branch 'master' into Functional-LEDS
This commit is contained in:
C4llSiqn
@@ -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;
|
||||
@@ -437,21 +437,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);
|
||||
|
||||
@@ -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, m_robotLED);
|
||||
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED);
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
|
||||
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
|
||||
|
||||
@@ -396,121 +397,208 @@ public class RobotContainer {
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
|
||||
boolean driverPreferenceControls = false;
|
||||
|
||||
// ? /* Driver Buttons */
|
||||
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
||||
|
||||
// ! /* Speed */
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
if (!driverPreferenceControls) {
|
||||
|
||||
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode()));
|
||||
// ! /* Speed */
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
|
||||
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
|
||||
|
||||
|
||||
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
|
||||
} else {
|
||||
|
||||
}
|
||||
|
||||
// ? /* Operator Buttons */
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar));
|
||||
if (!driverPreferenceControls) {
|
||||
|
||||
|
||||
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));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar));
|
||||
|
||||
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);
|
||||
|
||||
|
||||
// Lower coral removal
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Eight)
|
||||
.onTrue(leftAlgaeRemove);
|
||||
|
||||
// Upper coral removal
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Four)
|
||||
.onTrue(rightAlgaeRemove);
|
||||
|
||||
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));
|
||||
|
||||
|
||||
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);
|
||||
|
||||
|
||||
// Lower algae removal
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Eight)
|
||||
.onTrue(leftAlgaeRemove);
|
||||
|
||||
// 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.endefectorStop();
|
||||
m_robotElevator.endeffectorStop();
|
||||
m_robotSwerveDrive.endSlowPeriod();
|
||||
}, m_robotElevator));
|
||||
|
||||
// Score prep
|
||||
// Prime 4
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator));
|
||||
// Score prep
|
||||
// Prime 4
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator));
|
||||
|
||||
// Prime 3
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator));
|
||||
// Prime 3
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator));
|
||||
|
||||
|
||||
|
||||
//Trims
|
||||
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp()));
|
||||
|
||||
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()));
|
||||
|
||||
|
||||
//Trims
|
||||
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp()));
|
||||
|
||||
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 */
|
||||
|
||||
@@ -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() {}
|
||||
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -28,21 +28,24 @@ import frc4388.utility.TimesNegativeOne;
|
||||
public class Elevator extends SubsystemBase {
|
||||
/** Creates a new Elevator. */
|
||||
private TalonFX elevatorMotor;
|
||||
private TalonFX endefectorMotor;
|
||||
private TalonFX endeffectorMotor;
|
||||
private LED led;
|
||||
|
||||
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,
|
||||
@@ -58,19 +61,19 @@ public class Elevator extends SubsystemBase {
|
||||
|
||||
private CoordinationState currentState;
|
||||
|
||||
public Elevator(TalonFX elevatorTalonFX, TalonFX endefectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endefectorLimitSwitch, LED led) {
|
||||
public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
|
||||
elevatorMotor = elevatorTalonFX;
|
||||
endefectorMotor = endefectorTalonFX;
|
||||
endeffectorMotor = endeffectorTalonFX;
|
||||
this.led = led;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -78,7 +81,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);
|
||||
@@ -88,8 +91,8 @@ public class Elevator extends SubsystemBase {
|
||||
elevatorMotor.set(0);
|
||||
}
|
||||
|
||||
public void endefectorStop() {
|
||||
endefectorMotor.set(0);
|
||||
public void endeffectorStop() {
|
||||
endeffectorMotor.set(0);
|
||||
}
|
||||
|
||||
|
||||
@@ -99,90 +102,90 @@ 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);
|
||||
led.setMode(LEDConstants.WAITING_PATTERN);
|
||||
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);
|
||||
led.setMode(LEDConstants.DOWN_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case Ready: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
|
||||
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.DOWN_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case Hovering: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
|
||||
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.READY_PATTERN);
|
||||
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());
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
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);
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
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());
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
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);
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
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());
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
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);
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
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());
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
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());
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
break;
|
||||
}
|
||||
@@ -194,7 +197,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;
|
||||
@@ -205,15 +208,15 @@ 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));
|
||||
}
|
||||
@@ -234,12 +237,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
|
||||
@@ -247,12 +270,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();
|
||||
|
||||
Reference in New Issue
Block a user