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

Merge pull request #31 from Team4388/after-cougars-scrimmage

Browse Source 
After cougars scrimmage
This commit is contained in:
Michael Mikovsky
2025-02-25 18:33:10 +00:00
committed by GitHub
parent 99a61738e2 0d70bb4311
commit 186c7a82d3
15 changed files with 642 additions and 326 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.vscode/settings.json
Vendored
+2 -1
View File
@@ -57,5 +57,6 @@
"edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.proto.*",
"edu.wpi.first.math.**.struct.*", "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
} }
simgui-ds.json
+5
View File
@@ -1,4 +1,9 @@
{ {
"System Joysticks": {
"window": {
"enabled": false
}
},
"keyboardJoysticks": [ "keyboardJoysticks": [
{ {
"axisConfig": [ "axisConfig": [
src/main/deploy/pathplanner/autos/Line-up-no-arm.auto
+25
View File
@@ -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
}
src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path
+8 -8
View File
@@ -8,20 +8,20 @@
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 7.270707070707072, "x": 6.599873737373737,
"y": 7.013257575757576 "y": 8.029671717171716
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 5.309027777777779, "x": 5.464431818181818,
"y": 5.3768308080808085 "y": 5.6403977272727275
}, },
"prevControl": { "prevControl": {
"x": 4.945156854248459, "x": 5.240820707070706,
"y": 5.08761998976538 "y": 5.254160353535354
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 60.255118703057796 "rotation": -122.99770510121631
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -2.223961240385466 "rotation": 180.0
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/java/frc4388/robot/Constants.java
+45 -20
View File
@@ -106,7 +106,7 @@ public final class Constants {
public static final boolean DRIFT_CORRECTION_ENABLED = true; public static final boolean DRIFT_CORRECTION_ENABLED = true;
public static final boolean INVERT_X = false; 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 boolean INVERT_ROTATION = false;
// public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); // 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); private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Front Right //Front Right
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.62841796875-0.25); private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.62841796875-0.5);
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; 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_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); 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 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 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_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 LIDAR_MICROS_TO_CM = 10;
public static final int SECONDS_TO_MICROS = 1000000; public static final int SECONDS_TO_MICROS = 1000000;
public static final double XY_TOLERANCE = 0.07; // Meters 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 = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
// public static final Pose2d targetpos = // 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 LEFT_CAMERA_NAME = "CAMERA_LEFT";
public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; 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 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(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); 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 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 class FieldConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); 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(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 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 class OIConstants {
public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1; 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; 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 ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16); public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
public static final int BASIN_LIMIT_SWITCH = 0; // TODO: FIND public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
public static final int ENDEFFECTOR_LIMIT_SWITCH = 1; // 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 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 WAITING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
public static final double MAX_POSITION_ELEVATOR = 20 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position 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 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 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 double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
public static final Slot0Configs ELEVATOR_PID = new Slot0Configs() public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
src/main/java/frc4388/robot/RobotContainer.java
+248 -42
View File
@@ -16,11 +16,14 @@ import java.util.List;
import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.geometry.Translation2d; 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.GenericHID;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.utility.controller.XboxController; import frc4388.utility.controller.XboxController;
import frc4388.utility.controller.ButtonBox;
import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.robot.Constants.FieldConstants;
import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
@@ -40,6 +43,8 @@ import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.GotoLastApril; import frc4388.robot.commands.GotoLastApril;
import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.LidarAlign;
import frc4388.robot.commands.MoveForTimeCommand; 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.neoJoystickPlayback;
import frc4388.robot.commands.Swerve.neoJoystickRecorder; import frc4388.robot.commands.Swerve.neoJoystickRecorder;
@@ -85,7 +90,8 @@ public class RobotContainer {
/* Controllers */ /* Controllers */
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); 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); private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID);
/* Virtual Controllers */ /* Virtual Controllers */
@@ -109,25 +115,114 @@ public class RobotContainer {
// () -> autoplaybackName.get(), // lastAutoName // () -> autoplaybackName.get(), // lastAutoName
// new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
// true, false); // true, false);
private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
private Command AprilLidarAlign = new SequentialCommandGroup( private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
new GotoLastApril(m_robotSwerveDrive, m_vision), new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> System.out.println("Soup")),
new WaitCommand(1), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new LidarAlign(m_robotSwerveDrive, m_lidar)
); new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
private Command AprilLidarLeft = new SequentialCommandGroup(
AutoGotoPosition.asProxy(), new waitEndefectorRefrence(m_robotElevator),
new LidarAlign(m_robotSwerveDrive, m_lidar) 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( private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
AutoGotoPosition.asProxy(), new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> System.out.println("Soup")), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new WaitCommand(1), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar)//, // 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 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( private Command placeCoral = new SequentialCommandGroup(
@@ -147,15 +242,52 @@ public class RobotContainer {
new WaitCommand(2), new WaitCommand(2),
new InstantCommand(() -> System.out.println("Done yoinking some coral...")) 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. * The container for the robot. Contains subsystems, OI devices, and commands.
*/ */
public RobotContainer() { public RobotContainer() {
NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition); NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
NamedCommands.registerCommand("april-allign", AprilLidarAlign); NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
NamedCommands.registerCommand("place-coral", placeCoral); 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); NamedCommands.registerCommand("grab-coral", grabCoral);
configureButtonBindings(); configureButtonBindings();
@@ -175,6 +307,12 @@ public class RobotContainer {
}, m_robotSwerveDrive) }, m_robotSwerveDrive)
.withName("SwerveDrive DefaultCommand")); .withName("SwerveDrive DefaultCommand"));
m_robotSwerveDrive.setToSlow(); m_robotSwerveDrive.setToSlow();
// m_robotElevator.setDefaultCommand(new RunCommand(() -> {
// m_robotElevator.driveElevatorStick(m_operatorXbox.getRight());
// }, m_robotElevator)
// .withName("Elevator"));
makeAutoChooser(); makeAutoChooser();
SmartDashboard.putData("Auto Chooser", autoChooser); SmartDashboard.putData("Auto Chooser", autoChooser);
// this.subsystems.add(m_robotSwerveDrive); // this.subsystems.add(m_robotSwerveDrive);
@@ -220,54 +358,116 @@ public class RobotContainer {
private void configureButtonBindings() { private void configureButtonBindings() {
// ? /* Driver Buttons */ // ? /* Driver Buttons */
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
// ! /* Speed */ // ! /* Speed */
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); .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())); .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())); .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())); .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())); .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) 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 */ // ? /* Operator Buttons */
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
// .onTrue(AutoGotoPosition);
.onTrue(AprilLidarRight);
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); .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)); .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)); .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) new JoystickButton(getButtonBox(), ButtonBox.Five)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator)); .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)*/ // ? /* Programer Buttons (Controller 3)*/
@@ -320,16 +520,18 @@ public class RobotContainer {
*/ */
public Command getAutonomousCommand() { public Command getAutonomousCommand() {
//return autoPlayback; //return autoPlayback;
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision) //return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
//return autoChooser.getSelected(); //return autoChooser.getSelected();
// try{ // try{
// // Load the path you want to follow using its name in the GUI // // // Load the path you want to follow using its name in the GUI
return autoCommand; // return autoCommand;
// } catch (Exception e) { // } catch (Exception e) {
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); // 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 // zach told me to do the below comment
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision); //return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
// return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); // return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
@@ -373,6 +575,10 @@ public class RobotContainer {
return this.m_operatorXbox; return this.m_operatorXbox;
} }
public ButtonBox getButtonBox() {
return this.m_buttonBox;
}
public VirtualController getVirtualDriverController() { public VirtualController getVirtualDriverController() {
return m_virtualDriver; return m_virtualDriver;
} }
src/main/java/frc4388/robot/commands/Autos/Taxi.txt
-225
View File
@@ -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
src/main/java/frc4388/robot/commands/GotoLastApril.java
+38 -4
View File
@@ -1,8 +1,13 @@
package frc4388.robot.commands; package frc4388.robot.commands;
import java.util.Optional;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d; 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.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
@@ -28,15 +33,19 @@ public class GotoLastApril extends Command {
SwerveDrive swerveDrive; SwerveDrive swerveDrive;
Vision vision; Vision vision;
double distance;
Side side;
/** /**
* Command to drive robot to position. * Command to drive robot to position.
* @param SwerveDrive m_robotSwerveDrive * @param SwerveDrive m_robotSwerveDrive
*/ */
public GotoLastApril(SwerveDrive swerveDrive, Vision vision) { public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side) {
this.swerveDrive = swerveDrive; this.swerveDrive = swerveDrive;
this.vision = vision; this.vision = vision;
this.distance = distance;
this.side = side;
// addRequirements(swerveDrive); // addRequirements(swerveDrive);
} }
@@ -46,11 +55,18 @@ public class GotoLastApril extends Command {
tagRelativeXError = val; tagRelativeXError = val;
} }
Alliance alliance = null;
@Override @Override
public void initialize() { public void initialize() {
xPID.initialize(); xPID.initialize();
yPID.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; double xerr;
@@ -59,8 +75,18 @@ public class GotoLastApril extends Command {
@Override @Override
public void execute() { 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(); roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
// SmartDashboard.putNumber("PID X Error", xerr); // SmartDashboard.putNumber("PID X Error", xerr);
@@ -70,6 +96,12 @@ public class GotoLastApril extends Command {
double youtput = yPID.update(yerr); double youtput = yPID.update(yerr);
double rotoutput = rotPID.update(roterr); 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( Translation2d leftStick = new Translation2d(
Math.max(Math.min(-youtput, 1), -1), Math.max(Math.min(-youtput, 1), -1),
Math.max(Math.min(-xoutput, 1), -1) Math.max(Math.min(-xoutput, 1), -1)
@@ -81,6 +113,8 @@ public class GotoLastApril extends Command {
0 0
); );
setTagRelativeXError( setTagRelativeXError(
new Translation2d(xerr, yerr).getAngle() new Translation2d(xerr, yerr).getAngle()
.rotateBy(targetpos.getRotation()) .rotateBy(targetpos.getRotation())
src/main/java/frc4388/robot/commands/LidarAlign.java
+1 -1
View File
@@ -52,7 +52,7 @@ public class LidarAlign extends Command {
return; return;
} }
if (currentFinderTick > 100) { //arbutrary threshhold for now. if (currentFinderTick > 10) { //arbutrary threshhold for now.
headedRight = !headedRight; headedRight = !headedRight;
currentFinderTick *= -1; currentFinderTick *= -1;
} }
src/main/java/frc4388/robot/commands/waitElevatorRefrence.java
+36
View File
@@ -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();
}
}
src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java
+36
View File
@@ -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();
}
}
src/main/java/frc4388/robot/subsystems/Elevator.java
+145 -15
View File
@@ -4,26 +4,43 @@
package frc4388.robot.subsystems; package frc4388.robot.subsystems;
import java.time.Instant;
import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue; 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.DigitalInput;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ElevatorConstants; import frc4388.robot.Constants.ElevatorConstants;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
public class Elevator extends SubsystemBase { public class Elevator extends SubsystemBase {
/** Creates a new Elevator. */ /** Creates a new Elevator. */
private TalonFX elevatorMotor; private TalonFX elevatorMotor;
private TalonFX endefectorMotor; 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; private DigitalInput endefectorLimitSwitch;
public enum CoordinationState { public enum CoordinationState {
Waiting, // for coral into the though Waiting, // for coral into the though
Ready, // Has coral in enefector 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 PrimedThree, // Arm and elevator Waiting to score in the level 3 position
ScoringThree, // Arm and elevator in the level three position ScoringThree, // Arm and elevator in the level three position
PrimedFour, // Arm and elevator Waiting to score in the level 4 position PrimedFour, // Arm and elevator Waiting to score in the level 4 position
@@ -40,7 +57,7 @@ public class Elevator extends SubsystemBase {
elevatorMotor = elevatorTalonFX; elevatorMotor = elevatorTalonFX;
endefectorMotor = endefectorTalonFX; endefectorMotor = endefectorTalonFX;
this.basinLimitSwitch = basinLimitSwitch; this.basinBeamBreak = basinLimitSwitch;
this.endefectorLimitSwitch = endefectorLimitSwitch; this.endefectorLimitSwitch = endefectorLimitSwitch;
elevatorMotor.setNeutralMode(NeutralModeValue.Brake); elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
@@ -54,8 +71,11 @@ public class Elevator extends SubsystemBase {
//PID methods //PID methods
private void PIDPosition(TalonFX motor, double position) { private void PIDPosition(TalonFX motor, double position) {
var request = new PositionVoltage(position); if (motor == elevatorMotor) elevatorRefrence = position;
elevatorMotor.setControl(request); else endefectorRefrence = position;
var request = new PositionDutyCycle(position);
motor.setControl(request);
} }
public void elevatorStop() { public void elevatorStop() {
@@ -70,47 +90,157 @@ public class Elevator extends SubsystemBase {
currentState = state; currentState = state;
switch (currentState) { switch (currentState) {
case Waiting: { case Waiting: {
wait = System.currentTimeMillis() + maxWait;
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
break; break;
} }
case WatingBeamTriped: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
break;
}
case Ready: { case Ready: {
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR); PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
break; break;
} }
case ScoringThree: { case Hovering: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); 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); PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
break; break;
} }
case ScoringFour: { case ScoringFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR); PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break; 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() { public boolean elevatorAtRefrence() {
if (basinLimitSwitch.get()) transitionState(CoordinationState.Ready); // 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() { private void periodicScoring() {
if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting); if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
} }
@Override @Override
public void periodic() { public void periodic() {
// This method will be called once per scheduler run // 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) { if (currentState == CoordinationState.Waiting) {
periodicWaiting(); // periodicWaiting();
} else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { } else if (currentState == CoordinationState.WatingBeamTriped) {
periodicScoring(); // 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
} }
} }
} }
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+16 -5
View File
@@ -141,10 +141,20 @@ public class SwerveDrive extends Subsystem {
return; // don't bother doing swerve drive math and return early. return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
if (SwerveDriveConstants.INVERT_X) leftStick = new Translation2d(-leftStick.getX(), leftStick.getY()); Optional<Alliance> alliance = DriverStation.getAlliance();
if (SwerveDriveConstants.INVERT_Y) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
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); if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1);
stopped = false; stopped = false;
if (fieldRelative) { if (fieldRelative) {
// ! drift correction // ! drift correction
@@ -304,8 +314,9 @@ public class SwerveDrive extends Subsystem {
} }
public void stopModules() { public void stopModules() {
stopped = true; // stopped = true;
swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
softStop();
} }
@Override @Override
src/main/java/frc4388/utility/ReefPositionHelper.java
+18 -5
View File
@@ -12,7 +12,8 @@ import frc4388.robot.Constants.FieldConstants;
public class ReefPositionHelper { public class ReefPositionHelper {
public enum Side { public enum Side {
LEFT, LEFT,
RIGHT RIGHT,
CENTER
} }
public static final Pose2d[] RED_TAGS = { public static final Pose2d[] RED_TAGS = {
@@ -72,10 +73,22 @@ public class ReefPositionHelper {
return new Pose2d(); 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), return offset(getNearestTag(position),
(side == Side.LEFT ? -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET)) + xtrim, getSide(side) + xtrim,
FieldConstants.VERTICAL_SCORING_POSITION_OFFSET); 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( return new Pose2d(new Translation2d(
oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset, 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 oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset
), oldPose.getRotation()); ), oldPose.getRotation().rotateBy(Rotation2d.k180deg));
} }
} }
src/main/java/frc4388/utility/controller/ButtonBox.java
+19
View File
@@ -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);
}
}