mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-09 08:48:04 -06:00
Merge branch 'Functional-LEDS' into Create-TimesNegativeOne
This commit is contained in:
Michael Mikovsky
@@ -419,7 +419,7 @@ public final class Constants {
|
||||
//Max for elevator = 50%
|
||||
|
||||
public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
|
||||
public static final double WAITING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
|
||||
public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe
|
||||
public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
|
||||
public static final double SCORING_THREE_ELEVATOR = -9.25;
|
||||
public static final double DEALGAE_L2_ELEVATOR = -23.5;
|
||||
@@ -437,7 +437,7 @@ public final class Constants {
|
||||
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.5 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double COMPLETLY_TOP_ENDEFECTOR = 0.51 * GEAR_RATIO_ENDEFECTOR;
|
||||
|
||||
public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
|
||||
.withKP(1)
|
||||
|
||||
@@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.DriverStation;
|
||||
import java.io.File;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import edu.wpi.first.cameraserver.CameraServer;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
@@ -41,6 +42,7 @@ import edu.wpi.first.wpilibj2.command.Commands;
|
||||
// Autos
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
import frc4388.robot.commands.GotoLastApril;
|
||||
import frc4388.robot.commands.IfCommand;
|
||||
import frc4388.robot.commands.LidarAlign;
|
||||
import frc4388.robot.commands.MoveForTimeCommand;
|
||||
import frc4388.robot.commands.waitElevatorRefrence;
|
||||
@@ -119,11 +121,15 @@ public class RobotContainer {
|
||||
// true, false);
|
||||
// private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
|
||||
private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT)
|
||||
)),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
|
||||
// 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),
|
||||
@@ -138,35 +144,51 @@ public class RobotContainer {
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), 500, true),
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
|
||||
// new InstantCommand(() -> System.out.println("Soup")),
|
||||
// new WaitCommand(1),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT)
|
||||
)),
|
||||
|
||||
// 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, FieldConstants.L4_DISTANCE_1, Side.LEFT),
|
||||
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), 500, true),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
|
||||
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT),
|
||||
// new InstantCommand(() -> System.out.println("Soup")),
|
||||
// new WaitCommand(1),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT)
|
||||
)),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT),
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
|
||||
@@ -174,13 +196,19 @@ public class RobotContainer {
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
// new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
// new Translation2d(0,1), new Translation2d(), 500, true),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
private Command AprilLidarAlignL3Right = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT)
|
||||
)),
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
@@ -196,11 +224,12 @@ public class RobotContainer {
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
// new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
// new Translation2d(0,1), new Translation2d(), 500, true),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT),
|
||||
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
@@ -209,12 +238,13 @@ public class RobotContainer {
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), 500, true),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator)
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
|
||||
);
|
||||
|
||||
private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT),
|
||||
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
@@ -223,8 +253,8 @@ public class RobotContainer {
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), 500, true),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator)
|
||||
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
private Command placeCoral = new SequentialCommandGroup(
|
||||
@@ -246,7 +276,7 @@ public class RobotContainer {
|
||||
);
|
||||
|
||||
private Command leftAlgaeRemove = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
@@ -255,11 +285,12 @@ public class RobotContainer {
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), 500, true),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator)
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
private Command rightAlgaeRemove = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
@@ -268,7 +299,8 @@ public class RobotContainer {
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), 500, true),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator)
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
|
||||
@@ -449,7 +481,11 @@ public class RobotContainer {
|
||||
|
||||
// Cancel button
|
||||
new JoystickButton(getButtonBox(), ButtonBox.White)
|
||||
.onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator));
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
m_robotElevator.elevatorStop();
|
||||
m_robotElevator.endefectorStop();
|
||||
m_robotSwerveDrive.endSlowPeriod();
|
||||
}, m_robotElevator));
|
||||
|
||||
// Score prep
|
||||
// Prime 4
|
||||
@@ -535,7 +571,7 @@ public class RobotContainer {
|
||||
// return autoCommand;
|
||||
// } catch (Exception e) {
|
||||
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
|
||||
return Commands.none();
|
||||
return autoCommand;
|
||||
// }
|
||||
// return new PathPlannerAuto("Line-up-no-arm");
|
||||
// zach told me to do the below comment
|
||||
@@ -557,9 +593,9 @@ public class RobotContainer {
|
||||
|
||||
autoChooser.onChange((filename) -> {
|
||||
autoCommand = new PathPlannerAuto(filename);
|
||||
System.out.println("Robot Auto Changed");
|
||||
System.out.println("Robot Auto Changed " + filename);
|
||||
});
|
||||
SmartDashboard.putData(autoChooser);
|
||||
// SmartDashboard.putData(autoChooser);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -1,225 +0,0 @@
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,12
|
||||
0.0,0.0,0.0,0.0,26
|
||||
0.0,0.0,0.0,0.0,37
|
||||
0.0,0.0,0.0,0.0,50
|
||||
0.0,0.0,0.0,0.0,62
|
||||
0.0,0.0,0.0,0.0,73
|
||||
0.0,0.0,0.0,0.0,88
|
||||
0.0,0.0,0.0,0.0,103
|
||||
0.0,0.0,0.0,0.0,116
|
||||
0.0,0.0,0.0,0.0,160
|
||||
0.0,0.0,0.0,0.0,173
|
||||
0.0,0.0,0.0,0.0,185
|
||||
0.0,0.0,0.0,0.0,200
|
||||
0.0,0.0,0.0,0.0,211
|
||||
0.0,0.0,0.0,0.0,223
|
||||
0.0,0.0,0.0,0.0,235
|
||||
0.0,0.0,0.0,0.0,247
|
||||
0.0,0.0,0.0,0.0,263
|
||||
0.0,0.0,0.0,0.0,283
|
||||
0.0,0.0,0.0,0.0,303
|
||||
0.0,-0.109375,0.0,0.0,323
|
||||
0.0,-0.1484375,0.0,0.0,343
|
||||
0.0,-0.2109375,0.0,0.0,363
|
||||
0.0,-0.3671875,0.0,0.0,398
|
||||
0.0,-0.4140625,0.0,0.0,411
|
||||
0.0,-0.4765625,0.0,0.0,425
|
||||
0.0,-0.5078125,0.0,0.0,443
|
||||
0.0,-0.5078125,0.0,0.0,463
|
||||
0.0,-0.53125,0.0,0.0,483
|
||||
0.0,-0.5546875,0.0,0.0,504
|
||||
0.0,-0.5625,0.0,0.0,523
|
||||
0.0,-0.5625,0.0,0.0,544
|
||||
0.0,-0.5703125,0.0,0.0,563
|
||||
0.0,-0.5859375,0.0,0.0,584
|
||||
0.0,-0.5859375,0.0,0.0,603
|
||||
0.0,-0.5859375,0.0,0.0,640
|
||||
0.0,-0.59375,0.0,0.0,657
|
||||
0.0,-0.6015625,0.0,0.0,672
|
||||
0.0,-0.6015625,0.0,0.0,685
|
||||
0.0,-0.6015625,0.0,0.0,703
|
||||
0.0,-0.6015625,0.0,0.0,723
|
||||
0.0,-0.6015625,0.0,0.0,743
|
||||
0.0,-0.6015625,0.0,0.0,763
|
||||
0.0,-0.6015625,0.0,0.0,783
|
||||
0.0,-0.6015625,0.0,0.0,803
|
||||
0.0,-0.6015625,0.0,0.0,823
|
||||
0.0,-0.6015625,0.0,0.0,844
|
||||
0.0,-0.6015625,0.0,0.0,878
|
||||
0.0,-0.6015625,0.0,0.0,893
|
||||
0.0,-0.6015625,0.0,0.0,907
|
||||
0.0,-0.6015625,0.0,0.0,924
|
||||
0.0,-0.609375,0.0,0.0,943
|
||||
0.0,-0.609375,0.0,0.0,963
|
||||
0.0,-0.609375,0.0,0.0,983
|
||||
0.0,-0.609375,0.0,0.0,1004
|
||||
0.0,-0.609375,0.0,0.0,1023
|
||||
0.0,-0.609375,0.0,0.0,1043
|
||||
0.0,-0.609375,0.0,0.0,1064
|
||||
0.0,-0.609375,0.0,0.0,1083
|
||||
0.0,-0.609375,0.0,0.0,1156
|
||||
0.0,-0.609375,0.0,0.0,1172
|
||||
0.0,-0.609375,0.0,0.0,1185
|
||||
0.0,-0.609375,0.0,0.0,1200
|
||||
0.0,-0.609375,0.0,0.0,1215
|
||||
0.0,-0.609375,0.0,0.0,1225
|
||||
0.0,-0.609375,0.0,0.0,1236
|
||||
0.0,-0.609375,0.0,0.0,1249
|
||||
0.0,-0.609375,0.0,0.0,1263
|
||||
0.0,-0.609375,0.0,0.0,1283
|
||||
0.0,-0.609375,0.0,0.0,1303
|
||||
0.0,-0.609375,0.0,0.0,1323
|
||||
0.0,-0.609375,0.0,0.0,1363
|
||||
0.0,-0.6015625,0.0,0.0,1376
|
||||
0.0,-0.6015625,0.0,0.0,1394
|
||||
0.0,-0.6015625,0.0,0.0,1405
|
||||
0.0,-0.6015625,0.0,0.0,1423
|
||||
0.0,-0.6015625,0.0,0.0,1443
|
||||
0.0,-0.6015625,0.0,0.0,1463
|
||||
0.0,-0.6015625,0.0,0.0,1483
|
||||
0.0,-0.6015625,0.0,0.0,1503
|
||||
0.0,-0.6015625,0.0,0.0,1523
|
||||
0.0,-0.6015625,0.0,0.0,1543
|
||||
0.0,-0.6015625,0.0,0.0,1563
|
||||
0.0,-0.6015625,0.0,0.0,1597
|
||||
0.0,-0.6015625,0.0,0.0,1608
|
||||
0.0,-0.6015625,0.0,0.0,1624
|
||||
0.0,-0.6015625,0.0,0.0,1643
|
||||
0.0,-0.6015625,0.0,0.0,1664
|
||||
0.0,-0.5859375,0.0,0.0,1683
|
||||
0.0,-0.5859375,0.0,0.0,1703
|
||||
0.0,-0.5625,0.0,0.0,1723
|
||||
0.0,-0.5625,0.0,0.0,1743
|
||||
0.0,-0.5625,0.0,0.0,1763
|
||||
0.0,-0.5625,0.0,0.0,1783
|
||||
0.0,-0.5625,0.0,0.0,1803
|
||||
0.0,-0.5625,0.0,0.0,1843
|
||||
0.0,-0.5625,0.0,0.0,1855
|
||||
0.0,-0.5625,0.0,0.0,1868
|
||||
0.0,-0.5625,0.0,0.0,1883
|
||||
0.0,-0.5625,0.0,0.0,1903
|
||||
0.0,-0.5625,0.0,0.0,1923
|
||||
0.0,-0.5625,0.0,0.0,1943
|
||||
0.0,-0.5625,0.0,0.0,1963
|
||||
0.0,-0.5625,0.0,0.0,1983
|
||||
0.0,-0.5625,0.0,0.0,2003
|
||||
0.0,-0.5625,0.0,0.0,2024
|
||||
0.0,-0.5625,0.0,0.0,2043
|
||||
0.0,-0.5625,0.0,0.0,2081
|
||||
0.0,-0.5625,0.0,0.0,2093
|
||||
0.0,-0.5625,0.0,0.0,2105
|
||||
0.0,-0.5625,0.0,0.0,2123
|
||||
0.0,-0.5625,0.0,0.0,2143
|
||||
0.0,-0.5625,0.0,0.0,2163
|
||||
0.0,-0.5625,0.0,0.0,2183
|
||||
0.0,-0.5625,0.0,0.0,2203
|
||||
0.0,-0.5625,0.0,0.0,2223
|
||||
0.0,-0.5625,0.0,0.0,2243
|
||||
0.0,-0.5625,0.0,0.0,2263
|
||||
0.0,-0.5625,0.0,0.0,2283
|
||||
0.0,-0.5625,0.0,0.0,2366
|
||||
0.0,-0.5625,0.0,0.0,2377
|
||||
0.0,-0.5625,0.0,0.0,2394
|
||||
0.0,-0.5703125,0.0,0.0,2405
|
||||
0.0,-0.5703125,0.0,0.0,2418
|
||||
0.0,-0.5703125,0.0,0.0,2431
|
||||
0.0,-0.5703125,0.0,0.0,2444
|
||||
0.0,-0.5703125,0.0,0.0,2458
|
||||
0.0,-0.5703125,0.0,0.0,2470
|
||||
0.0,-0.5703125,0.0,0.0,2485
|
||||
0.0,-0.5703125,0.0,0.0,2503
|
||||
0.0,-0.5703125,0.0,0.0,2523
|
||||
0.0,-0.5703125,0.0,0.0,2563
|
||||
0.0,-0.5703125,0.0,0.0,2577
|
||||
0.0,-0.5703125,0.0,0.0,2591
|
||||
0.0,-0.5703125,0.0,0.0,2608
|
||||
0.0,-0.5703125,0.0,0.0,2624
|
||||
0.0,-0.5703125,0.0,0.0,2643
|
||||
0.0,-0.5703125,0.0,0.0,2677
|
||||
0.0,-0.5703125,0.0,0.0,2698
|
||||
0.0,-0.5703125,0.0,0.0,2711
|
||||
0.0,-0.5703125,0.0,0.0,2725
|
||||
0.0,-0.5703125,0.0,0.0,2743
|
||||
0.0,-0.5703125,0.0,0.0,2764
|
||||
0.0,-0.5703125,0.0,0.0,2810
|
||||
0.0,-0.5703125,0.0,0.0,2820
|
||||
0.0,-0.5703125,0.0,0.0,2833
|
||||
0.0,-0.5703125,0.0,0.0,2845
|
||||
0.0,-0.5703125,0.0,0.0,2863
|
||||
0.0,-0.5703125,0.0,0.0,2883
|
||||
0.0,-0.5703125,0.0,0.0,2904
|
||||
0.0,-0.5703125,0.0,0.0,2924
|
||||
0.0,-0.5703125,0.0,0.0,2943
|
||||
0.0,-0.5703125,0.0,0.0,2963
|
||||
0.0,-0.5703125,0.0,0.0,2983
|
||||
0.0,-0.5703125,0.0,0.0,3003
|
||||
0.0,-0.5703125,0.0,0.0,3033
|
||||
0.0,-0.5703125,0.0,0.0,3050
|
||||
0.0,-0.5703125,0.0,0.0,3065
|
||||
0.0,-0.5703125,0.0,0.0,3083
|
||||
0.0,-0.5703125,0.0,0.0,3103
|
||||
0.0,-0.5703125,0.0,0.0,3123
|
||||
0.0,-0.5703125,0.0,0.0,3144
|
||||
0.0,-0.5703125,0.0,0.0,3164
|
||||
0.0,-0.5703125,0.0,0.0,3184
|
||||
0.0,-0.5703125,0.0,0.0,3203
|
||||
0.0,-0.5703125,0.0,0.0,3223
|
||||
0.0,-0.5703125,0.0,0.0,3243
|
||||
0.0,-0.5703125,0.0,0.0,3272
|
||||
0.0,-0.5703125,0.0,0.0,3289
|
||||
0.0,-0.5703125,0.0,0.0,3303
|
||||
0.0,-0.5703125,0.0,0.0,3323
|
||||
0.0,-0.5703125,0.0,0.0,3343
|
||||
0.0,-0.5703125,0.0,0.0,3363
|
||||
0.0,-0.5703125,0.0,0.0,3383
|
||||
0.0,-0.5703125,0.0,0.0,3403
|
||||
0.0,-0.5703125,0.0,0.0,3423
|
||||
0.0,-0.5703125,0.0,0.0,3443
|
||||
0.0,-0.5703125,0.0,0.0,3463
|
||||
0.0,-0.5703125,0.0,0.0,3483
|
||||
0.0,-0.5703125,0.0,0.0,3566
|
||||
0.0,-0.5703125,0.0,0.0,3578
|
||||
0.0,-0.5703125,0.0,0.0,3596
|
||||
0.0,-0.5703125,0.0,0.0,3610
|
||||
0.0,-0.5703125,0.0,0.0,3623
|
||||
0.0,-0.5703125,0.0,0.0,3640
|
||||
0.0,-0.5703125,0.0,0.0,3651
|
||||
0.0,-0.5703125,0.0,0.0,3663
|
||||
0.0,-0.5703125,0.0,0.0,3678
|
||||
0.0,-0.5703125,0.0,0.0,3691
|
||||
0.0,-0.5703125,0.0,0.0,3706
|
||||
0.0,-0.5703125,0.0,0.0,3723
|
||||
0.0,-0.5703125,0.0,0.0,3766
|
||||
0.0,-0.5703125,0.0,0.0,3778
|
||||
0.0,-0.5703125,0.0,0.0,3792
|
||||
0.0,-0.5703125,0.0,0.0,3807
|
||||
0.0,-0.5703125,0.0,0.0,3823
|
||||
0.0,-0.5703125,0.0,0.0,3843
|
||||
0.0,-0.53125,0.0,0.0,3863
|
||||
0.0,-0.53125,0.0,0.0,3884
|
||||
0.0,-0.421875,0.0,0.0,3904
|
||||
0.0,0.0,0.0,0.0,3924
|
||||
0.0,0.0,0.0,0.0,3944
|
||||
0.0,0.0,0.0,0.0,3963
|
||||
0.0,0.0,0.0,0.0,3999
|
||||
0.0,0.0,0.0,0.0,4010
|
||||
0.0,0.0,0.0,0.0,4025
|
||||
0.0,0.0,0.0,0.0,4043
|
||||
0.0,0.0,0.0,0.0,4063
|
||||
0.0,0.0,0.0,0.0,4083
|
||||
0.0,0.0,0.0,0.0,4103
|
||||
0.0,0.0,0.0,0.0,4123
|
||||
0.0,0.0,0.0,0.0,4143
|
||||
0.0,0.0,0.0,0.0,4163
|
||||
0.0,0.0,0.0,0.0,4183
|
||||
0.0,0.0,0.0,0.0,4203
|
||||
0.0,0.0,0.0,0.0,4236
|
||||
0.0,0.0,0.0,0.0,4247
|
||||
0.0,0.0,0.0,0.0,4264
|
||||
0.0,0.0,0.0,0.0,4284
|
||||
0.0,0.0,0.0,0.0,4304
|
||||
0.0,0.0,0.0,0.0,4324
|
||||
0.0,0.0,0.0,0.0,4343
|
||||
0.0,0.0,0.0,0.0,4363
|
||||
@@ -0,0 +1,33 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.time.Instant;
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
|
||||
// Command that is called if something is true
|
||||
public class IfCommand extends InstantCommand {
|
||||
BooleanSupplier isTrue;
|
||||
Command trueCommand;
|
||||
Command falseCommand;
|
||||
|
||||
public IfCommand(BooleanSupplier isTrue, Command trueCommand, Command falseCommand){
|
||||
this.isTrue = isTrue;
|
||||
this.trueCommand = trueCommand;
|
||||
this.falseCommand = falseCommand;
|
||||
}
|
||||
|
||||
public IfCommand(BooleanSupplier isTrue, Command trueCommand){
|
||||
this(isTrue, trueCommand, Commands.none());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
if(isTrue.getAsBoolean())
|
||||
trueCommand.schedule();
|
||||
else
|
||||
falseCommand.schedule();
|
||||
}
|
||||
}
|
||||
@@ -178,20 +178,25 @@ public class Elevator extends SubsystemBase {
|
||||
public boolean elevatorAtRefrence() {
|
||||
// double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
|
||||
double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
|
||||
boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.5;
|
||||
if (atPos) {
|
||||
SmartDashboard.putNumber("Elevator Refrence", elevatorRefrence);
|
||||
SmartDashboard.putNumber("Elevator Pos", elevatorPosition);
|
||||
}
|
||||
double diffrence = elevatorRefrence - elevatorPosition;
|
||||
|
||||
return atPos;
|
||||
boolean headedUp = diffrence < 0;
|
||||
boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
|
||||
boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
|
||||
|
||||
return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
|
||||
}
|
||||
|
||||
|
||||
public boolean endefectorAtRefrence() {
|
||||
// double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
|
||||
double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble();
|
||||
double diffrence = endefectorRefrence - endefectorPosition;
|
||||
|
||||
return Math.abs(endefectorRefrence - endefectorPosition) <= 0.2;
|
||||
boolean headedUp = diffrence < 0;
|
||||
boolean forwardLimit = endefectorMotor.getForwardLimit().asSupplier().get().value == 0;
|
||||
boolean reverseLimit = endefectorMotor.getReverseLimit().asSupplier().get().value == 0;
|
||||
|
||||
return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
|
||||
}
|
||||
// public void driveElevatorStick(Translation2d stick) {
|
||||
// if (stick.getNorm() > 0.05) {
|
||||
@@ -210,7 +215,7 @@ public class Elevator extends SubsystemBase {
|
||||
// }
|
||||
|
||||
private void periodicReady() {
|
||||
if (elevatorAtRefrence())
|
||||
if (elevatorMotor.getForwardLimit().asSupplier().get().value == 0)
|
||||
transitionState(CoordinationState.Hovering);
|
||||
}
|
||||
|
||||
@@ -227,7 +232,7 @@ public class Elevator extends SubsystemBase {
|
||||
SmartDashboard.putString("State", currentState.toString());
|
||||
|
||||
if (currentState == CoordinationState.Waiting) {
|
||||
// periodicWaiting();
|
||||
periodicWaiting();
|
||||
} else if (currentState == CoordinationState.WatingBeamTriped) {
|
||||
// periodicWaitingTripped();
|
||||
} else if (currentState == CoordinationState.Ready) {
|
||||
@@ -238,6 +243,14 @@ public class Elevator extends SubsystemBase {
|
||||
// }
|
||||
}
|
||||
|
||||
public boolean isL4Primed(){
|
||||
return currentState == CoordinationState.PrimedFour;
|
||||
}
|
||||
|
||||
public boolean isL3Primed(){
|
||||
return currentState == CoordinationState.PrimedThree;
|
||||
}
|
||||
|
||||
public void armShuffle(){
|
||||
if(!basinBeamBreak.get()){
|
||||
//shuffle the coral with the arm until coral hits beam break
|
||||
|
||||
@@ -72,7 +72,7 @@ public class Lidar extends Subsystem {
|
||||
if(distance == -1){
|
||||
s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED");
|
||||
}else{
|
||||
s.addReport(ReportLevel.ERROR, "LIDAR CONNECTED");
|
||||
s.addReport(ReportLevel.INFO, "LIDAR CONNECTED");
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
@@ -380,6 +380,18 @@ public class SwerveDrive extends Subsystem {
|
||||
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
|
||||
}
|
||||
|
||||
private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR;
|
||||
|
||||
public void startSlowPeriod() {
|
||||
tmp_gear_index = gear_index;
|
||||
setToSlow();
|
||||
}
|
||||
|
||||
public void endSlowPeriod() {
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
|
||||
gear_index = tmp_gear_index;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
return "Swerve Drive Controller";
|
||||
|
||||
@@ -0,0 +1,6 @@
|
||||
package frc4388.utility;
|
||||
|
||||
// A very stupid class to hold the state of the swerve speed setting for a short period of time
|
||||
public class SlowPeriod {
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user