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

end of day one

Browse Source
This commit is contained in:
C4llSiqn
2025-03-18 11:02:39 -06:00
parent 1f6a077517
commit 4747f6233b
13 changed files with 243 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto
+13 -1
View File
@@ -19,7 +19,7 @@
{
"type": "named",
"data": {
"name": "place-coral-left-l4"
"name": "place-coral-right-l4"
}
},
{
@@ -46,6 +46,12 @@
"pathName": "Bottom Feed to Reef 2"
}
},
{
"type": "named",
"data": {
"name": "await-coral"
}
},
{
"type": "named",
"data": {
@@ -76,6 +82,12 @@
"pathName": "Bottom Feed to Reef 2"
}
},
{
"type": "named",
"data": {
"name": "await-coral"
}
},
{
"type": "named",
"data": {
src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path
+1 -1
View File
@@ -41,7 +41,7 @@
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"velocity": 0.0,
"rotation": 59.28109573597083
},
"reversed": false,
src/main/deploy/pathplanner/paths/Cage 5 to Reef.path
+10 -21
View File
@@ -8,20 +8,20 @@
},
"prevControl": null,
"nextControl": {
"x": 5.959532828282828,
"y": 1.5754419191919191
"x": 6.670197826108406,
"y": 2.0396270358261894
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 5.349684343434343,
"y": 2.2869318181818175
"x": 5.754837328767123,
"y": 2.492380136986301
},
"prevControl": {
"x": 5.747433712121213,
"y": 1.9321212121212112
"x": 6.077263063386753,
"y": 2.432264238502225
},
"nextControl": null,
"isLocked": false,
@@ -30,18 +30,7 @@
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [
{
"fieldPosition": {
"x": 4.8,
"y": 3.0
},
"rotationOffset": 0.0,
"minWaypointRelativePos": 0.0,
"maxWaypointRelativePos": 1.0,
"name": "Point Towards Zone"
}
],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
@@ -52,14 +41,14 @@
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 122.79876573246848
"velocity": 1.0,
"rotation": 119.99999999999999
},
"reversed": false,
"folder": "Barge to Reef",
"idealStartingState": {
"velocity": 0,
"rotation": 126.34745820888533
"rotation": 123.5866498700801
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path
+1 -1
View File
@@ -41,7 +41,7 @@
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"velocity": 1.0,
"rotation": 55.37584492005105
},
"reversed": false,
src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path
+8 -8
View File
@@ -8,8 +8,8 @@
},
"prevControl": null,
"nextControl": {
"x": 4.703267045454546,
"y": 1.075625
"x": 4.106435503617635,
"y": 1.8391383607824139
},
"isLocked": false,
"linkedName": null
@@ -20,8 +20,8 @@
"y": 1.3559375
},
"prevControl": {
"x": 2.0014457070707063,
"y": 2.5936426767676766
"x": 2.9441094150144833,
"y": 1.668218251495801
},
"nextControl": null,
"isLocked": false,
@@ -33,15 +33,15 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 5.0,
"maxAcceleration": 5.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"velocity": 1.0,
"rotation": 55.05578949900953
},
"reversed": false,
@@ -50,5 +50,5 @@
"velocity": 0,
"rotation": 121.42956561483854
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path
+1 -1
View File
@@ -41,7 +41,7 @@
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"velocity": 1.0,
"rotation": 52.857102599919905
},
"reversed": false,
src/main/java/frc4388/robot/Constants.java
+31 -12
View File
@@ -54,6 +54,7 @@ import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.Trim;
import frc4388.utility.configurable.ConfigurableDouble;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -193,7 +194,7 @@ public final class Constants {
.withKS(0).withKV(0.124);
public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(2.5, 0.2, 1);
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1);
}
public static final class Configurations {
@@ -298,7 +299,11 @@ public final class Constants {
}
public static final class AutoConstants {
public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
// public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
public static final Gains XY_GAINS = new Gains(8,0,0.0);
// public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
// public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
// public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
// public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
@@ -316,24 +321,37 @@ public final class Constants {
public static final double MIN_XY_PID_OUTPUT = 0.0;
public static final double MIN_ROT_PID_OUTPUT = 0.0;
public static final double VELOCITY_THRESHHOLD = 0.005;
public static final double VELOCITY_THRESHHOLD = 0.01;
// X is tangent to reef side
// Y is normal to reef side
public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field
public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1);
public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18);
public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
// public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP;
// public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5);
public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1);
public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6);
public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5);
public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE;
// public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
// public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
// // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
// public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
// public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
// public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
// public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
// public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
public static final int L4_DRIVE_TIME = 250; //Milliseconds
// public static final int L3_DRIVE_TIME = 500;
@@ -437,7 +455,8 @@ public final class Constants {
public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * 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 SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR;
public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
src/main/java/frc4388/robot/RobotContainer.java
+97 -5
View File
@@ -38,6 +38,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
// Commands
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
@@ -49,6 +50,7 @@ import frc4388.robot.commands.DriveUntilLiDAR;
import frc4388.robot.commands.GotoLastApril;
import frc4388.robot.commands.LidarAlign;
import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.MoveUntilSuply;
import frc4388.robot.commands.WhileTrueCommand;
import frc4388.robot.commands.waitElevatorRefrence;
import frc4388.robot.commands.waitEndefectorRefrence;
@@ -148,6 +150,48 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
waitDebuger.asProxy(),
new ParallelDeadlineGroup(
new WaitCommand(1),
new MoveUntilSuply(
m_robotSwerveDrive,
new Translation2d(0,-0.5),
new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
),
new InstantCommand(m_robotSwerveDrive::softStop, m_robotElevator),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
new ConditionalCommand(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
() -> m_robotElevator.hasCoral()),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
/* private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
@@ -204,6 +248,53 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
); */
private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
waitDebuger.asProxy(),
new ParallelDeadlineGroup(
new WaitCommand(1),
new MoveUntilSuply(
m_robotSwerveDrive,
new Translation2d(0,-0.5),
new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
),
new InstantCommand(m_robotSwerveDrive::softStop, m_robotElevator),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
new ConditionalCommand(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
() -> m_robotElevator.hasCoral()),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
@@ -408,7 +499,7 @@ public class RobotContainer {
new Translation2d(0, 1),
new Translation2d(), 300, true
), new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)),
), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)),
new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
@@ -421,7 +512,7 @@ public class RobotContainer {
NamedCommands.registerCommand("prepare-l4", new SequentialCommandGroup(
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Hovering)),
// new waitElevatorRefrence(m_robotElevator),
new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)),
// new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour))
));
@@ -545,14 +636,15 @@ public class RobotContainer {
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new DriveUntilLiDAR(m_robotSwerveDrive,
new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
.onTrue( new DriveUntilLiDAR(m_robotSwerveDrive,
new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
// .onTrue(new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.HALF_ROBOT_SIZE, Side.CENTER, true));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(thrustIntake.asProxy());
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_reefLidar));
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_reefLidar));
// ? /* Operator Buttons */
src/main/java/frc4388/robot/RobotMap.java
+4 -1
View File
@@ -55,11 +55,14 @@ public class RobotMap {
/* Elevator Subsystem */
public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
void configureDriveMotorControllers() {}
void configureDriveMotorControllers() {
// endeffector.saf
}
}
src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java
+5 -1
View File
@@ -45,6 +45,10 @@ public class DriveUntilLiDAR extends Command {
@Override
public boolean isFinished() {
return Math.abs(m_lidar.getDistance()) < mindistance;
if (Math.abs(m_lidar.getDistance()) < mindistance) {
swerveDrive.softStop();
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/GotoLastApril.java
+19 -2
View File
@@ -50,8 +50,8 @@ public class GotoLastApril extends Command {
this.vision = vision;
this.distance = distance;
this.side = side;
this.waitVelocityZero = waitVelocityZero;
// addRequirements(swerveDrive);
this.waitVelocityZero = waitVelocityZero && false;
addRequirements(swerveDrive);
}
@@ -62,6 +62,23 @@ public class GotoLastApril extends Command {
@Override
public void initialize() {
// double kP = AutoConstants.P_XY_GAINS.get();
// double kI = AutoConstants.I_XY_GAINS.get();
// double kD = AutoConstants.D_XY_GAINS.get();
// xPID = new PID(new Gains(
// kP,
// kI,
// kD),
// 0);
// yPID = new PID(new Gains(
// kP,
// kI,
// kD),
// 0);
// System.out.println("kP: "+kP);
// System.out.println("kI: "+kI);
// System.out.println("kD: "+kD);
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(
src/main/java/frc4388/robot/commands/MoveUntilSuply.java
+47
View File
@@ -0,0 +1,47 @@
package frc4388.robot.commands;
import java.time.Instant;
import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.TimesNegativeOne;
// Command to repeat a joystick movement for a specific time.
public class MoveUntilSuply extends Command {
private final SwerveDrive swerveDrive;
private final Translation2d leftStick;
private final Translation2d rightStick;
private final Supplier<Boolean> truth;
private final boolean robotRelative;
public MoveUntilSuply(
SwerveDrive swerveDrive,
Translation2d leftStick,
Translation2d rightStick,
Supplier<Boolean> truth,
boolean robotRelative) {
addRequirements(swerveDrive);
this.swerveDrive = swerveDrive;
this.leftStick = leftStick;
this.rightStick = rightStick;
this.truth = truth;
this.robotRelative = robotRelative;
}
@Override
public void initialize() {
}
@Override
public void execute() {
swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
}
@Override
public boolean isFinished() {
return truth.get();
}
}
src/main/java/frc4388/robot/subsystems/Elevator.java
+6 -2
View File
@@ -162,7 +162,7 @@ public class Elevator extends Subsystem {
case PrimedFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
@@ -255,6 +255,10 @@ public class Elevator extends Subsystem {
// }
// }
public boolean getEndeffectorLimit() {
return endeffectorLimitSwitch.get();
}
private void periodicWaiting() {
if (!basinBeamBreak.get())
transitionState(CoordinationState.Ready);
@@ -363,7 +367,7 @@ public class Elevator extends Subsystem {
}
public boolean hasCoral() {
return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get();
return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get();
}
public boolean readyToMove() {