mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-08 16:28:04 -06:00
Merge pull request #31 from Team4388/after-cougars-scrimmage
After cougars scrimmage
This commit is contained in:
Michael Mikovsky
Vendored
+2
-1
@@ -57,5 +57,6 @@
|
||||
"edu.wpi.first.math.**.proto.*",
|
||||
"edu.wpi.first.math.**.struct.*",
|
||||
],
|
||||
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
|
||||
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable",
|
||||
"wpilib.autoStartRioLog": false
|
||||
}
|
||||
|
||||
@@ -1,4 +1,9 @@
|
||||
{
|
||||
"System Joysticks": {
|
||||
"window": {
|
||||
"enabled": false
|
||||
}
|
||||
},
|
||||
"keyboardJoysticks": [
|
||||
{
|
||||
"axisConfig": [
|
||||
|
||||
@@ -0,0 +1,25 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Blue Cage to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "lineup-no-arm"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -8,20 +8,20 @@
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 7.270707070707072,
|
||||
"y": 7.013257575757576
|
||||
"x": 6.599873737373737,
|
||||
"y": 8.029671717171716
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.309027777777779,
|
||||
"y": 5.3768308080808085
|
||||
"x": 5.464431818181818,
|
||||
"y": 5.6403977272727275
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 4.945156854248459,
|
||||
"y": 5.08761998976538
|
||||
"x": 5.240820707070706,
|
||||
"y": 5.254160353535354
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -42,13 +42,13 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 60.255118703057796
|
||||
"rotation": -122.99770510121631
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -2.223961240385466
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -106,7 +106,7 @@ public final class Constants {
|
||||
|
||||
public static final boolean DRIFT_CORRECTION_ENABLED = true;
|
||||
public static final boolean INVERT_X = false;
|
||||
public static final boolean INVERT_Y = false;
|
||||
public static final boolean INVERT_Y = true;
|
||||
public static final boolean INVERT_ROTATION = false;
|
||||
|
||||
// public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
|
||||
@@ -121,8 +121,8 @@ public final class Constants {
|
||||
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Front Right
|
||||
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.62841796875-0.25);
|
||||
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.62841796875-0.5);
|
||||
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
|
||||
@@ -234,15 +234,18 @@ public final class Constants {
|
||||
public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
|
||||
public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
|
||||
|
||||
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0);
|
||||
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 int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
|
||||
public static final int LIDAR_DIO_CHANNEL = 2;
|
||||
public static final int LIDAR_DIO_CHANNEL = 7;
|
||||
public static final int LIDAR_MICROS_TO_CM = 10;
|
||||
public static final int SECONDS_TO_MICROS = 1000000;
|
||||
|
||||
public static final double XY_TOLERANCE = 0.07; // Meters
|
||||
public static final double ROT_TOLERANCE = 1; // Degrees
|
||||
public static final double ROT_TOLERANCE = 5; // Degrees
|
||||
|
||||
// public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
|
||||
// public static final Pose2d targetpos =
|
||||
@@ -343,8 +346,8 @@ public final class Constants {
|
||||
public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT";
|
||||
public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
|
||||
|
||||
public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI));
|
||||
public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI));
|
||||
public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
|
||||
public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
|
||||
|
||||
public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters
|
||||
|
||||
@@ -358,11 +361,21 @@ public final class Constants {
|
||||
public static final class FieldConstants {
|
||||
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
|
||||
// public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(9.5);
|
||||
public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5);
|
||||
;;;;;;;;;;;;;;;;;;;;;;;;;;;;
|
||||
|
||||
public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5);
|
||||
|
||||
// public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
|
||||
|
||||
public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20);
|
||||
public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(17);
|
||||
public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
|
||||
public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
|
||||
public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
|
||||
public static final double L3_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
|
||||
|
||||
public static final double L2_ALGAE_REMOVAL = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(3);
|
||||
|
||||
|
||||
public static final double L2_SCORE_DISTANCE = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(1);
|
||||
|
||||
|
||||
|
||||
@@ -389,7 +402,8 @@ public final class Constants {
|
||||
public static final class OIConstants {
|
||||
public static final int XBOX_DRIVER_ID = 0;
|
||||
public static final int XBOX_OPERATOR_ID = 1;
|
||||
public static final int XBOX_PROGRAMMER_ID = 2;
|
||||
public static final int BUTTONBOX_ID = 2;
|
||||
public static final int XBOX_PROGRAMMER_ID = 3;
|
||||
public static final double LEFT_AXIS_DEADBAND = 0.1;
|
||||
|
||||
}
|
||||
@@ -398,20 +412,31 @@ public final class Constants {
|
||||
public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
|
||||
public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
|
||||
|
||||
public static final int BASIN_LIMIT_SWITCH = 0; // TODO: FIND
|
||||
public static final int ENDEFFECTOR_LIMIT_SWITCH = 1; // TODO: FIND
|
||||
public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
|
||||
public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
|
||||
|
||||
public static final double GEAR_RATIO_ELEVATOR = -9.0;
|
||||
//Max for elevator = 50%
|
||||
|
||||
public static final double GEAR_RATIO_ELEVATOR = 36.0;
|
||||
|
||||
public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
|
||||
public static final double WAITING_POSITION_ELEVATOR = 2 * GEAR_RATIO_ELEVATOR; // TODO: find 4-6 off the ground
|
||||
public static final double MAX_POSITION_ELEVATOR = 20 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
|
||||
public static final double WAITING_POSITION_ELEVATOR = -7.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;
|
||||
public static final double DEALGAE_L3_ELEVATOR = -33.75;
|
||||
public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
|
||||
|
||||
public static final double GEAR_RATIO_ENDEFECTOR = -100.0;
|
||||
public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
|
||||
//Max for endefector = 60%
|
||||
|
||||
public static final double GEAR_RATIO_ENDEFECTOR = 100.0;
|
||||
public static final double L2_SCORE_ENDEFECTOR = -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 SCORING_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
|
||||
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 Slot0Configs ELEVATOR_PID = new Slot0Configs()
|
||||
|
||||
@@ -16,11 +16,14 @@ import java.util.List;
|
||||
|
||||
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.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
import frc4388.utility.controller.ButtonBox;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.robot.Constants.FieldConstants;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
|
||||
@@ -40,6 +43,8 @@ import frc4388.utility.controller.VirtualController;
|
||||
import frc4388.robot.commands.GotoLastApril;
|
||||
import frc4388.robot.commands.LidarAlign;
|
||||
import frc4388.robot.commands.MoveForTimeCommand;
|
||||
import frc4388.robot.commands.waitElevatorRefrence;
|
||||
import frc4388.robot.commands.waitEndefectorRefrence;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
|
||||
|
||||
@@ -85,7 +90,8 @@ public class RobotContainer {
|
||||
|
||||
/* Controllers */
|
||||
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
|
||||
private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID);
|
||||
|
||||
/* Virtual Controllers */
|
||||
@@ -109,25 +115,114 @@ public class RobotContainer {
|
||||
// () -> autoplaybackName.get(), // lastAutoName
|
||||
// new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
// true, false);
|
||||
private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
|
||||
private Command AprilLidarAlign = new SequentialCommandGroup(
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision),
|
||||
new InstantCommand(() -> System.out.println("Soup")),
|
||||
new WaitCommand(1),
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar)
|
||||
);
|
||||
private Command AprilLidarLeft = new SequentialCommandGroup(
|
||||
AutoGotoPosition.asProxy(),
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar)
|
||||
// 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_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, FieldConstants.L4_DISTANCE_1, Side.RIGHT),
|
||||
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)
|
||||
);
|
||||
|
||||
private Command AprilLidarRight = new SequentialCommandGroup(
|
||||
AutoGotoPosition.asProxy(),
|
||||
new InstantCommand(() -> System.out.println("Soup")),
|
||||
new WaitCommand(1),
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar)//,
|
||||
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 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)
|
||||
);
|
||||
|
||||
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 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),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
// new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
// new Translation2d(0, 0.5), new Translation2d(), 1000, true)
|
||||
// new Translation2d(0,1), new Translation2d(), 500, true),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
|
||||
);
|
||||
|
||||
private Command AprilLidarAlignL3Right = 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.RIGHT),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT),
|
||||
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
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)
|
||||
);
|
||||
|
||||
private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT),
|
||||
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
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)
|
||||
|
||||
);
|
||||
|
||||
private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT),
|
||||
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
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)
|
||||
|
||||
);
|
||||
|
||||
private Command placeCoral = new SequentialCommandGroup(
|
||||
@@ -147,15 +242,52 @@ public class RobotContainer {
|
||||
new WaitCommand(2),
|
||||
new InstantCommand(() -> System.out.println("Done yoinking some coral..."))
|
||||
);
|
||||
|
||||
private Command leftAlgaeRemove = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT),
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
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)
|
||||
);
|
||||
|
||||
private Command rightAlgaeRemove = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT),
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
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)
|
||||
);
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
|
||||
NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition);
|
||||
NamedCommands.registerCommand("april-allign", AprilLidarAlign);
|
||||
NamedCommands.registerCommand("place-coral", placeCoral);
|
||||
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
|
||||
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
|
||||
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL3Left);
|
||||
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL3Right);
|
||||
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL2Left);
|
||||
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL2Right);
|
||||
|
||||
NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup(
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar)
|
||||
));
|
||||
|
||||
NamedCommands.registerCommand("grab-coral", grabCoral);
|
||||
|
||||
configureButtonBindings();
|
||||
@@ -175,6 +307,12 @@ public class RobotContainer {
|
||||
}, m_robotSwerveDrive)
|
||||
.withName("SwerveDrive DefaultCommand"));
|
||||
m_robotSwerveDrive.setToSlow();
|
||||
// m_robotElevator.setDefaultCommand(new RunCommand(() -> {
|
||||
// m_robotElevator.driveElevatorStick(m_operatorXbox.getRight());
|
||||
// }, m_robotElevator)
|
||||
// .withName("Elevator"));
|
||||
|
||||
|
||||
makeAutoChooser();
|
||||
SmartDashboard.putData("Auto Chooser", autoChooser);
|
||||
// this.subsystems.add(m_robotSwerveDrive);
|
||||
@@ -220,54 +358,116 @@ public class RobotContainer {
|
||||
private void configureButtonBindings() {
|
||||
|
||||
// ? /* Driver Buttons */
|
||||
|
||||
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
||||
|
||||
// ! /* Speed */
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) // final
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) // final
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
|
||||
|
||||
|
||||
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() == 90)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
|
||||
|
||||
// ? /* Operator Buttons */
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
|
||||
// .onTrue(AutoGotoPosition);
|
||||
.onTrue(AprilLidarRight);
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar));
|
||||
|
||||
|
||||
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
|
||||
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator));
|
||||
|
||||
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
|
||||
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator));
|
||||
|
||||
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator));
|
||||
|
||||
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), 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 coral removal
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Eight)
|
||||
.onTrue(leftAlgaeRemove);
|
||||
|
||||
// Upper coral 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));
|
||||
|
||||
// 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));
|
||||
|
||||
|
||||
|
||||
//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()));
|
||||
|
||||
// ? /* Programer Buttons (Controller 3)*/
|
||||
|
||||
@@ -320,16 +520,18 @@ public class RobotContainer {
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
|
||||
|
||||
//return autoPlayback;
|
||||
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
|
||||
//return autoChooser.getSelected();
|
||||
// try{
|
||||
// // Load the path you want to follow using its name in the GUI
|
||||
return autoCommand;
|
||||
// // // Load the path you want to follow using its name in the GUI
|
||||
// return autoCommand;
|
||||
// } catch (Exception e) {
|
||||
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
|
||||
// return Commands.none();
|
||||
return Commands.none();
|
||||
// }
|
||||
// return new PathPlannerAuto("Line-up-no-arm");
|
||||
// zach told me to do the below comment
|
||||
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
|
||||
// return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
|
||||
@@ -373,6 +575,10 @@ public class RobotContainer {
|
||||
return this.m_operatorXbox;
|
||||
}
|
||||
|
||||
public ButtonBox getButtonBox() {
|
||||
return this.m_buttonBox;
|
||||
}
|
||||
|
||||
public VirtualController getVirtualDriverController() {
|
||||
return m_virtualDriver;
|
||||
}
|
||||
|
||||
@@ -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
|
||||
@@ -1,8 +1,13 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.util.Optional;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
|
||||
@@ -28,15 +33,19 @@ public class GotoLastApril extends Command {
|
||||
|
||||
SwerveDrive swerveDrive;
|
||||
Vision vision;
|
||||
double distance;
|
||||
Side side;
|
||||
|
||||
/**
|
||||
* Command to drive robot to position.
|
||||
* @param SwerveDrive m_robotSwerveDrive
|
||||
*/
|
||||
|
||||
public GotoLastApril(SwerveDrive swerveDrive, Vision vision) {
|
||||
public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side) {
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.vision = vision;
|
||||
this.distance = distance;
|
||||
this.side = side;
|
||||
// addRequirements(swerveDrive);
|
||||
}
|
||||
|
||||
@@ -46,11 +55,18 @@ public class GotoLastApril extends Command {
|
||||
tagRelativeXError = val;
|
||||
}
|
||||
|
||||
Alliance alliance = null;
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
xPID.initialize();
|
||||
yPID.initialize();
|
||||
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.LEFT, AutoConstants.X_OFFSET_TRIM.get());
|
||||
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side,
|
||||
Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()),
|
||||
distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get()));
|
||||
Optional<Alliance> a = DriverStation.getAlliance();
|
||||
if(!a.isEmpty())
|
||||
alliance = a.get();
|
||||
}
|
||||
|
||||
double xerr;
|
||||
@@ -59,8 +75,18 @@ public class GotoLastApril extends Command {
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
xerr = targetpos.getX() - vision.getPose2d().getX();
|
||||
yerr = targetpos.getY() - vision.getPose2d().getY();
|
||||
|
||||
if (alliance == Alliance.Red) {
|
||||
xerr = -(targetpos.getX() - vision.getPose2d().getX());
|
||||
yerr = (targetpos.getY() - vision.getPose2d().getY());
|
||||
}
|
||||
else{
|
||||
|
||||
xerr = targetpos.getX() - vision.getPose2d().getX();
|
||||
yerr = targetpos.getY() - vision.getPose2d().getY();
|
||||
}
|
||||
|
||||
|
||||
roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
|
||||
|
||||
// SmartDashboard.putNumber("PID X Error", xerr);
|
||||
@@ -70,6 +96,12 @@ public class GotoLastApril extends Command {
|
||||
double youtput = yPID.update(yerr);
|
||||
double rotoutput = rotPID.update(roterr);
|
||||
|
||||
xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
|
||||
youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
|
||||
rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
|
||||
|
||||
|
||||
|
||||
Translation2d leftStick = new Translation2d(
|
||||
Math.max(Math.min(-youtput, 1), -1),
|
||||
Math.max(Math.min(-xoutput, 1), -1)
|
||||
@@ -81,6 +113,8 @@ public class GotoLastApril extends Command {
|
||||
0
|
||||
);
|
||||
|
||||
|
||||
|
||||
setTagRelativeXError(
|
||||
new Translation2d(xerr, yerr).getAngle()
|
||||
.rotateBy(targetpos.getRotation())
|
||||
|
||||
@@ -52,7 +52,7 @@ public class LidarAlign extends Command {
|
||||
return;
|
||||
}
|
||||
|
||||
if (currentFinderTick > 100) { //arbutrary threshhold for now.
|
||||
if (currentFinderTick > 10) { //arbutrary threshhold for now.
|
||||
headedRight = !headedRight;
|
||||
currentFinderTick *= -1;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.Elevator;
|
||||
|
||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||
public class waitElevatorRefrence extends Command {
|
||||
/** Creates a new waitElevatorRefrence. */
|
||||
private Elevator elevator;
|
||||
public waitElevatorRefrence(Elevator elevator) {
|
||||
this.elevator = elevator;
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return elevator.elevatorAtRefrence();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.Elevator;
|
||||
|
||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||
public class waitEndefectorRefrence extends Command {
|
||||
/** Creates a new waitElevatorRefrence. */
|
||||
private Elevator elevator;
|
||||
public waitEndefectorRefrence(Elevator elevator) {
|
||||
this.elevator = elevator;
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return elevator.endefectorAtRefrence();
|
||||
}
|
||||
}
|
||||
@@ -4,26 +4,43 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.time.Instant;
|
||||
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.controls.PositionDutyCycle;
|
||||
import com.ctre.phoenix6.controls.PositionVoltage;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.ElevatorConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
|
||||
|
||||
public class Elevator extends SubsystemBase {
|
||||
/** Creates a new Elevator. */
|
||||
private TalonFX elevatorMotor;
|
||||
private TalonFX endefectorMotor;
|
||||
|
||||
private DigitalInput basinLimitSwitch;
|
||||
private long wait = 0;
|
||||
private long maxWait = 1000;
|
||||
|
||||
private double elevatorRefrence = 0;
|
||||
private double endefectorRefrence = 0;
|
||||
|
||||
private DigitalInput basinBeamBreak;
|
||||
private DigitalInput endefectorLimitSwitch;
|
||||
|
||||
public enum CoordinationState {
|
||||
Waiting, // for coral into the though
|
||||
Ready, // Has coral in enefector
|
||||
Waiting, // for coral into the though
|
||||
WatingBeamTriped, //once the beam break trips
|
||||
Ready, // Has coral in endefector
|
||||
Hovering, // Has coral in endefector
|
||||
L2Score,
|
||||
PrimedThree, // Arm and elevator Waiting to score in the level 3 position
|
||||
ScoringThree, // Arm and elevator in the level three position
|
||||
PrimedFour, // Arm and elevator Waiting to score in the level 4 position
|
||||
@@ -40,7 +57,7 @@ public class Elevator extends SubsystemBase {
|
||||
elevatorMotor = elevatorTalonFX;
|
||||
endefectorMotor = endefectorTalonFX;
|
||||
|
||||
this.basinLimitSwitch = basinLimitSwitch;
|
||||
this.basinBeamBreak = basinLimitSwitch;
|
||||
this.endefectorLimitSwitch = endefectorLimitSwitch;
|
||||
|
||||
elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
|
||||
@@ -54,8 +71,11 @@ public class Elevator extends SubsystemBase {
|
||||
//PID methods
|
||||
|
||||
private void PIDPosition(TalonFX motor, double position) {
|
||||
var request = new PositionVoltage(position);
|
||||
elevatorMotor.setControl(request);
|
||||
if (motor == elevatorMotor) elevatorRefrence = position;
|
||||
else endefectorRefrence = position;
|
||||
|
||||
var request = new PositionDutyCycle(position);
|
||||
motor.setControl(request);
|
||||
}
|
||||
|
||||
public void elevatorStop() {
|
||||
@@ -70,47 +90,157 @@ public class Elevator extends SubsystemBase {
|
||||
currentState = state;
|
||||
switch (currentState) {
|
||||
case Waiting: {
|
||||
wait = System.currentTimeMillis() + maxWait;
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
|
||||
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
|
||||
break;
|
||||
}
|
||||
|
||||
case WatingBeamTriped: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
|
||||
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
|
||||
break;
|
||||
}
|
||||
|
||||
case Ready: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
|
||||
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
|
||||
break;
|
||||
}
|
||||
|
||||
case ScoringThree: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
|
||||
case Hovering: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
|
||||
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
|
||||
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());
|
||||
break;
|
||||
}
|
||||
|
||||
case PrimedFour: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
|
||||
break;
|
||||
}
|
||||
|
||||
case ScoringFour: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
|
||||
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR + 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);
|
||||
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());
|
||||
break;
|
||||
}
|
||||
|
||||
case BallRemoverL2Primed: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
|
||||
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());
|
||||
break;
|
||||
}
|
||||
|
||||
case BallRemoverL3Primed: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
|
||||
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());
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
assert false;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
private void periodicWaiting() {
|
||||
if (basinLimitSwitch.get()) transitionState(CoordinationState.Ready);
|
||||
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);
|
||||
}
|
||||
|
||||
return atPos;
|
||||
}
|
||||
|
||||
public boolean endefectorAtRefrence() {
|
||||
// double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
|
||||
double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble();
|
||||
|
||||
return Math.abs(endefectorRefrence - endefectorPosition) <= 0.2;
|
||||
}
|
||||
// public void driveElevatorStick(Translation2d stick) {
|
||||
// if (stick.getNorm() > 0.05) {
|
||||
// elevatorMotor.set(stick.getY());
|
||||
// }
|
||||
// }
|
||||
|
||||
private void periodicWaiting() {
|
||||
if (!basinBeamBreak.get())
|
||||
transitionState(CoordinationState.Ready);
|
||||
}
|
||||
|
||||
// private void periodicWaitingTripped() {
|
||||
// if (!basinBeamBreak.get() && System.currentTimeMillis() > wait)
|
||||
// transitionState(CoordinationState.Ready);
|
||||
// }
|
||||
|
||||
private void periodicReady() {
|
||||
if (elevatorAtRefrence())
|
||||
transitionState(CoordinationState.Hovering);
|
||||
}
|
||||
|
||||
private void periodicScoring() {
|
||||
if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
// This method will be called once per scheduler run
|
||||
SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
|
||||
SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0);
|
||||
SmartDashboard.putString("State", currentState.toString());
|
||||
|
||||
if (currentState == CoordinationState.Waiting) {
|
||||
periodicWaiting();
|
||||
} else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
|
||||
periodicScoring();
|
||||
// periodicWaiting();
|
||||
} else if (currentState == CoordinationState.WatingBeamTriped) {
|
||||
// periodicWaitingTripped();
|
||||
} else if (currentState == CoordinationState.Ready) {
|
||||
periodicReady();
|
||||
}
|
||||
// } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
|
||||
// periodicScoring();
|
||||
// }
|
||||
}
|
||||
|
||||
public void armShuffle(){
|
||||
if(!basinBeamBreak.get()){
|
||||
//shuffle the coral with the arm until coral hits beam break
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -141,10 +141,20 @@ public class SwerveDrive extends Subsystem {
|
||||
return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
|
||||
|
||||
if (SwerveDriveConstants.INVERT_X) leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
|
||||
if (SwerveDriveConstants.INVERT_Y) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
|
||||
|
||||
Optional<Alliance> alliance = DriverStation.getAlliance();
|
||||
|
||||
if(!alliance.isEmpty()){
|
||||
if (alliance.get() == Alliance.Red)
|
||||
leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
|
||||
else
|
||||
leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
|
||||
// if (alliance.get() != Alliance.Red) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
|
||||
}
|
||||
if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1);
|
||||
|
||||
|
||||
|
||||
stopped = false;
|
||||
if (fieldRelative) {
|
||||
// ! drift correction
|
||||
@@ -304,8 +314,9 @@ public class SwerveDrive extends Subsystem {
|
||||
}
|
||||
|
||||
public void stopModules() {
|
||||
stopped = true;
|
||||
swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
|
||||
// stopped = true;
|
||||
// swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
|
||||
softStop();
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
@@ -12,7 +12,8 @@ import frc4388.robot.Constants.FieldConstants;
|
||||
public class ReefPositionHelper {
|
||||
public enum Side {
|
||||
LEFT,
|
||||
RIGHT
|
||||
RIGHT,
|
||||
CENTER
|
||||
}
|
||||
|
||||
public static final Pose2d[] RED_TAGS = {
|
||||
@@ -72,10 +73,22 @@ public class ReefPositionHelper {
|
||||
return new Pose2d();
|
||||
}
|
||||
|
||||
public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim) {
|
||||
public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {
|
||||
return offset(getNearestTag(position),
|
||||
(side == Side.LEFT ? -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET)) + xtrim,
|
||||
FieldConstants.VERTICAL_SCORING_POSITION_OFFSET);
|
||||
getSide(side) + xtrim,
|
||||
ydistance);
|
||||
}
|
||||
|
||||
public static double getSide(Side side){
|
||||
switch(side) {
|
||||
case LEFT:
|
||||
return -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET);
|
||||
case RIGHT:
|
||||
return (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET);
|
||||
case CENTER:
|
||||
return 0;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -87,6 +100,6 @@ public class ReefPositionHelper {
|
||||
return new Pose2d(new Translation2d(
|
||||
oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset,
|
||||
oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset
|
||||
), oldPose.getRotation());
|
||||
), oldPose.getRotation().rotateBy(Rotation2d.k180deg));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
|
||||
public class ButtonBox extends GenericHID {
|
||||
public static final int White = 1;
|
||||
public static final int One = 2;
|
||||
public static final int Two = 3;
|
||||
public static final int Three = 4;
|
||||
public static final int Four = 5;
|
||||
public static final int Five = 6;
|
||||
public static final int Six = 7;
|
||||
public static final int Seven = 8;
|
||||
public static final int Eight = 9;
|
||||
|
||||
public ButtonBox(int ID){
|
||||
super(ID);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user