From 204ba6d5110d68a9d2c45fa0cc4e4923e2b07981 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Mon, 3 Mar 2025 16:32:27 -0700 Subject: [PATCH 01/29] Optimization and improvements --- .../paths/Bottom Feed to Reef.path | 14 ++++---- src/main/java/frc4388/robot/Constants.java | 18 ++++++---- .../java/frc4388/robot/RobotContainer.java | 34 ++++++++++++++----- .../frc4388/robot/commands/GotoLastApril.java | 17 +++++++--- .../frc4388/robot/subsystems/Elevator.java | 13 ++++--- .../frc4388/robot/subsystems/SwerveDrive.java | 7 +++- .../java/frc4388/robot/subsystems/Vision.java | 5 +-- 7 files changed, 74 insertions(+), 34 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path index 25f3281..0762347 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.39747159090909, - "y": 1.3027556818181814 + "x": 2.4125846006173957, + "y": 1.7771562346874141 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.3048863636363635, - "y": 2.5292613636363637 + "x": 3.5997443181818185, + "y": 2.6888068181818174 }, "prevControl": { - "x": 5.893210227272728, - "y": 1.9808238636363626 + "x": 2.6837095087024796, + "y": 1.9835311238550895 }, "nextControl": null, "isLocked": false, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 114.22774531795413 + "rotation": 56.633633998940475 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4807698..e9cce41 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -69,7 +69,7 @@ public final class Constants { public static final class SwerveDriveConstants { - public static final double MAX_ROT_SPEED = 3.5; + public static final double MAX_ROT_SPEED = Math.PI * 2; public static final double AUTO_MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 1.0; public static double ROTATION_SPEED = MAX_ROT_SPEED; @@ -329,7 +329,7 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains XY_GAINS = new Gains(3,0.01,0.0); + public static final Gains XY_GAINS = new Gains(3,0.3,0.0); public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); @@ -339,6 +339,9 @@ public final class Constants { public static final double XY_TOLERANCE = 0.07; // Meters public static final double ROT_TOLERANCE = 5; // Degrees + + public static final double MIN_XY_PID_OUTPUT = 0.0; + public static final double MIN_ROT_PID_OUTPUT = 0.0; // X is tangent to reef side // Y is normal to reef side @@ -346,7 +349,7 @@ public final class Constants { public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16); public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); - public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); @@ -366,15 +369,15 @@ public final class Constants { public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; 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 Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547+(1*-1)), -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 = 2; // Meters // Photonvision thing // The standard deviations of our vision estimated poses, which affect correction rate // X, Y, Theta // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2 - public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(3, 3, 4); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); } @@ -424,6 +427,8 @@ 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 = 8; // TODO: FIND + public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND @@ -432,6 +437,7 @@ public final class Constants { public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR; public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe + public static final double HOVERING_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; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c31278f..7ad5266 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -148,8 +148,12 @@ public class RobotContainer { new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), - - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + + new ConditionalCommand( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + () -> m_robotElevator.hasCoral()), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -177,11 +181,17 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), + new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + new ConditionalCommand( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + () -> m_robotElevator.hasCoral()), + + - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -295,8 +305,10 @@ public class RobotContainer { ); private Command thrustIntake = new SequentialCommandGroup( - new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 100, true), - new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive) + new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod(), m_robotSwerveDrive), + new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 300, true), + new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive), + new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod(), m_robotSwerveDrive) ); private Boolean operatorManualMode = false; @@ -310,7 +322,7 @@ public class RobotContainer { NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0, 1), - new Translation2d(), 200, true + new Translation2d(), 400, true ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); @@ -428,6 +440,10 @@ public class RobotContainer { new Trigger(() -> getDeadbandedDriverController().getPOV() == 270 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); + new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod())) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod())); + // While Left Trigger NOT Pressed: Fine Alignment new Trigger(() -> getDeadbandedDriverController().getPOV() != -1 && !(getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)) .whileTrue(new RunCommand( @@ -436,7 +452,7 @@ public class RobotContainer { 1, Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) ), - getDeadbandedDriverController().getRight(), 0.15 + getDeadbandedDriverController().getRight(), 0.30 ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); @@ -511,10 +527,10 @@ public class RobotContainer { .onTrue (new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed), m_robotElevator)) .onFalse(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go), m_robotElevator)); - new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> {operatorManualMode = !operatorManualMode;})); - new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking())); // Auto Scoring diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index 5f7f285..47b0430 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -1,5 +1,7 @@ package frc4388.robot.commands; +import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; + import java.util.Optional; import edu.wpi.first.math.geometry.Pose2d; @@ -74,6 +76,10 @@ public class GotoLastApril extends Command { double yerr; double roterr; + double xoutput; + double youtput; + double rotoutput; + @Override public void execute() { xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis); @@ -100,9 +106,9 @@ public class GotoLastApril extends Command { // SmartDashboard.putNumber("PID X Error", xerr); // SmartDashboard.putNumber("PID Y Error", yerr); - double xoutput = xPID.update(xerr); - double youtput = yPID.update(yerr); - double rotoutput = rotPID.update(roterr); + xoutput = xPID.update(xerr); + youtput = yPID.update(yerr); + rotoutput = rotPID.update(roterr); xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; @@ -133,7 +139,10 @@ public class GotoLastApril extends Command { @Override public final boolean isFinished() { - boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE); + boolean finished = ( + (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && + (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && + (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE || Math.abs(rotoutput) <= AutoConstants.MIN_ROT_PID_OUTPUT)); // System.out.println(finished); if(finished) diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 7e775d2..06162a4 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -49,6 +49,7 @@ public class Elevator extends Subsystem { private DigitalInput basinBeamBreak; private DigitalInput endeffectorLimitSwitch; + // private DigitalInput intakeIR; public enum CoordinationState { Waiting, // for coral into the though @@ -69,12 +70,14 @@ public class Elevator extends Subsystem { private CoordinationState currentState; public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { + // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) { elevatorMotor = elevatorTalonFX; endeffectorMotor = endeffectorTalonFX; this.led = led; this.basinBeamBreak = basinLimitSwitch; this.endeffectorLimitSwitch = endeffectorLimitSwitch; + // this.intakeIR = intakeDigitalInput; elevatorMotor.setNeutralMode(NeutralModeValue.Brake); endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); @@ -129,14 +132,14 @@ public class Elevator extends Subsystem { } case Hovering: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); + PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); led.setMode(LEDConstants.READY_PATTERN); break; } case L2Score: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); break; @@ -240,8 +243,8 @@ public class Elevator extends Subsystem { private void periodicWaiting() { if (!basinBeamBreak.get()) transitionState(CoordinationState.Ready); - if(!endeffectorLimitSwitch.get()) - transitionState(CoordinationState.Hovering); + // if(!endeffectorLimitSwitch.get()) + // transitionState(CoordinationState.Hovering); } // private void periodicWaitingTripped() { @@ -253,7 +256,7 @@ public class Elevator extends Subsystem { if (elevatorAtReference() && !endeffectorLimitSwitch.get()) transitionState(CoordinationState.Hovering); if(elevatorAtReference() && endeffectorLimitSwitch.get()) - transitionState(CoordinationState.Waiting); + transitionState(CoordinationState.Hovering); } private void periodicScoring() { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 484480a..f8770a0 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -326,7 +326,7 @@ public class SwerveDrive extends Subsystem { rotTarget += deltaAngle; } - swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); + swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); } // if(e.isPresent()) @@ -392,6 +392,11 @@ public class SwerveDrive extends Subsystem { setToSlow(); } + public void startTurboPeriod() { + tmp_gear_index = gear_index; + setToTurbo(); + } + public void endSlowPeriod() { setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]); gear_index = tmp_gear_index; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index ed5ebe9..347d741 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -48,7 +48,7 @@ public class Vision extends Subsystem { private boolean isTagDetected = false; private boolean isTagProcessed = false; - private Pose2d lastVisionPose = new Pose2d(); + public Pose2d lastVisionPose = new Pose2d(); private Pose2d lastPhysOdomPose = new Pose2d(); private Matrix curStdDevs; @@ -265,7 +265,8 @@ public class Vision extends Subsystem { public Pose2d getPose2d() { if(isTagDetected && isTagProcessed) - return lastVisionPose; + // return lastVisionPose; + return lastPhysOdomPose; else return lastPhysOdomPose; } From ebc08d7eaa4a22dd9ad1e9001aa0fb1f835c0816 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Mon, 3 Mar 2025 16:40:33 -0700 Subject: [PATCH 02/29] Rename autos --- ...d Bottom Cage.auto => 2 Coral Cage 1.auto} | 14 ++- ...d Center Cage.auto => 2 Coral Cage 2.auto} | 6 +- .../{Mira's Auto.auto => 2 Coral Cage 3.auto} | 28 +++--- ...Blue Top Cage.auto => 2 Coral Cage 4.auto} | 12 ++- .../pathplanner/autos/2 Coral Cage 5.auto | 55 +++++++++++ .../pathplanner/autos/2 Coral Cage 6.auto | 55 +++++++++++ .../autos/2 Coral Center Barge Bottom.auto | 55 +++++++++++ ...Top.auto => 2 Coral Center Barge Top.auto} | 28 +++--- .../pathplanner/autos/3 Coral Cage 5.auto | 91 +++++++++++++++++++ .../pathplanner/autos/Blue Bottom Cage.auto | 61 ------------- .../pathplanner/autos/Blue Center Cage.auto | 61 ------------- .../autos/Center Barge Bottom.auto | 61 ------------- .../pathplanner/autos/Line-up-no-arm.auto | 25 ----- .../pathplanner/autos/Red Top Cage.auto | 61 ------------- .../paths/Bottom Feed to Reef 2.path | 54 +++++++++++ .../paths/Bottom Feed to Reef.path | 54 ----------- ... Cage to Reef.path => Cage 1 to Reef.path} | 10 +- ...age to Reef.path => Cage 2 to Reef 5.path} | 10 +- ... Cage to Reef.path => Cage 3 to Reef.path} | 14 +-- ... Cage to Reef.path => Cage 4 to Reef.path} | 10 +- ... Cage to Reef.path => Cage 5 to Reef.path} | 16 ++-- ... Cage to Reef.path => Cage 6 to Reef.path} | 18 ++-- ... Reef.path => Center Barge to Reef 4.path} | 8 +- .../paths/Center Reef to Feed.path | 70 -------------- .../paths/Reef 2 to Bottom Feed.path | 54 +++++++++++ ...o Feed.path => Reef 3 to Bottom Feed.path} | 18 ++-- ...m Feed.path => Reef 4 to Bottom Feed.path} | 18 ++-- .../pathplanner/paths/Reef 4 to Top Feed.path | 54 +++++++++++ ...f to Feed.path => Reef 5 to Top Feed.path} | 10 +- src/main/deploy/pathplanner/paths/Taxi.path | 24 +---- ...d to Reef.path => Top Feed to Reef 6.path} | 2 +- src/main/deploy/pathplanner/settings.json | 8 +- .../java/frc4388/robot/RobotContainer.java | 9 +- 33 files changed, 544 insertions(+), 530 deletions(-) rename src/main/deploy/pathplanner/autos/{Red Bottom Cage.auto => 2 Coral Cage 1.auto} (71%) rename src/main/deploy/pathplanner/autos/{Red Center Cage.auto => 2 Coral Cage 2.auto} (86%) rename src/main/deploy/pathplanner/autos/{Mira's Auto.auto => 2 Coral Cage 3.auto} (68%) rename src/main/deploy/pathplanner/autos/{Blue Top Cage.auto => 2 Coral Cage 4.auto} (75%) create mode 100644 src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto create mode 100644 src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto create mode 100644 src/main/deploy/pathplanner/autos/2 Coral Center Barge Bottom.auto rename src/main/deploy/pathplanner/autos/{Center Barge Top.auto => 2 Coral Center Barge Top.auto} (68%) create mode 100644 src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto delete mode 100644 src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto delete mode 100644 src/main/deploy/pathplanner/autos/Blue Center Cage.auto delete mode 100644 src/main/deploy/pathplanner/autos/Center Barge Bottom.auto delete mode 100644 src/main/deploy/pathplanner/autos/Line-up-no-arm.auto delete mode 100644 src/main/deploy/pathplanner/autos/Red Top Cage.auto create mode 100644 src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path delete mode 100644 src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path rename src/main/deploy/pathplanner/paths/{Top Blue Cage to Reef.path => Cage 1 to Reef.path} (87%) rename src/main/deploy/pathplanner/paths/{Center Blue Cage to Reef.path => Cage 2 to Reef 5.path} (87%) rename src/main/deploy/pathplanner/paths/{Bottom Blue Cage to Reef.path => Cage 3 to Reef.path} (81%) rename src/main/deploy/pathplanner/paths/{Top Red Cage to Reef.path => Cage 4 to Reef.path} (87%) rename src/main/deploy/pathplanner/paths/{Center Red Cage to Reef.path => Cage 5 to Reef.path} (79%) rename src/main/deploy/pathplanner/paths/{Bottom Red Cage to Reef.path => Cage 6 to Reef.path} (75%) rename src/main/deploy/pathplanner/paths/{Center Barge to Reef.path => Center Barge to Reef 4.path} (89%) delete mode 100644 src/main/deploy/pathplanner/paths/Center Reef to Feed.path create mode 100644 src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path rename src/main/deploy/pathplanner/paths/{Bottom Reef to Feed.path => Reef 3 to Bottom Feed.path} (76%) rename src/main/deploy/pathplanner/paths/{Center Reef to Bottom Feed.path => Reef 4 to Bottom Feed.path} (75%) create mode 100644 src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path rename src/main/deploy/pathplanner/paths/{Top Reef to Feed.path => Reef 5 to Top Feed.path} (87%) rename src/main/deploy/pathplanner/paths/{Top Feed to Reef.path => Top Feed to Reef 6.path} (97%) diff --git a/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto similarity index 71% rename from src/main/deploy/pathplanner/autos/Red Bottom Cage.auto rename to src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto index 3439f6b..e35d8f0 100644 --- a/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "Bottom Red Cage to Reef" + "pathName": "Cage 1 to Reef" } }, { @@ -19,7 +19,13 @@ { "type": "path", "data": { - "pathName": "Bottom Reef to Feed" + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "align-feed" } }, { @@ -31,13 +37,13 @@ { "type": "path", "data": { - "pathName": "Bottom Feed to Reef" + "pathName": "Top Feed to Reef 6" } }, { "type": "named", "data": { - "name": "place-coral-left-l4" + "name": "place-coral-right-l4" } } ] diff --git a/src/main/deploy/pathplanner/autos/Red Center Cage.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto similarity index 86% rename from src/main/deploy/pathplanner/autos/Red Center Cage.auto rename to src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto index 7bfeb66..9a62160 100644 --- a/src/main/deploy/pathplanner/autos/Red Center Cage.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "Center Red Cage to Reef" + "pathName": "Cage 2 to Reef 5" } }, { @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": "Bottom Reef to Feed" + "pathName": "Reef 5 to Top Feed" } }, { @@ -37,7 +37,7 @@ { "type": "path", "data": { - "pathName": "Bottom Feed to Reef" + "pathName": "Top Feed to Reef 6" } }, { diff --git a/src/main/deploy/pathplanner/autos/Mira's Auto.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto similarity index 68% rename from src/main/deploy/pathplanner/autos/Mira's Auto.auto rename to src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto index 3acc852..bd7da14 100644 --- a/src/main/deploy/pathplanner/autos/Mira's Auto.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto @@ -7,25 +7,25 @@ { "type": "path", "data": { - "pathName": "Top Blue Cage to Reef" + "pathName": "Cage 3 to Reef" } }, { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-right-l4" } }, { "type": "path", "data": { - "pathName": "Top Reef to Feed" + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "align-feed" } }, { @@ -37,19 +37,13 @@ { "type": "path", "data": { - "pathName": "Top Feed to Reef" + "pathName": "Top Feed to Reef 6" } }, { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-right-l4" } } ] diff --git a/src/main/deploy/pathplanner/autos/Blue Top Cage.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto similarity index 75% rename from src/main/deploy/pathplanner/autos/Blue Top Cage.auto rename to src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto index 555bb6c..766c054 100644 --- a/src/main/deploy/pathplanner/autos/Blue Top Cage.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "Top Blue Cage to Reef" + "pathName": "Cage 4 to Reef" } }, { @@ -19,7 +19,13 @@ { "type": "path", "data": { - "pathName": "Top Reef to Feed" + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "align-feed" } }, { @@ -31,7 +37,7 @@ { "type": "path", "data": { - "pathName": "Top Feed to Reef" + "pathName": "Bottom Feed to Reef 2" } }, { diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto new file mode 100644 index 0000000..387787a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Cage 5 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "align-feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto new file mode 100644 index 0000000..c861420 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Cage 6 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "align-feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Bottom.auto new file mode 100644 index 0000000..dbe71b6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Bottom.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 4 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "align-feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Barge Top.auto b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Top.auto similarity index 68% rename from src/main/deploy/pathplanner/autos/Center Barge Top.auto rename to src/main/deploy/pathplanner/autos/2 Coral Center Barge Top.auto index a5f3397..68320cc 100644 --- a/src/main/deploy/pathplanner/autos/Center Barge Top.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Top.auto @@ -7,25 +7,25 @@ { "type": "path", "data": { - "pathName": "Center Barge to Reef" + "pathName": "Center Barge to Reef 4" } }, { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-right-l4" } }, { "type": "path", "data": { - "pathName": "Center Reef to Feed" + "pathName": "Reef 4 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "align-feed" } }, { @@ -37,19 +37,13 @@ { "type": "path", "data": { - "pathName": "Top Feed to Reef" + "pathName": "Top Feed to Reef 6" } }, { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-right-l4" } } ] diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto new file mode 100644 index 0000000..1145cb6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Cage 5 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "align-feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "align-feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto b/src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto deleted file mode 100644 index 98b8016..0000000 --- a/src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Bottom Blue Cage to Reef" - } - }, - { - "type": "named", - "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Top Reef to Feed" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Top Feed to Reef" - } - }, - { - "type": "named", - "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue Center Cage.auto b/src/main/deploy/pathplanner/autos/Blue Center Cage.auto deleted file mode 100644 index a7a32d6..0000000 --- a/src/main/deploy/pathplanner/autos/Blue Center Cage.auto +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center Blue Cage to Reef" - } - }, - { - "type": "named", - "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Top Reef to Feed" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Top Feed to Reef" - } - }, - { - "type": "named", - "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/Center Barge Bottom.auto deleted file mode 100644 index 70a439a..0000000 --- a/src/main/deploy/pathplanner/autos/Center Barge Bottom.auto +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center Barge to Reef" - } - }, - { - "type": "named", - "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Center Reef to Bottom Feed" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom Feed to Reef" - } - }, - { - "type": "named", - "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto b/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto deleted file mode 100644 index 4d51c0d..0000000 --- a/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Top Blue Cage to Reef" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-left-l4" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red Top Cage.auto b/src/main/deploy/pathplanner/autos/Red Top Cage.auto deleted file mode 100644 index 429e075..0000000 --- a/src/main/deploy/pathplanner/autos/Red Top Cage.auto +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Top Red Cage to Reef" - } - }, - { - "type": "named", - "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom Reef to Feed" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom Feed to Reef" - } - }, - { - "type": "named", - "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path new file mode 100644 index 0000000..bef1742 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.0603125, + "y": 1.0390625 + }, + "prevControl": null, + "nextControl": { + "x": 2.3463856916648673, + "y": 1.797152889765366 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.680625, + "y": 2.635625 + }, + "prevControl": { + "x": 2.741625910703272, + "y": 2.050875079374932 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.28109573597083 + }, + "reversed": false, + "folder": "Bottom Feed to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 53.32565033042684 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path deleted file mode 100644 index 0762347..0000000 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.3145202020202018, - "y": 0.9655934343434341 - }, - "prevControl": null, - "nextControl": { - "x": 2.4125846006173957, - "y": 1.7771562346874141 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.5997443181818185, - "y": 2.6888068181818174 - }, - "prevControl": { - "x": 2.6837095087024796, - "y": 1.9835311238550895 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 56.633633998940475 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 53.446462811652175 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path similarity index 87% rename from src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path rename to src/main/deploy/pathplanner/paths/Cage 1 to Reef.path index 66ebba7..7e7a001 100644 --- a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.599873737373737, - "y": 8.029671717171716 + "x": 5.8378125, + "y": 6.9378125 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 5.391107954545454 }, "prevControl": { - "x": 5.121161616161616, - "y": 5.00487058080808 + "x": 5.630625, + "y": 5.5728125 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": -122.99770510121631 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path similarity index 87% rename from src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path rename to src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path index 434e054..4dc63d4 100644 --- a/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.3210457316672, - "y": 6.004686892287159 + "x": 6.451471554401174, + "y": 5.958542570253511 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 5.417487373737374 }, "prevControl": { - "x": 6.306061170284897, - "y": 5.670117895431184 + "x": 5.8621875, + "y": 6.12125 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": -120.86136963407526 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, "rotation": -179.99261564513898 diff --git a/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path similarity index 81% rename from src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path rename to src/main/deploy/pathplanner/paths/Cage 3 to Reef.path index f63510a..6ad2651 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 8.269913357759823, - "y": 5.035328772529629 + "x": 7.366483068336041, + "y": 5.1021119267533175 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.370012626262627, - "y": 5.346338383838384 + "x": 5.31375, + "y": 5.43875 }, "prevControl": { - "x": 4.997649306769947, - "y": 5.369630479499745 + "x": 5.7403125, + "y": 5.6215625 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": -121.26879518614867 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, "rotation": 179.34296858150984 diff --git a/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path similarity index 87% rename from src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path rename to src/main/deploy/pathplanner/paths/Cage 4 to Reef.path index 0cefb5a..e904b37 100644 --- a/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.319147727272727, - "y": 2.3198579545454536 + "x": 6.2521875, + "y": 2.6234375000000005 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.6934974747474745 }, "prevControl": { - "x": 6.401761363636364, - "y": 1.5819602272727276 + "x": 5.630625, + "y": 2.148125 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": 123.49004753500587 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, "rotation": -178.9832812445108 diff --git a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path similarity index 79% rename from src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path rename to src/main/deploy/pathplanner/paths/Cage 5 to Reef.path index 7e6f511..e015f20 100644 --- a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.96969696969697, - "y": 1.1282196969696958 + "x": 7.044375, + "y": 1.8921875 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.319191919191919, - "y": 2.6833333333333327 + "x": 5.2771875, + "y": 2.61125 }, "prevControl": { - "x": 5.75625, - "y": 2.256439393939394 + "x": 5.3503125, + "y": 2.1603125 }, "nextControl": null, "isLocked": false, @@ -42,10 +42,10 @@ }, "goalEndState": { "velocity": 0, - "rotation": 122.87136694597014 + "rotation": 118.99790479482637 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, "rotation": 179.5295681977034 diff --git a/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path similarity index 75% rename from src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path rename to src/main/deploy/pathplanner/paths/Cage 6 to Reef.path index 226a1a8..42a1ac9 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 8.123574009981496, - "y": 0.3592362071562901 + "x": 6.625889381464077, + "y": 1.4477106264692527 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.3903409090909085, - "y": 2.6833333333333327 + "x": 5.3015625, + "y": 2.6234375 }, "prevControl": { - "x": 5.06636861749931, - "y": 2.9361778277713553 + "x": 5.721332938841585, + "y": 2.265481302925542 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -57.4259428654274 + "rotation": 120.98817835541992 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": -0.9548412538721401 + "rotation": -179.94258861633824 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Barge to Reef.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4.path similarity index 89% rename from src/main/deploy/pathplanner/paths/Center Barge to Reef.path rename to src/main/deploy/pathplanner/paths/Center Barge to Reef 4.path index cfbbb95..596548a 100644 --- a/src/main/deploy/pathplanner/paths/Center Barge to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 6.172414772727272, + "x": 6.1059375, "y": 4.045328282828282 }, "prevControl": { - "x": 5.696091638273898, - "y": 4.0462802990619195 + "x": 6.35555109493404, + "y": 4.031433923393444 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": -179.96096735022735 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, "rotation": -179.2890804030095 diff --git a/src/main/deploy/pathplanner/paths/Center Reef to Feed.path b/src/main/deploy/pathplanner/paths/Center Reef to Feed.path deleted file mode 100644 index 5bee8d7..0000000 --- a/src/main/deploy/pathplanner/paths/Center Reef to Feed.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.982954545454545, - "y": 4.03497159090909 - }, - "prevControl": null, - "nextControl": { - "x": 7.109744318181819, - "y": 3.8355397727272718 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.232244318181818, - "y": 5.281420454545455 - }, - "prevControl": { - "x": 7.1397939460232385, - "y": 4.676387369317844 - }, - "nextControl": { - "x": 5.574119318181818, - "y": 5.720170454545454 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.121401515151515, - "y": 7.003093434343434 - }, - "prevControl": { - "x": 0.32627122293847843, - "y": 7.229156472141377 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -53.93780932479241 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 179.1574757392596 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path new file mode 100644 index 0000000..562a21c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.680625, + "y": 2.61125 + }, + "prevControl": null, + "nextControl": { + "x": 3.2541591082344743, + "y": 2.3264921263556886 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.12125, + "y": 1.0025 + }, + "prevControl": { + "x": 1.3989083079519464, + "y": 1.1682576997602765 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 55.37584492005105 + }, + "reversed": false, + "folder": "Reef to Bottom Feed", + "idealStartingState": { + "velocity": 0, + "rotation": 58.10920819815426 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path similarity index 76% rename from src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path rename to src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path index 6bb67cc..48b8fd6 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.344772727272727, - "y": 2.6688636363636364 + "x": 5.265, + "y": 2.61125 }, "prevControl": null, "nextControl": { - "x": 6.4715625, - "y": 1.432386363636364 + "x": 4.137208815063533, + "y": 2.0700494801809173 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.1823863636363636, - "y": 1.026578282828283 + "x": 1.096875, + "y": 1.0390625 }, "prevControl": { - "x": 0.9229528762543462, - "y": 0.9200437072077402 + "x": 1.3299317623663032, + "y": 1.1501468126694903 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": 55.05578949900953 }, "reversed": false, - "folder": null, + "folder": "Reef to Bottom Feed", "idealStartingState": { "velocity": 0, "rotation": 121.42956561483854 diff --git a/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path similarity index 75% rename from src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path rename to src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path index 200490f..cbd1bbf 100644 --- a/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 6.092642045454545, - "y": 3.9651704545454542 + "x": 6.1059375, + "y": 4.0249999999999995 }, "prevControl": null, "nextControl": { - "x": 9.024289772727268, - "y": 3.446647727272728 + "x": 7.264857954545455, + "y": 2.244517045454545 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.3755050505050503, - "y": 0.8842803030303026 + "x": 1.1090625, + "y": 1.0146875 }, "prevControl": { - "x": 1.5707222528658078, - "y": 1.040454064918909 + "x": 1.95, + "y": 2.1115625000000002 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": 52.857102599919905 }, "reversed": false, - "folder": null, + "folder": "Reef to Bottom Feed", "idealStartingState": { "velocity": 0, "rotation": -179.46454101443553 diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path new file mode 100644 index 0000000..8f6baad --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.1059375, + "y": 4.037187499999999 + }, + "prevControl": null, + "nextControl": { + "x": 7.1784375, + "y": 6.4015625 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.121401515151515, + "y": 7.003093434343434 + }, + "prevControl": { + "x": 1.7184374999999998, + "y": 5.999375 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.93780932479242 + }, + "reversed": false, + "folder": "Reef to Top Feed", + "idealStartingState": { + "velocity": 0, + "rotation": 179.1574757392596 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Reef to Feed.path b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path similarity index 87% rename from src/main/deploy/pathplanner/paths/Top Reef to Feed.path rename to src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path index 40748ed..612587c 100644 --- a/src/main/deploy/pathplanner/paths/Top Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 4.844554924242423, - "y": 6.744466540404039 + "x": 4.996875, + "y": 6.8525 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 7.003093434343434 }, "prevControl": { - "x": 1.9644034090909093, - "y": 5.750085227272726 + "x": 2.34, + "y": 5.5484375 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": -54.83470564502973 }, "reversed": false, - "folder": null, + "folder": "Reef to Top Feed", "idealStartingState": { "velocity": 0, "rotation": -121.0370223454415 diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path index c4ec989..c5d3778 100644 --- a/src/main/deploy/pathplanner/paths/Taxi.path +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -16,28 +16,12 @@ }, { "anchor": { - "x": 6.305113636363636, + "x": 6.0571875, "y": 7.348674242424242 }, "prevControl": { - "x": 5.928529040722133, - "y": 7.353679406601922 - }, - "nextControl": { - "x": 6.681698232005139, - "y": 7.343669078246562 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.8889204545454543, - "y": 7.348674242424242 - }, - "prevControl": { - "x": 4.138853417354843, - "y": 7.3544633789160185 + "x": 6.306992295436264, + "y": 7.338796784398704 }, "nextControl": null, "isLocked": false, @@ -58,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 179.59913485447024 + "rotation": 179.5653636269807 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Top Feed to Reef.path b/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path similarity index 97% rename from src/main/deploy/pathplanner/paths/Top Feed to Reef.path rename to src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path index fce25be..42b5477 100644 --- a/src/main/deploy/pathplanner/paths/Top Feed to Reef.path +++ b/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path @@ -45,7 +45,7 @@ "rotation": -59.239047023115106 }, "reversed": false, - "folder": null, + "folder": "Top Feed to Reef", "idealStartingState": { "velocity": 0, "rotation": -52.93323259086456 diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 5bf438c..e658ac9 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,7 +2,13 @@ "robotWidth": 0.9, "robotLength": 0.9, "holonomicMode": true, - "pathFolders": [], + "pathFolders": [ + "Barge to Reef", + "Bottom Feed to Reef", + "Reef to Top Feed", + "Reef to Bottom Feed", + "Top Feed to Reef" + ], "autoFolders": [], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7ad5266..064ce54 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -90,7 +90,7 @@ public class RobotContainer { public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Lidar m_lidar = new Lidar(); - public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED); + public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_vision, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); public final Climber m_robotClimber = new Climber(m_robotMap.climber); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -332,10 +332,9 @@ public class RobotContainer { NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right); - NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup( - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar) - )); + NamedCommands.registerCommand("enable-preraise-l4", new InstantCommand(() -> { + m_robotElevator.enablePreRaiseElevator(); + })); configureButtonBindings(); configureVirtualButtonBindings(); From fe5466484b77d67ee5baa6b85ed637757fb43226 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Mon, 3 Mar 2025 16:45:07 -0700 Subject: [PATCH 03/29] Revert "Merge pull request #40 from Team4388/Climber" This reverts commit 97630fe2b456542fb5889d181065b53f75a81bfe, reversing changes made to 1763225d5349eda3d04e4bec55a57d38059d00c6. --- src/main/java/frc4388/robot/Constants.java | 6 +-- .../java/frc4388/robot/RobotContainer.java | 10 ----- src/main/java/frc4388/robot/RobotMap.java | 18 +++++--- .../frc4388/robot/subsystems/Climber.java | 43 ------------------- 4 files changed, 13 insertions(+), 64 deletions(-) delete mode 100644 src/main/java/frc4388/robot/subsystems/Climber.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e9cce41..70df5bc 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -21,7 +21,6 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; @@ -419,10 +418,7 @@ public final class Constants { public static final double LEFT_AXIS_DEADBAND = 0.1; } - public static final class ClimberConstants { - public static final CanDevice CLIMBER_ID = new CanDevice("Climber", 311); - public static final double CLIMBER_SPEED = 0.1; - } + public static final class ElevatorConstants { public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15); public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 064ce54..982e1a7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -61,7 +61,6 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Elevator.CoordinationState; import frc4388.robot.subsystems.Lidar; -import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Elevator; // import frc4388.robot.subsystems.Endeffector; import frc4388.robot.subsystems.SwerveDrive; @@ -92,7 +91,6 @@ public class RobotContainer { public final Lidar m_lidar = new Lidar(); public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_vision, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); - public final Climber m_robotClimber = new Climber(m_robotMap.climber); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -559,14 +557,6 @@ public class RobotContainer { new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0 && !operatorManualMode) .onTrue(rightAlgaeRemove); - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90) - .onTrue(new InstantCommand(() -> m_robotClimber.climberOut())) - .onFalse(new InstantCommand(() -> m_robotClimber.stopClimber())); - - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270) - .onTrue(new InstantCommand(() -> m_robotClimber.climberIn())) - .onFalse(new InstantCommand(() -> m_robotClimber.stopClimber())); - // ? /* Programer Buttons (Controller 3)*/ // * /* Auto Recording */ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index cd58c9c..0313a70 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,16 +7,25 @@ package frc4388.robot; +import com.ctre.phoenix6.hardware.TalonFX; + import org.photonvision.PhotonCamera; import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.wpilibj.DigitalInput; -import frc4388.robot.Constants.ClimberConstants; import frc4388.robot.Constants.ElevatorConstants; +// import edu.wpi.first.wpilibj.motorcontrol.Spark; +// import frc4388.robot.Constants.LEDConstants; +import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.VisionConstants; +// import frc4388.robot.subsystems.SwerveModule; +import frc4388.utility.RobotGyro; /** * Defines and holds all I/O objects on the Roborio. This is useful for unit @@ -50,9 +59,6 @@ public class RobotMap { public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); - /* Climber Subsytem */ - public final TalonFX climber = new TalonFX(ClimberConstants.CLIMBER_ID.id); - void configureDriveMotorControllers() {} - + } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java deleted file mode 100644 index 34d8e65..0000000 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ /dev/null @@ -1,43 +0,0 @@ -// 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.subsystems; - -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.ClimberConstants; - -public class Climber extends SubsystemBase { - /** Creates a new Climber. */ - - public TalonFX climberMotor; - - public Climber(TalonFX climberTalonFX) { - climberMotor = climberTalonFX; - climberMotor.setNeutralMode(NeutralModeValue.Brake); - - } - - public void stopClimber(){ - climberMotor.set(0); - } - - - public void climberOut(){ - climberMotor.set(ClimberConstants.CLIMBER_SPEED); - - } - - public void climberIn(){ - climberMotor.set(-ClimberConstants.CLIMBER_SPEED); - - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} From 8bea59723078dfda9653a8fda0e8ddb62e557c94 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Mon, 3 Mar 2025 17:49:06 -0700 Subject: [PATCH 04/29] i --- .../pathplanner/autos/2 Coral Cage 1.auto | 12 +++++++ .../pathplanner/autos/2 Coral Cage 5.auto | 12 +++++++ src/main/java/frc4388/robot/Constants.java | 2 ++ .../java/frc4388/robot/RobotContainer.java | 15 +++++--- src/main/java/frc4388/robot/RobotMap.java | 1 + .../frc4388/robot/commands/waitSupplier.java | 36 +++++++++++++++++++ .../frc4388/robot/subsystems/Elevator.java | 20 +++++++---- 7 files changed, 86 insertions(+), 12 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/waitSupplier.java diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto index e35d8f0..fd705bb 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { @@ -40,6 +46,12 @@ "pathName": "Top Feed to Reef 6" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto index 387787a..c538e81 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { @@ -40,6 +46,12 @@ "pathName": "Bottom Feed to Reef 2" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 70df5bc..731bb76 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -427,6 +427,8 @@ public final class Constants { public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND + public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND + public static final double GEAR_RATIO_ELEVATOR = -9.0; //Max for elevator = 50% diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 982e1a7..990fd31 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -49,6 +49,7 @@ import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.waitElevatorRefrence; import frc4388.robot.commands.waitEndefectorRefrence; import frc4388.robot.commands.waitFeedCoral; +import frc4388.robot.commands.waitSupplier; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; @@ -89,7 +90,7 @@ public class RobotContainer { public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Lidar m_lidar = new Lidar(); - public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_vision, m_robotLED); + public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.basinLimitSwitch, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -108,7 +109,7 @@ public class RobotContainer { // ! Teleop Commands public void stop() { - new InstantCommand(()->{}, m_robotSwerveDrive).schedule();;;;; + new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); m_robotSwerveDrive.stopModules(); } @@ -117,6 +118,8 @@ public class RobotContainer { private SendableChooser autoChooser; private Command autoCommand; + private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove); + // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, // () -> autoplaybackName.get(), // lastAutoName @@ -316,7 +319,9 @@ public class RobotContainer { */ public RobotContainer() { - NamedCommands.registerCommand("grab-coral", new waitFeedCoral(m_robotElevator)); + NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy()); + NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator)); + NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0, 1), @@ -330,8 +335,8 @@ public class RobotContainer { NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right); - NamedCommands.registerCommand("enable-preraise-l4", new InstantCommand(() -> { - m_robotElevator.enablePreRaiseElevator(); + NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> { + m_robotElevator.transitionState(CoordinationState.PrimedFour); })); configureButtonBindings(); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 0313a70..72bafdd 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -58,6 +58,7 @@ public class RobotMap { public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); + public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); void configureDriveMotorControllers() {} diff --git a/src/main/java/frc4388/robot/commands/waitSupplier.java b/src/main/java/frc4388/robot/commands/waitSupplier.java new file mode 100644 index 0000000..0ea5a3b --- /dev/null +++ b/src/main/java/frc4388/robot/commands/waitSupplier.java @@ -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 java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; + +/* 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 waitSupplier extends Command { + /** Creates a new waitSupplier. */ + private final Supplier truth; + public waitSupplier(Supplier truth) { + this.truth = truth; + } + + // 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 truth.get(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 06162a4..57d119c 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -49,7 +49,7 @@ public class Elevator extends Subsystem { private DigitalInput basinBeamBreak; private DigitalInput endeffectorLimitSwitch; - // private DigitalInput intakeIR; + private DigitalInput intakeIR; public enum CoordinationState { Waiting, // for coral into the though @@ -69,15 +69,15 @@ public class Elevator extends Subsystem { private CoordinationState currentState; - public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { - // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) { + // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { + public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) { elevatorMotor = elevatorTalonFX; endeffectorMotor = endeffectorTalonFX; this.led = led; this.basinBeamBreak = basinLimitSwitch; this.endeffectorLimitSwitch = endeffectorLimitSwitch; - // this.intakeIR = intakeDigitalInput; + this.intakeIR = intakeDigitalInput; elevatorMotor.setNeutralMode(NeutralModeValue.Brake); endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); @@ -294,6 +294,7 @@ public class Elevator extends Subsystem { // This method will be called once per scheduler run SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); + SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); SmartDashboard.putString("State", currentState.toString()); if (disableAutoIntake) return; @@ -310,18 +311,23 @@ public class Elevator extends Subsystem { // } } - public boolean isL4Primed(){ + public boolean isL4Primed() { return currentState == CoordinationState.PrimedFour; } - public boolean isL3Primed(){ + public boolean isL3Primed() { return currentState == CoordinationState.PrimedThree; } - public boolean hasCoral(){ + public boolean hasCoral() { return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get(); } + public boolean readyToMove() { + // return !intakeIR.get() || hasCoral(); + return hasCoral(); + } + public void armShuffle(){ if(!basinBeamBreak.get()){ //shuffle the coral with the arm until coral hits beam break From 6762ed16e65bf93bd8cbb43449b5f204d3c33af6 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Mon, 3 Mar 2025 19:43:36 -0700 Subject: [PATCH 05/29] Elevator L2 Fix --- src/main/java/frc4388/robot/Constants.java | 5 ++++- src/main/java/frc4388/robot/subsystems/Elevator.java | 2 +- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 3 ++- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 731bb76..ceeeda9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -329,6 +329,8 @@ public final class Constants { public static final class AutoConstants { public static final Gains XY_GAINS = new Gains(3,0.3,0.0); + // public static final Gains XY_GAINS = new Gains(3,0.3,0.0); + public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); @@ -353,7 +355,7 @@ public final class Constants { public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); - public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(1); + public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); @@ -445,6 +447,7 @@ public final class Constants { 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 L2_SCORE_ELEVATOR = -5; public static final double L2_SCORE_ENDEFFECTOR = -19; diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 57d119c..bc41f4b 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -139,7 +139,7 @@ public class Elevator extends Subsystem { } case L2Score: { - PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); break; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f8770a0..54855a9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -326,7 +326,8 @@ public class SwerveDrive extends Subsystem { rotTarget += deltaAngle; } - swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); + swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);// - 0.020); + //back to the ~~future~~ *past* } // if(e.isPresent()) From 7dc6105145fd789a8f33cb4d5535c59335736050 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 4 Mar 2025 19:46:52 -0700 Subject: [PATCH 06/29] Swerve calibration, L2, Autos --- .../pathplanner/autos/3 Coral Cage 5.auto | 8 +++- .../pathplanner/paths/Cage 5 to Reef.path | 12 ++--- src/main/java/frc4388/robot/Constants.java | 48 ++++--------------- .../java/frc4388/robot/RobotContainer.java | 4 +- .../frc4388/robot/subsystems/Elevator.java | 8 +++- .../frc4388/robot/subsystems/SwerveDrive.java | 2 + .../frc4388/utility/ReefPositionHelper.java | 2 + 7 files changed, 34 insertions(+), 50 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto index 1145cb6..a04fbe0 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { @@ -13,7 +19,7 @@ { "type": "named", "data": { - "name": "place-coral-right-l4" + "name": "place-coral-left-l4" } }, { diff --git a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path index e015f20..44a5838 100644 --- a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.585795454545455, - "y": 1.9413510101010092 + "x": 7.129687499999999, + "y": 1.9409375 }, "prevControl": null, "nextControl": { - "x": 7.044375, - "y": 1.8921875 + "x": 6.072698863636364, + "y": 1.0434943181818186 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.61125 }, "prevControl": { - "x": 5.3503125, - "y": 2.1603125 + "x": 5.504318181818181, + "y": 2.1902272727272725 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ceeeda9..a24aa67 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -113,7 +113,7 @@ public final class Constants { private static final class ModuleSpecificConstants { //2025 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.4794921875+0.25); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.22705078125+0.5); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; @@ -121,7 +121,7 @@ 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.5); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.07666015625); 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; @@ -129,7 +129,7 @@ public final class Constants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(-0.867431640625+0.25); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.37646484375); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; @@ -137,7 +137,7 @@ public final class Constants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.425537109375-0.25+0.25); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.219970703125+0.5); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; @@ -145,40 +145,6 @@ public final class Constants { private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); } - /* private static final class ModuleSpecificConstants { // 2024 - //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); - private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; - private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; - private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; - private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); - - //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625); - private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; - 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); - private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); - - //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5); - private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; - private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; - private static final boolean BACK_LEFT_ENCODER_INVERTED = false; - private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); - - //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5); - private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; - private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; - private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; - private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); - } */ - public static final class IDs { public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4); public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 5); @@ -328,10 +294,11 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains XY_GAINS = new Gains(3,0.3,0.0); + public static final Gains XY_GAINS = new Gains(5,0.4,0.0); // public static final Gains XY_GAINS = new Gains(3,0.3,0.0); - public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); + public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); + // 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.5, 0); public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); @@ -355,6 +322,7 @@ public final class Constants { public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 990fd31..f214584 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -90,7 +90,7 @@ public class RobotContainer { public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Lidar m_lidar = new Lidar(); - public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.basinLimitSwitch, m_robotLED); + public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -250,6 +250,7 @@ public class RobotContainer { private Command AprilLidarAlignL2Left = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT), new LidarAlign(m_robotSwerveDrive, m_lidar), @@ -265,6 +266,7 @@ public class RobotContainer { private Command AprilLidarAlignL2Right = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT), new LidarAlign(m_robotSwerveDrive, m_lidar), diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index bc41f4b..9f5cf56 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -306,6 +306,10 @@ public class Elevator extends Subsystem { } else if (currentState == CoordinationState.Ready) { periodicReady(); } + + if(!intakeIR.get()){ + led.setMode(LEDConstants.DOWN_PATTERN); + } // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { // periodicScoring(); // } @@ -324,8 +328,8 @@ public class Elevator extends Subsystem { } public boolean readyToMove() { - // return !intakeIR.get() || hasCoral(); - return hasCoral(); + return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get(); + // return hasCoral(); } public void armShuffle(){ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 54855a9..6f7ef5e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -184,10 +184,12 @@ public class SwerveDrive extends Subsystem { public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) { stopped = false; // Create robot-relative speeds. + if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0); swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + } diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index a3ab63e..7c020b7 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -56,6 +56,8 @@ public class ReefPositionHelper { minDistance = dist; } } + + System.out.println(minPos.getRotation().getDegrees()); return minPos; } From 357b054c8642dc12b8bf76865776e6c4efded8ed Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 5 Mar 2025 12:15:45 -0700 Subject: [PATCH 07/29] April align now uses robot oriented --- src/main/java/frc4388/robot/Constants.java | 2 +- .../frc4388/robot/commands/GotoLastApril.java | 38 +++++++++---------- .../frc4388/robot/commands/LidarAlign.java | 2 +- 3 files changed, 19 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a24aa67..adb29c4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -297,7 +297,7 @@ public final class Constants { public static final Gains XY_GAINS = new Gains(5,0.4,0.0); // public static final Gains XY_GAINS = new Gains(3,0.3,0.0); - public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); + // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); // 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.5, 0); diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index 47b0430..7e9d1db 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -31,7 +31,7 @@ public class GotoLastApril extends Command { private PID xPID = new PID(AutoConstants.XY_GAINS, 0); private PID yPID = new PID(AutoConstants.XY_GAINS, 0); - private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); + // private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); private Pose2d targetpos; SwerveDrive swerveDrive; @@ -58,18 +58,16 @@ 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, - Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), - distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())); - Optional a = DriverStation.getAlliance(); - if(!a.isEmpty()) - alliance = a.get(); + this.targetpos = ReefPositionHelper.getNearestPosition( + this.vision.getPose2d(), + side, + Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), + distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get()) + ); } double xerr; @@ -91,8 +89,6 @@ public class GotoLastApril extends Command { roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees())); - boolean invert = Math.abs(roterr) > 180; - if(roterr > 180){ roterr -= 360; }else if(roterr < -180){ @@ -108,11 +104,11 @@ public class GotoLastApril extends Command { xoutput = xPID.update(xerr); youtput = yPID.update(yerr); - rotoutput = rotPID.update(roterr); + // 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; + // rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1; @@ -122,19 +118,18 @@ public class GotoLastApril extends Command { // 0,0 ); - Translation2d rightStick = new Translation2d( - Math.max(Math.min(rotoutput, 1), -1), - 0 - ); - - + // Translation2d rightStick = new Translation2d( + // Math.max(Math.min(rotoutput, 1), -1), + // 0 + // ); setTagRelativeXError( new Translation2d(xerr, yerr).getAngle() .rotateBy(targetpos.getRotation()) .getCos()); - swerveDrive.driveWithInput(leftStick, rightStick, true); + swerveDrive.driveRelativeLockedAngle(leftStick, targetpos.getRotation()); + // swerveDrive.driveWithInputOrientation(leftStick, rightStick, true); } @Override @@ -142,7 +137,8 @@ public class GotoLastApril extends Command { boolean finished = ( (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && - (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE || Math.abs(rotoutput) <= AutoConstants.MIN_ROT_PID_OUTPUT)); + (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) + ); // System.out.println(finished); if(finished) diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index ce5c147..d4c0c82 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -97,7 +97,7 @@ public class LidarAlign extends Command { currentFinderTick = 0; foundReef = false; return false; - } else if (bounces == 2) { + } else if (bounces >= 2) { swerveDrive.stopModules(); return true; } else { From aeedebd0955a49903893fffffa8bce6358627bc8 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 5 Mar 2025 13:07:28 -0700 Subject: [PATCH 08/29] Add Loop Command --- .../java/frc4388/robot/RobotContainer.java | 21 +++- .../robot/commands/WhileTrueCommand.java | 103 ++++++++++++++++++ 2 files changed, 118 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/WhileTrueCommand.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f214584..c938162 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -46,6 +46,7 @@ import frc4388.utility.controller.VirtualController; import frc4388.robot.commands.GotoLastApril; import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.MoveForTimeCommand; +import frc4388.robot.commands.WhileTrueCommand; import frc4388.robot.commands.waitElevatorRefrence; import frc4388.robot.commands.waitEndefectorRefrence; import frc4388.robot.commands.waitFeedCoral; @@ -316,6 +317,12 @@ public class RobotContainer { private Boolean operatorManualMode = false; + public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left, () -> !m_robotElevator.hasCoral()); + public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right, () -> !m_robotElevator.hasCoral()); + public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left, () -> !m_robotElevator.hasCoral()); + public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right, () -> !m_robotElevator.hasCoral()); + public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left, () -> !m_robotElevator.hasCoral()); + public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right, () -> !m_robotElevator.hasCoral()); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -330,12 +337,12 @@ public class RobotContainer { new Translation2d(), 400, true ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); - NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); - NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); - NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left); - NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right); - NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); - NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right); + NamedCommands.registerCommand("place-coral-left-l4", LoopAprilLidarAlignL4Left); + NamedCommands.registerCommand("place-coral-right-l4", LoopAprilLidarAlignL4Right); + NamedCommands.registerCommand("place-coral-left-l3", LoopAprilLidarAlignL3Left); + NamedCommands.registerCommand("place-coral-right-l3", LoopAprilLidarAlignL3Right); + NamedCommands.registerCommand("place-coral-left-l2", LoopAprilLidarAlignL2Left); + NamedCommands.registerCommand("place-coral-right-l2", LoopAprilLidarAlignL2Right); NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> { m_robotElevator.transitionState(CoordinationState.PrimedFour); @@ -639,6 +646,8 @@ public class RobotContainer { // File dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\"); String[] autos = dir.list(); + if(autos == null) return; + for (String auto : autos) { if (auto.endsWith(".auto")) autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", "")); diff --git a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java new file mode 100644 index 0000000..31b189e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java @@ -0,0 +1,103 @@ +package frc4388.robot.commands; + +// 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. + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +import java.util.function.BooleanSupplier; + +/** + * A command composition that runs one of two commands, depending on the value of the given + * condition when this command is initialized. + * + *

The rules for command compositions apply: command instances that are passed to it cannot be + * added to any other composition or scheduled individually, and the composition requires all + * subsystems its components require. + * + *

This class is provided by the NewCommands VendorDep + */ +public class WhileTrueCommand extends Command { + private final Command m_whileTrue; + private final BooleanSupplier m_condition; + + /** + * Creates a new WhileTrueCommand. + * + * @param whileTrue the command to run while the condition is true + * @param condition the condition to determine which command to run + */ + @SuppressWarnings("this-escape") + public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) { + m_whileTrue = requireNonNullParam(whileTrue, "onTrue", "WhileTrueCommand"); + m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand"); + + CommandScheduler.getInstance().registerComposedCommands(whileTrue); + + addRequirements(whileTrue.getRequirements()); + } + + @Override + public void initialize() { + if(m_condition.getAsBoolean()) + m_whileTrue.initialize(); + } + + @Override + public void execute() { + m_whileTrue.execute(); + + if(!m_whileTrue.isFinished()) + return; + + if(m_condition.getAsBoolean()){ + m_whileTrue.end(false); + m_whileTrue.initialize(); + } + } + + @Override + public void end(boolean interrupted) { + m_whileTrue.end(interrupted); + } + + @Override + public boolean isFinished() { + return !m_condition.getAsBoolean() && m_whileTrue.isFinished(); + } + + @Override + public boolean runsWhenDisabled() { + return m_whileTrue.runsWhenDisabled(); + } + + @Override + public InterruptionBehavior getInterruptionBehavior() { + if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) { + return InterruptionBehavior.kCancelSelf; + } else { + return InterruptionBehavior.kCancelIncoming; + } + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + builder.addStringProperty("whileTrue", m_whileTrue::getName, null); + builder.addStringProperty( + "selected", + () -> { + if (m_whileTrue == null) { + return "null"; + } else { + return m_whileTrue.getName(); + } + }, + null); + } +} From a6daadb537e6283af127692cb055b49484ade4ab Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 5 Mar 2025 19:42:36 -0700 Subject: [PATCH 09/29] Work on Autos --- .../pathplanner/autos/3 Coral Cage 4.auto | 85 +++++++++++++++++++ .../pathplanner/autos/3 Coral Cage 5.auto | 12 --- .../paths/Bottom Feed to Reef 2.path | 8 +- .../pathplanner/paths/Cage 4 to Reef.path | 14 +-- .../pathplanner/paths/Cage 5 to Reef.path | 20 ++--- .../pathplanner/paths/Cage 6 to Reef.path | 14 +-- .../paths/Reef 2 to Bottom Feed.path | 22 ++--- .../paths/Reef 3 to Bottom Feed.path | 22 ++--- .../paths/Reef 4 to Bottom Feed.path | 2 +- src/main/java/frc4388/robot/Constants.java | 8 +- .../java/frc4388/robot/RobotContainer.java | 33 ++++--- .../frc4388/robot/commands/GotoLastApril.java | 13 +-- .../frc4388/robot/commands/LidarAlign.java | 2 +- .../robot/commands/WhileTrueCommand.java | 10 ++- .../frc4388/robot/subsystems/SwerveDrive.java | 19 ++++- 15 files changed, 191 insertions(+), 93 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto new file mode 100644 index 0000000..e51e2f1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 4 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto index a04fbe0..b9b9e6b 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -28,12 +28,6 @@ "pathName": "Reef 3 to Bottom Feed" } }, - { - "type": "named", - "data": { - "name": "align-feed" - } - }, { "type": "named", "data": { @@ -58,12 +52,6 @@ "pathName": "Reef 2 to Bottom Feed" } }, - { - "type": "named", - "data": { - "name": "align-feed" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path index bef1742..6c86f54 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.0603125, - "y": 1.0390625 + "x": 1.0807449494949497, + "y": 1.0875631313131315 }, "prevControl": null, "nextControl": { - "x": 2.3463856916648673, - "y": 1.797152889765366 + "x": 2.3668181411598166, + "y": 1.8456535210784974 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path index e904b37..6a79779 100644 --- a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.595959595959596, - "y": 3.0289141414141416 + "x": 7.006439393939393, + "y": 3.1508838383838382 }, "prevControl": null, "nextControl": { - "x": 6.2521875, - "y": 2.6234375000000005 + "x": 6.498232323232323, + "y": 2.480050505050505 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.6934974747474745 }, "prevControl": { - "x": 5.630625, - "y": 2.148125 + "x": 5.979861111111112, + "y": 1.992171717171717 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": -178.9832812445108 + "rotation": 124.11447294534119 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path index 44a5838..4d15bb2 100644 --- a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.129687499999999, - "y": 1.9409375 + "x": 7.016603535353535, + "y": 1.9718434343434341 }, "prevControl": null, "nextControl": { - "x": 6.072698863636364, - "y": 1.0434943181818186 + "x": 6.000189393939394, + "y": 1.6364267676767676 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.2771875, - "y": 2.61125 + "x": 5.47165404040404, + "y": 2.1141414141414145 }, "prevControl": { - "x": 5.504318181818181, - "y": 2.1902272727272725 + "x": 5.86940340909091, + "y": 1.7593308080808083 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 118.99790479482637 + "rotation": 124.8244891569568 }, "reversed": false, "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": 179.5295681977034 + "rotation": 126.34745820888533 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path index 42a1ac9..f42fbe9 100644 --- a/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.565467171717171, - "y": 0.8334595959595947 + "x": 7.057260101010102, + "y": 0.8537878787878788 }, "prevControl": null, "nextControl": { - "x": 6.625889381464077, - "y": 1.4477106264692527 + "x": 6.315277777777778, + "y": 1.5144570707070704 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.6234375 }, "prevControl": { - "x": 5.721332938841585, - "y": 2.265481302925542 + "x": 5.624116161616161, + "y": 2.205618686868687 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": -179.94258861633824 + "rotation": 132.5633517531898 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path index 562a21c..bd485e9 100644 --- a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.680625, - "y": 2.61125 + "x": 3.749318181818182, + "y": 2.868295454545454 }, "prevControl": null, "nextControl": { - "x": 3.2541591082344743, - "y": 2.3264921263556886 + "x": 3.093244949494949, + "y": 2.2869318181818175 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.12125, - "y": 1.0025 + "x": 1.3246843434343436, + "y": 1.5347853535353522 }, "prevControl": { - "x": 1.3989083079519464, - "y": 1.1682576997602765 + "x": 1.802550505050505, + "y": 2.1992045454545446 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 58.10920819815426 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path index 48b8fd6..9df45e7 100644 --- a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.265, - "y": 2.61125 + "x": 5.268371212121212, + "y": 2.4597222222222217 }, "prevControl": null, "nextControl": { - "x": 4.137208815063533, - "y": 2.0700494801809173 + "x": 4.670075757575758, + "y": 1.2531597222222217 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.096875, - "y": 1.0390625 + "x": 1.1010732323232322, + "y": 1.00625 }, "prevControl": { - "x": 1.3299317623663032, - "y": 1.1501468126694903 + "x": 1.786268939393939, + "y": 2.2439551767676766 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 121.42956561483854 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path index cbd1bbf..42d3c4c 100644 --- a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path @@ -20,7 +20,7 @@ "y": 1.0146875 }, "prevControl": { - "x": 1.95, + "x": 1.9500000000000002, "y": 2.1115625000000002 }, "nextControl": null, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index adb29c4..a37693b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -193,7 +193,7 @@ public final class Constants { .withKS(0).withKV(0.124); public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); - public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST + public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0.2, 0.1); // TODO: TEST } public static final class Configurations { @@ -294,7 +294,7 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains XY_GAINS = new Gains(5,0.4,0.0); + public static final Gains XY_GAINS = new Gains(5,0.6,0.0); // public static final Gains XY_GAINS = new Gains(3,0.3,0.0); // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); @@ -316,10 +316,10 @@ public final class Constants { public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16); - public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); + public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); - public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); + public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c938162..5eed240 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -317,12 +317,19 @@ public class RobotContainer { private Boolean operatorManualMode = false; - public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left, () -> !m_robotElevator.hasCoral()); - public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right, () -> !m_robotElevator.hasCoral()); - public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left, () -> !m_robotElevator.hasCoral()); - public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right, () -> !m_robotElevator.hasCoral()); - public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left, () -> !m_robotElevator.hasCoral()); - public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right, () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right.asProxy(), () -> !m_robotElevator.hasCoral()); + + // public Command LoopAprilLidarAlignL4Left = new SequentialCommandGroup(AprilLidarAlignL4Left.asProxy(), new ConditionalCommand(AprilLidarAlignL4Left.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL4Right = new SequentialCommandGroup(AprilLidarAlignL4Right.asProxy(), new ConditionalCommand(AprilLidarAlignL4Right.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL3Left = new SequentialCommandGroup(AprilLidarAlignL3Left.asProxy(), new ConditionalCommand(AprilLidarAlignL3Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL3Right = new SequentialCommandGroup(AprilLidarAlignL3Right.asProxy(), new ConditionalCommand(AprilLidarAlignL3Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL2Left = new SequentialCommandGroup(AprilLidarAlignL2Left.asProxy(), new ConditionalCommand(AprilLidarAlignL2Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL2Right = new SequentialCommandGroup(AprilLidarAlignL2Right.asProxy(), new ConditionalCommand(AprilLidarAlignL2Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral())); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -334,15 +341,15 @@ public class RobotContainer { NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0, 1), - new Translation2d(), 400, true + new Translation2d(), 300, true ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); - NamedCommands.registerCommand("place-coral-left-l4", LoopAprilLidarAlignL4Left); - NamedCommands.registerCommand("place-coral-right-l4", LoopAprilLidarAlignL4Right); - NamedCommands.registerCommand("place-coral-left-l3", LoopAprilLidarAlignL3Left); - NamedCommands.registerCommand("place-coral-right-l3", LoopAprilLidarAlignL3Right); - NamedCommands.registerCommand("place-coral-left-l2", LoopAprilLidarAlignL2Left); - NamedCommands.registerCommand("place-coral-right-l2", LoopAprilLidarAlignL2Right); + NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); + NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); + NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left); + NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right); + NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); + NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right); NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> { m_robotElevator.transitionState(CoordinationState.PrimedFour); diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index 7e9d1db..e0320ad 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -95,12 +95,13 @@ public class GotoLastApril extends Command { roterr += 360; } - SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees()); - SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees()); - SmartDashboard.putNumber("Rotational PID error", roterr); + // SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees()); + // SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees()); + // SmartDashboard.putNumber("Rotational PID error", roterr); - // SmartDashboard.putNumber("PID X Error", xerr); - // SmartDashboard.putNumber("PID Y Error", yerr); + SmartDashboard.putNumber("PID X Error", xerr); + SmartDashboard.putNumber("PID Y Error", yerr); + SmartDashboard.putNumber("PID Rot Error", roterr); xoutput = xPID.update(xerr); youtput = yPID.update(yerr); @@ -128,7 +129,7 @@ public class GotoLastApril extends Command { .rotateBy(targetpos.getRotation()) .getCos()); - swerveDrive.driveRelativeLockedAngle(leftStick, targetpos.getRotation()); + swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation()); // swerveDrive.driveWithInputOrientation(leftStick, rightStick, true); } diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index d4c0c82..050cff2 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -41,7 +41,7 @@ public class LidarAlign extends Command { this.currentFinderTick = 0; this.speed = 0.4; // TODO: find good speed for this this.foundReef = false; - this.headedRight = !(GotoLastApril.tagRelativeXError < 0); + this.headedRight = (GotoLastApril.tagRelativeXError < 0); this.additionalDistance = 0; this.bounces = 0; } diff --git a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java index 31b189e..8db5f64 100644 --- a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java +++ b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java @@ -34,24 +34,26 @@ public class WhileTrueCommand extends Command { */ @SuppressWarnings("this-escape") public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) { - m_whileTrue = requireNonNullParam(whileTrue, "onTrue", "WhileTrueCommand"); + m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand"); m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand"); - CommandScheduler.getInstance().registerComposedCommands(whileTrue); + //CommandScheduler.getInstance().registerComposedCommands(whileTrue); - addRequirements(whileTrue.getRequirements()); + // addRequirements(whileTrue.getRequirements()); } @Override public void initialize() { if(m_condition.getAsBoolean()) - m_whileTrue.initialize(); + m_whileTrue.initialize(); } @Override public void execute() { m_whileTrue.execute(); + System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean()); + if(!m_whileTrue.isFinished()) return; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 6f7ef5e..1f7f9a1 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -141,7 +141,7 @@ public class SwerveDrive extends Subsystem { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput return; // don't bother doing swerve drive math and return early. - leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); stopped = false; if (fieldRelative) { @@ -213,13 +213,28 @@ public class SwerveDrive extends Subsystem { .withTargetDirection(rightStick.getAngle())); } + public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) { + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(heading); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + ); + swerveDriveTrain.setControl(ctrl); + } + public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { leftStick = leftStick.rotateBy(heading); var ctrl = new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) - .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); + .withTargetDirection(heading); ctrl.HeadingController.setPID( SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, From 90f8e0ee70d04a147f828f48aee2215937580964 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 6 Mar 2025 16:31:53 -0700 Subject: [PATCH 10/29] Add Odometry Speed --- .../frc4388/robot/subsystems/Elevator.java | 4 ++++ .../frc4388/robot/subsystems/SwerveDrive.java | 8 ++++++-- .../java/frc4388/robot/subsystems/Vision.java | 20 +++++++++++++------ 3 files changed, 24 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 9f5cf56..49b253f 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -16,6 +16,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -107,6 +108,9 @@ public class Elevator extends Subsystem { public void transitionState(CoordinationState state) { + // elevatorMotor.enable(); + + currentState = state; switch (currentState) { case Waiting: { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1f7f9a1..254204d 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -330,9 +330,13 @@ public class SwerveDrive extends Subsystem { SmartDashboard.putNumber("RotTartget", rotTarget); double time = Vision.getTime(); - + double freq = swerveDriveTrain.getOdometryFrequency(); - vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); + Optional curpose = swerveDriveTrain.samplePoseAt(time); + Optional lastPose = swerveDriveTrain.samplePoseAt(time - freq); + + vision.setLastOdomPose(curpose); + vision.setLastOdomSpeed(curpose, lastPose, freq); if (vision.isTag()) { Pose2d pose = vision.getPose2d(); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 347d741..b7cd090 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -24,6 +24,9 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; @@ -48,13 +51,14 @@ public class Vision extends Subsystem { private boolean isTagDetected = false; private boolean isTagProcessed = false; + public Pose2d lastVisionPose = new Pose2d(); private Pose2d lastPhysOdomPose = new Pose2d(); + private double lastOdomSpeed; private Matrix curStdDevs; private Field2d field = new Field2d(); - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") .getLayout(getSubsystemName(), BuiltInLayouts.kList) @@ -253,16 +257,20 @@ public class Vision extends Subsystem { - - - - - public void setLastOdomPose(Optional pose){ if(pose.isPresent()) lastPhysOdomPose = pose.get(); } + public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ + if(curPose.isPresent() && lastPose.isPresent()) + this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; + } + + public double getLastOdomSpeed(){ + return lastOdomSpeed; + } + public Pose2d getPose2d() { if(isTagDetected && isTagProcessed) // return lastVisionPose; From 85965a892a65557b8bc73917dc587787a5c76be6 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 6 Mar 2025 17:34:16 -0700 Subject: [PATCH 11/29] Min speed and endeffector motor safety --- .../pathplanner/autos/3 Coral Cage 5.auto | 6 +++++ src/main/java/frc4388/robot/Constants.java | 4 ++++ .../java/frc4388/robot/RobotContainer.java | 4 ++++ .../frc4388/robot/subsystems/Elevator.java | 14 +++++++++++ .../frc4388/robot/subsystems/SwerveDrive.java | 14 +++++++++-- .../java/frc4388/robot/subsystems/Vision.java | 23 +++++++++++-------- 6 files changed, 54 insertions(+), 11 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto index b9b9e6b..f0d0c95 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -22,6 +22,12 @@ "name": "place-coral-left-l4" } }, + { + "type": "named", + "data": { + "name": "stop" + } + }, { "type": "path", "data": { diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a37693b..150fffa 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -331,6 +331,7 @@ public final class Constants { // public static final int L3_DRIVE_TIME = 500; public static final int L2_DRIVE_TIME = 250; //Milliseconds public static final int ALGAE_DRIVE_TIME = 500; + public static final double STOP_VELOCITY = 0.25; } public static final class VisionConstants { @@ -393,6 +394,9 @@ 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 double SAFETY_ENDEFFECTOR_MAX_TORQUE = 0; + public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 999; + // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5eed240..f2cd73f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,6 +14,7 @@ import java.io.File; import java.util.ArrayList; import java.util.List; import java.util.function.BooleanSupplier; +import java.util.function.Supplier; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Rotation2d; @@ -338,6 +339,9 @@ public class RobotContainer { NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy()); NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator)); + + NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY)); + NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0, 1), diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 49b253f..e651ede 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -295,7 +295,19 @@ public class Elevator extends Subsystem { @Override public void periodic() { + // double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble(); + // double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble(); + double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble(); + double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble(); + + + if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){ + PIDPosition(elevatorMotor, endeffectorMotor.getPosition().getValueAsDouble()); + } + // This method will be called once per scheduler run + SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity); + SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque); SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); @@ -314,6 +326,8 @@ public class Elevator extends Subsystem { if(!intakeIR.get()){ led.setMode(LEDConstants.DOWN_PATTERN); } + + // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { // periodicScoring(); // } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 254204d..bb597f8 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -58,6 +58,8 @@ public class SwerveDrive extends Subsystem { public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to // 25% + public double lastOdomSpeed; + public Pose2d initalPose2d = null; @@ -336,7 +338,7 @@ public class SwerveDrive extends Subsystem { Optional lastPose = swerveDriveTrain.samplePoseAt(time - freq); vision.setLastOdomPose(curpose); - vision.setLastOdomSpeed(curpose, lastPose, freq); + setLastOdomSpeed(curpose, lastPose, freq); if (vision.isTag()) { Pose2d pose = vision.getPose2d(); @@ -347,7 +349,7 @@ public class SwerveDrive extends Subsystem { rotTarget += deltaAngle; } - swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);// - 0.020); + swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, vision.getLastLatency()); //back to the ~~future~~ *past* } @@ -424,6 +426,14 @@ public class SwerveDrive extends Subsystem { gear_index = tmp_gear_index; } + + + public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ + if(curPose.isPresent() && lastPose.isPresent()) + this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; + } + + @Override public String getSubsystemName() { return "Swerve Drive Controller"; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index b7cd090..c0e4305 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -52,9 +52,14 @@ public class Vision extends Subsystem { private boolean isTagDetected = false; private boolean isTagProcessed = false; + private double lastLatency = 0; + + public double getLastLatency() { + return lastLatency; + } + public Pose2d lastVisionPose = new Pose2d(); private Pose2d lastPhysOdomPose = new Pose2d(); - private double lastOdomSpeed; private Matrix curStdDevs; @@ -102,6 +107,7 @@ public class Vision extends Subsystem { public void periodic() { update(); field.setRobotPose(getPose2d()); + // cameras[0]. } @@ -115,6 +121,7 @@ public class Vision extends Subsystem { double X = 0; double Y = 0; double Yaw = 0; + double latency = 0; for(int i = 0; i < cameras.length; i++){ PhotonCamera camera = cameras[i]; @@ -128,6 +135,7 @@ public class Vision extends Subsystem { var result = results.get(results.size()-1); + latency += result.getTimestampSeconds(); isTagDetected = isTagDetected | result.hasTargets(); @@ -151,6 +159,8 @@ public class Vision extends Subsystem { } + lastLatency = latency / cams; + if(isTagProcessed){ lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(Yaw/cams)); } @@ -262,14 +272,9 @@ public class Vision extends Subsystem { lastPhysOdomPose = pose.get(); } - public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ - if(curPose.isPresent() && lastPose.isPresent()) - this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; - } - - public double getLastOdomSpeed(){ - return lastOdomSpeed; - } + // public double getLastOdomSpeed(){ + // return lastOdomSpeed; + // } public Pose2d getPose2d() { if(isTagDetected && isTagProcessed) From 5ae62c89e5876c4cab0066f7d26fe0bd361b077d Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 6 Mar 2025 19:55:26 -0700 Subject: [PATCH 12/29] Swerve calibration --- src/main/java/frc4388/robot/Constants.java | 20 +++++--- .../java/frc4388/robot/RobotContainer.java | 29 +++++++---- .../robot/commands/DriveUntilLiDAR.java | 50 +++++++++++++++++++ .../frc4388/robot/subsystems/Elevator.java | 2 +- .../java/frc4388/robot/subsystems/Lidar.java | 42 +++++++++------- .../frc4388/robot/subsystems/SwerveDrive.java | 12 ++--- 6 files changed, 112 insertions(+), 43 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 150fffa..059b974 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -113,7 +113,7 @@ public final class Constants { private static final class ModuleSpecificConstants { //2025 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.22705078125+0.5); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; @@ -121,7 +121,7 @@ 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.07666015625); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375); 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; @@ -129,7 +129,7 @@ public final class Constants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.37646484375); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; @@ -137,7 +137,7 @@ public final class Constants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.219970703125+0.5); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; @@ -287,8 +287,12 @@ public final class Constants { } public static final class LiDARConstants { + public static final int REEF_LIDAR_DIO_CHANNEL = 7; + public static final int REVERSE_LIDAR_DIO_CHANNEL = 4; + + public static final int HUMAN_PLAYER_STATION_DISTANCE = 25; + public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole - 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; } @@ -331,7 +335,7 @@ public final class Constants { // public static final int L3_DRIVE_TIME = 500; public static final int L2_DRIVE_TIME = 250; //Milliseconds public static final int ALGAE_DRIVE_TIME = 500; - public static final double STOP_VELOCITY = 0.25; + public static final double STOP_VELOCITY = 0.0; } public static final class VisionConstants { @@ -394,8 +398,8 @@ 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 double SAFETY_ENDEFFECTOR_MAX_TORQUE = 0; - public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 999; + public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75; + public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20; // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f2cd73f..38a76ac 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -28,6 +28,7 @@ import frc4388.utility.controller.XboxController; import frc4388.utility.controller.ButtonBox; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.robot.Constants.FieldConstants; +import frc4388.robot.Constants.LiDARConstants; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.AutoConstants; @@ -44,6 +45,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; // Autos import frc4388.utility.controller.VirtualController; +import frc4388.robot.commands.DriveUntilLiDAR; import frc4388.robot.commands.GotoLastApril; import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.MoveForTimeCommand; @@ -91,7 +93,8 @@ public class RobotContainer { /* Subsystems */ public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); - public final Lidar m_lidar = new Lidar(); + public final Lidar m_reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef"); + public final Lidar m_reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse"); public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -143,7 +146,7 @@ public class RobotContainer { new waitElevatorRefrence(m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), @@ -178,7 +181,7 @@ public class RobotContainer { new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), @@ -211,7 +214,7 @@ public class RobotContainer { new waitElevatorRefrence(m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -238,7 +241,7 @@ public class RobotContainer { new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), @@ -255,7 +258,7 @@ public class RobotContainer { new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -271,7 +274,7 @@ public class RobotContainer { new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -287,7 +290,7 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), @@ -301,7 +304,7 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), @@ -339,7 +342,8 @@ public class RobotContainer { NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy()); NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator)); - + NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, + new Translation2d(0,1), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, false)); NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY)); NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( @@ -478,11 +482,14 @@ public class RobotContainer { ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new LidarAlign(m_robotSwerveDrive, m_reefLidar)); + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); + .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_reefLidar)); // ? /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java new file mode 100644 index 0000000..029d7a6 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java @@ -0,0 +1,50 @@ +package frc4388.robot.commands; + +import java.time.Instant; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.Lidar; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.TimesNegativeOne; + +// Command to repeat a joystick movement for a specific time. +public class DriveUntilLiDAR extends Command { + private final SwerveDrive swerveDrive; + private final Translation2d leftStick; + private final Translation2d rightStick; + private final Lidar m_lidar; + private final double mindistance; + private final boolean robotRelative; + + public DriveUntilLiDAR( + SwerveDrive swerveDrive, + Translation2d leftStick, + Translation2d rightStick, + Lidar lidar, + double mindistance, + boolean robotRelative) { + addRequirements(swerveDrive); + + this.swerveDrive = swerveDrive; + this.leftStick = leftStick; + this.rightStick = rightStick; + this.m_lidar = lidar; + this.mindistance = mindistance; + this.robotRelative = robotRelative; + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + swerveDrive.driveFine(leftStick, rightStick, 0.1); + } + + @Override + public boolean isFinished() { + return Math.abs(m_lidar.getDistance()) < mindistance; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index e651ede..ebaf5d3 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -302,7 +302,7 @@ public class Elevator extends Subsystem { if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){ - PIDPosition(elevatorMotor, endeffectorMotor.getPosition().getValueAsDouble()); + PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble()); } // This method will be called once per scheduler run diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index 3ac5cff..e660301 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -15,13 +15,31 @@ import frc4388.utility.Status.ReportLevel; // https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface public class Lidar extends Subsystem { - private double distance = -1; - Counter LidarPWM = new Counter(LiDARConstants.LIDAR_DIO_CHANNEL); + private Counter LidarPWM; + private String name = "Lidar"; - public Lidar() { + private double distance = -1; + public Lidar(int port, String name) { + this.name = name; + LidarPWM = new Counter(port); LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement LidarPWM.reset(); + + + subsystemLayout = Shuffleboard.getTab("Subsystems") + .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .withSize(2, 2); + + sbDistance = subsystemLayout + .add("Distance", 0) + .withWidget(BuiltInWidgets.kGraph) + .getEntry(); + + sbWithinDistance = subsystemLayout + . add("Within Distance", 0) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); } @Override @@ -41,23 +59,13 @@ public class Lidar extends Subsystem { return distance < LiDARConstants.LIDAR_DETECT_DISTANCE; } - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) - .withSize(2, 2); - - GenericEntry sbDistance = subsystemLayout - .add("Distance", 0) - .withWidget(BuiltInWidgets.kGraph) - .getEntry(); - - GenericEntry sbWithinDistance = subsystemLayout - .add("Within Distance", 0) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); + ShuffleboardLayout subsystemLayout; + GenericEntry sbDistance; + GenericEntry sbWithinDistance; @Override public String getSubsystemName() { - return "Lidar"; + return "Lidar " + name; } @Override diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index bb597f8..a7fb68f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -237,11 +237,11 @@ public class SwerveDrive extends Subsystem { .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); - ctrl.HeadingController.setPID( - SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, - SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, - SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD - ); + // ctrl.HeadingController.setPID( + // SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + // SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + // SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + // ); swerveDriveTrain.setControl(ctrl); } @@ -349,7 +349,7 @@ public class SwerveDrive extends Subsystem { rotTarget += deltaAngle; } - swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, vision.getLastLatency()); + swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); //back to the ~~future~~ *past* } From 63808a652e502983f06e152ae758274f9175e7f8 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Fri, 7 Mar 2025 21:46:25 -0700 Subject: [PATCH 13/29] Greatly Improved auto alignment. --- src/main/java/frc4388/robot/Constants.java | 10 ++-- .../java/frc4388/robot/RobotContainer.java | 49 ++++++++++++------- .../frc4388/robot/commands/GotoLastApril.java | 7 ++- .../frc4388/robot/commands/LidarAlign.java | 2 +- .../frc4388/robot/subsystems/Elevator.java | 14 +++--- .../frc4388/robot/subsystems/SwerveDrive.java | 5 +- 6 files changed, 55 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 059b974..a392b2f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -193,7 +193,7 @@ public final class Constants { .withKS(0).withKV(0.124); public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); - public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0.2, 0.1); // TODO: TEST + public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(2.5, 0.2, 1); } public static final class Configurations { @@ -314,6 +314,8 @@ public final class Constants { public static final double MIN_XY_PID_OUTPUT = 0.0; public static final double MIN_ROT_PID_OUTPUT = 0.0; + + public static final double VELOCITY_THRESHHOLD = 0.005; // X is tangent to reef side // Y is normal to reef side @@ -324,10 +326,10 @@ public final class Constants { public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); - public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5); public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); - public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); + public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5); public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); @@ -343,7 +345,7 @@ public final class Constants { public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; 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+(1*-1)), -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 = 2; // Meters diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 38a76ac..fc17912 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -124,6 +124,8 @@ public class RobotContainer { private Command autoCommand; private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove); + // private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed); + private Command waitDebuger = new waitSupplier(() -> true); // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, @@ -136,7 +138,7 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT) + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true) ), () -> m_robotElevator.isL4Primed()), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), @@ -145,8 +147,10 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), + waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), @@ -170,7 +174,7 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT) + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true) ), () -> m_robotElevator.isL4Primed()), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), @@ -179,9 +183,10 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT), - + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), + waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), @@ -207,14 +212,16 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT) + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT, true) ), () -> m_robotElevator.isL3Primed()), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT, true), + waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -230,7 +237,7 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT) + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT, true) ),() -> m_robotElevator.isL3Primed()), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), @@ -239,9 +246,11 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true), + waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), @@ -255,10 +264,13 @@ public class RobotContainer { private Command AprilLidarAlignL2Left = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT, true), + waitDebuger.asProxy(), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true), + waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -271,10 +283,13 @@ public class RobotContainer { private Command AprilLidarAlignL2Right = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT, true), + waitDebuger.asProxy(), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true), + waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -289,7 +304,7 @@ public class RobotContainer { new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, @@ -303,7 +318,7 @@ public class RobotContainer { new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, @@ -482,8 +497,8 @@ public class RobotContainer { ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new LidarAlign(m_robotSwerveDrive, m_reefLidar)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .onTrue(new LidarAlign(m_robotSwerveDrive, m_reefLidar)); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy()); diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index e0320ad..b11f4d0 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -38,17 +38,19 @@ public class GotoLastApril extends Command { Vision vision; double distance; Side side; + boolean waitVelocityZero; /** * Command to drive robot to position. * @param SwerveDrive m_robotSwerveDrive */ - public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side) { + public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) { this.swerveDrive = swerveDrive; this.vision = vision; this.distance = distance; this.side = side; + this.waitVelocityZero = waitVelocityZero; // addRequirements(swerveDrive); } @@ -138,7 +140,8 @@ public class GotoLastApril extends Command { boolean finished = ( (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && - (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) + (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) && + (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) ); // System.out.println(finished); diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index 050cff2..8da22a3 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -97,7 +97,7 @@ public class LidarAlign extends Command { currentFinderTick = 0; foundReef = false; return false; - } else if (bounces >= 2) { + } else if (bounces >= 3) { swerveDrive.stopModules(); return true; } else { diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index ebaf5d3..17b6727 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -297,17 +297,17 @@ public class Elevator extends Subsystem { // double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble(); // double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble(); - double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble(); - double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble(); + // double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble(); + // double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble(); - if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){ - PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble()); - } + // if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){ + // PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble()); + // } // This method will be called once per scheduler run - SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity); - SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque); + // SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity); + // SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque); SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index a7fb68f..1c259e4 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -429,8 +429,11 @@ public class SwerveDrive extends Subsystem { public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ - if(curPose.isPresent() && lastPose.isPresent()) + if(curPose.isPresent() && lastPose.isPresent()){ this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; + + SmartDashboard.putNumber("Speed", lastOdomSpeed); + } } From c58a2406ac44d3f1a81bc3a2842e829ed873a107 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sat, 8 Mar 2025 13:22:14 -0700 Subject: [PATCH 14/29] Calibration --- src/main/java/frc4388/robot/Constants.java | 9 +-- .../java/frc4388/robot/RobotContainer.java | 67 ++++++++++++++----- .../frc4388/robot/subsystems/Elevator.java | 24 +++---- .../frc4388/robot/subsystems/SwerveDrive.java | 11 ++- .../java/frc4388/robot/subsystems/Vision.java | 29 +++++++- .../frc4388/utility/ReefPositionHelper.java | 7 +- 6 files changed, 110 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a392b2f..f562627 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -382,6 +382,7 @@ public final class Constants { public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED; public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW; public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK; + public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW; public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES; public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES; @@ -418,8 +419,8 @@ public final class Constants { public static final double HOVERING_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 DEALGAE_L2_ELEVATOR = -4; + public static final double DEALGAE_L3_ELEVATOR = -26.5; 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; @@ -429,8 +430,8 @@ public final class Constants { public static final double L2_SCORE_ENDEFFECTOR = -19; - public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; - public static final double DEALGAE_L2_ENDEFFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR; + public static final double COMPLETLY_DOWN_ENDEFFECTOR = 9001; + public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fc17912..5d3c31b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -299,31 +299,62 @@ public class RobotContainer { new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); - private Command leftAlgaeRemove = new SequentialCommandGroup( + // private Command leftAlgaeRemove = new SequentialCommandGroup( + // new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), + // new waitEndefectorRefrence(m_robotElevator), + // new waitElevatorRefrence(m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), + // new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + // new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + // ); + + private Command lowerAlgaeRemove = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true), + waitDebuger.asProxy(), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new Translation2d(1,0), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME / 2, true), + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME * 2, true), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); - private Command rightAlgaeRemove = new SequentialCommandGroup( + // private Command rightAlgaeRemove = new SequentialCommandGroup( + // new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), + // new waitEndefectorRefrence(m_robotElevator), + // new waitElevatorRefrence(m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), + // new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(1,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + // new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + // ); + private Command upperAlgaeRemove = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true), + waitDebuger.asProxy(), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME, true), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -536,11 +567,11 @@ public class RobotContainer { // Lower algae removal new JoystickButton(getButtonBox(), ButtonBox.Eight) - .onTrue(leftAlgaeRemove); + .onTrue(lowerAlgaeRemove); // Upper algae removal new JoystickButton(getButtonBox(), ButtonBox.Four) - .onTrue(rightAlgaeRemove); + .onTrue(upperAlgaeRemove); // Cancel button @@ -574,8 +605,8 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> {operatorManualMode = !operatorManualMode;})); - new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking())); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking())); // Auto Scoring new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) @@ -598,11 +629,11 @@ public class RobotContainer { //Controller Lower Algae Removal new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180 && !operatorManualMode) - .onTrue(leftAlgaeRemove); + .onTrue(lowerAlgaeRemove); //Controller Upper Algae Removal new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0 && !operatorManualMode) - .onTrue(rightAlgaeRemove); + .onTrue(upperAlgaeRemove); // ? /* Programer Buttons (Controller 3)*/ diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 17b6727..4b0ccdd 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -145,62 +145,62 @@ public class Elevator extends Subsystem { case L2Score: { PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case PrimedFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case ScoringFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case PrimedThree: { PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case ScoringThree: { PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL2Primed: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL2Go: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL3Primed: { PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); break; } case BallRemoverL3Go: { PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1c259e4..8ba2b72 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -307,6 +307,7 @@ public class SwerveDrive extends Subsystem { swerveDriveTrain.tareEverything(); robotKnowsWhereItIs = false; rotTarget = 0; + // vision.resetRotations(); } @@ -431,7 +432,15 @@ public class SwerveDrive extends Subsystem { public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ if(curPose.isPresent() && lastPose.isPresent()){ this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; - + + + // double rotDiff = curPose.get().getRotation().getDegrees() - lastPose.get().getRotation().getDegrees(); + + // if(rotDiff >= 180){ + // vision.subtractRotation(); + // }else if(rotDiff <= -180){ + // vision.addRotation(); + // } SmartDashboard.putNumber("Speed", lastOdomSpeed); } } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index c0e4305..4998f03 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -110,6 +110,20 @@ public class Vision extends Subsystem { // cameras[0]. } + public double rotations = 0; + + public void resetRotations(){ + rotations = 0; + } + + public void addRotation(){ + rotations += 180; + } + + public void subtractRotation(){ + rotations -= 180; + } + private void update() { isTagProcessed = false; @@ -162,7 +176,20 @@ public class Vision extends Subsystem { lastLatency = latency / cams; if(isTagProcessed){ - lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(Yaw/cams)); + + // double lastAngle = lastVisionPose.getRotation().getDegrees(); + double curAngle = Yaw/cams; + + // if(lastAngle - curAngle >= 90){ + // subtractRotation(); + // }else if(lastAngle - curAngle <= -90){ + // addRotation(); + // } + + // SmartDashboard.putNumber("curAngle", curAngle); + // SmartDashboard.putNumber("Rotations", rotations); + + lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); } } diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index 7c020b7..75c4ad0 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -5,6 +5,7 @@ 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 frc4388.robot.Constants.AutoConstants; @@ -14,7 +15,8 @@ public class ReefPositionHelper { public enum Side { LEFT, RIGHT, - CENTER + CENTER, + FAR_LEFT } public static final Pose2d[] RED_TAGS = { @@ -83,11 +85,14 @@ public class ReefPositionHelper { switch(side) { case LEFT: return -(AutoConstants.X_SCORING_POSITION_OFFSET); + case FAR_LEFT: + return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8)); case RIGHT: return (AutoConstants.X_SCORING_POSITION_OFFSET); case CENTER: return 0; } + assert false; return 0; } From a264e466bcc9d2ff7e230b4e40ca64a2385c8429 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sat, 8 Mar 2025 17:11:56 -0700 Subject: [PATCH 15/29] March 5th driver practice --- .../pathplanner/autos/3 Coral Cage 5.auto | 6 ----- .../pathplanner/paths/Cage 5 to Reef.path | 25 +++++++++++++------ .../paths/Reef 2 to Bottom Feed.path | 6 ++--- .../paths/Reef 3 to Bottom Feed.path | 14 +++++------ src/main/java/frc4388/robot/Constants.java | 3 ++- .../java/frc4388/robot/RobotContainer.java | 8 ++++-- .../frc4388/robot/subsystems/Elevator.java | 25 +++++++++++++++++-- .../java/frc4388/robot/subsystems/Vision.java | 20 +++++++++++---- 8 files changed, 74 insertions(+), 33 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto index f0d0c95..b9b9e6b 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -22,12 +22,6 @@ "name": "place-coral-left-l4" } }, - { - "type": "named", - "data": { - "name": "stop" - } - }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path index 4d15bb2..0a88f46 100644 --- a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.000189393939394, - "y": 1.6364267676767676 + "x": 5.959532828282828, + "y": 1.5754419191919191 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.47165404040404, - "y": 2.1141414141414145 + "x": 5.349684343434343, + "y": 2.2869318181818175 }, "prevControl": { - "x": 5.86940340909091, - "y": 1.7593308080808083 + "x": 5.747433712121213, + "y": 1.9321212121212112 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,18 @@ ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.8, + "y": 3.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path index bd485e9..6c4d421 100644 --- a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 58.10920819815426 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path index 9df45e7..fb33877 100644 --- a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.1010732323232322, - "y": 1.00625 + "x": 1.286335227272727, + "y": 1.2828125 }, "prevControl": { - "x": 1.786268939393939, - "y": 2.2439551767676766 + "x": 1.9715309343434337, + "y": 2.5205176767676765 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 121.42956561483854 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f562627..cd388bb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -427,10 +427,11 @@ public final class Constants { public static final double ENDEFECTOR_DRIVE_SLOW = -0.08; //Max for endefector = 60% public static final double L2_SCORE_ELEVATOR = -5; + public static final double L2_LEAVE_ELEVATOR = -11; public static final double L2_SCORE_ENDEFFECTOR = -19; - public static final double COMPLETLY_DOWN_ENDEFFECTOR = 9001; + public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5d3c31b..58768c5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -274,6 +274,8 @@ public class RobotContainer { new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator), + new waitElevatorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), @@ -293,6 +295,8 @@ public class RobotContainer { new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator), + new waitElevatorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), @@ -326,7 +330,7 @@ public class RobotContainer { new Translation2d(1,0), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME / 2, true), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME * 2, true), - // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -354,7 +358,7 @@ public class RobotContainer { // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME, true), - // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 4b0ccdd..4526bcd 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -48,6 +48,9 @@ public class Elevator extends Subsystem { private boolean disableAutoIntake = false; + private boolean seededZeroEndefector = false; + private boolean seededZeroElevator = false; + private DigitalInput basinBeamBreak; private DigitalInput endeffectorLimitSwitch; private DigitalInput intakeIR; @@ -58,6 +61,7 @@ public class Elevator extends Subsystem { Ready, // Has coral in endefector Hovering, // Has coral in endefector L2Score, + L2ScoreLeave, 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 @@ -116,7 +120,7 @@ public class Elevator extends Subsystem { case Waiting: { wait = System.currentTimeMillis() + maxWait; PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0)); led.setMode(LEDConstants.WAITING_PATTERN); break; } @@ -129,7 +133,7 @@ public class Elevator extends Subsystem { } case Ready: { - PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR); + PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0)); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); led.setMode(LEDConstants.DOWN_PATTERN); break; @@ -148,6 +152,13 @@ public class Elevator extends Subsystem { led.setMode(LEDConstants.SCORING_PATTERN); break; } + + case L2ScoreLeave: { + PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } case PrimedFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); @@ -312,6 +323,16 @@ public class Elevator extends Subsystem { SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); SmartDashboard.putString("State", currentState.toString()); + + if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) { + endeffectorMotor.setPosition(0); + seededZeroEndefector = !seededZeroEndefector; + } + + if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) { + elevatorMotor.setPosition(0); + seededZeroElevator = !seededZeroElevator; + } if (disableAutoIntake) return; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 4998f03..47ca4f8 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import java.time.Instant; import java.util.List; import java.util.Optional; @@ -124,6 +125,8 @@ public class Vision extends Subsystem { rotations -= 180; } + private Instant lastVisionTime = null; + private void update() { isTagProcessed = false; @@ -176,20 +179,27 @@ public class Vision extends Subsystem { lastLatency = latency / cams; if(isTagProcessed){ + Instant now = Instant.now(); - // double lastAngle = lastVisionPose.getRotation().getDegrees(); double curAngle = Yaw/cams; - // if(lastAngle - curAngle >= 90){ - // subtractRotation(); - // }else if(lastAngle - curAngle <= -90){ - // addRotation(); + // if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) > 1){ + // double lastAngle = lastVisionPose.getRotation().getDegrees(); + + // if(lastAngle - curAngle >= 80){ + // addRotation(); + // }else if(lastAngle - curAngle <= -80){ + // subtractRotation(); + // } // } + + // SmartDashboard.putNumber("curAngle", curAngle); // SmartDashboard.putNumber("Rotations", rotations); lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); + lastVisionTime = now; } } From 04586542812fdbef75887aa4f0c46c2227045c57 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Mon, 10 Mar 2025 13:13:48 -0600 Subject: [PATCH 16/29] Fix vision noise? --- .../java/frc4388/robot/subsystems/Vision.java | 41 +++++++++---------- 1 file changed, 19 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 47ca4f8..1b9ac39 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -111,20 +111,12 @@ public class Vision extends Subsystem { // cameras[0]. } - public double rotations = 0; + public int rotations = 0; public void resetRotations(){ rotations = 0; } - public void addRotation(){ - rotations += 180; - } - - public void subtractRotation(){ - rotations -= 180; - } - private Instant lastVisionTime = null; @@ -178,27 +170,32 @@ public class Vision extends Subsystem { lastLatency = latency / cams; - if(isTagProcessed){ + if(isTagProcessed || true){ Instant now = Instant.now(); - double curAngle = Yaw/cams; + // double curAngle = (Yaw/cams); + double curAngle = + (((Math.random() * 2) - 1 + 360) % 360) - 180; // Generate loopover noise - // if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) > 1){ - // double lastAngle = lastVisionPose.getRotation().getDegrees(); - // if(lastAngle - curAngle >= 80){ - // addRotation(); - // }else if(lastAngle - curAngle <= -80){ - // subtractRotation(); - // } - // } + if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) <= 1){ + double diff = curAngle - lastVisionPose.getRotation().getDegrees() + rotations*360; + + if(diff > 180){ + rotations -= 1; + }else if(diff < -180){ + rotations += 1; + } + } - // SmartDashboard.putNumber("curAngle", curAngle); - // SmartDashboard.putNumber("Rotations", rotations); - lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); + // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle + rotations*360)); + lastVisionPose = new Pose2d(0, 0, Rotation2d.fromDegrees(curAngle + rotations*360)); + + SmartDashboard.putNumber("curAngle", lastVisionPose.getRotation().getRadians()); + SmartDashboard.putNumber("Rotations", rotations); + lastVisionTime = now; } } From da976c13b92a84f3f2f4a93f572972fdc2a50a59 Mon Sep 17 00:00:00 2001 From: C4llSiqn Date: Mon, 10 Mar 2025 13:52:40 -0600 Subject: [PATCH 17/29] add || true to swervedrive seeding logic --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 2 +- src/main/java/frc4388/robot/subsystems/Vision.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 8ba2b72..4605f6a 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -341,7 +341,7 @@ public class SwerveDrive extends Subsystem { vision.setLastOdomPose(curpose); setLastOdomSpeed(curpose, lastPose, freq); - if (vision.isTag()) { + if (vision.isTag() || true) { Pose2d pose = vision.getPose2d(); if (!robotKnowsWhereItIs) { robotKnowsWhereItIs = true; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 1b9ac39..d53bcad 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -191,7 +191,7 @@ public class Vision extends Subsystem { // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle + rotations*360)); - lastVisionPose = new Pose2d(0, 0, Rotation2d.fromDegrees(curAngle + rotations*360)); + lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360)); SmartDashboard.putNumber("curAngle", lastVisionPose.getRotation().getRadians()); SmartDashboard.putNumber("Rotations", rotations); From e029739cee4a805a0aae4af3a1bfc6dd0b68ab61 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Mon, 10 Mar 2025 17:13:27 -0600 Subject: [PATCH 18/29] Fix: interpolate avage vision --- .../java/frc4388/robot/subsystems/Vision.java | 80 ++++++++++++------- 1 file changed, 50 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index d53bcad..c662a40 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -102,6 +102,7 @@ public class Vision extends Subsystem { photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight}; + // resetRotations(); } @Override @@ -111,11 +112,13 @@ public class Vision extends Subsystem { // cameras[0]. } - public int rotations = 0; + // public int[] rotations; + // public Instant[] lastUpdateTimes; - public void resetRotations(){ - rotations = 0; - } + // public void resetRotations(){ + // rotations = new int[cameras.length]; + // lastUpdateTimes = new Instant[cameras.length]; + // } private Instant lastVisionTime = null; @@ -124,14 +127,17 @@ public class Vision extends Subsystem { isTagProcessed = false; isTagDetected = false; + Instant now = Instant.now(); int cams = 0; - double X = 0; - double Y = 0; - double Yaw = 0; + // double X = 0; + // double Y = 0; + // double Yaw = 0; double latency = 0; + Pose2d pose = null; + for(int i = 0; i < cameras.length; i++){ PhotonCamera camera = cameras[i]; PhotonPoseEstimator estimator = estimators[i]; @@ -158,11 +164,18 @@ public class Vision extends Subsystem { if(estimatedRobotPose.isEmpty()) continue; - Pose2d pose = estimatedRobotPose.get().estimatedPose.toPose2d(); - X += pose.getX(); - Y += pose.getY(); - Yaw += pose.getRotation().getDegrees(); - cams++; + if(pose == null) + pose = estimatedRobotPose.get().estimatedPose.toPose2d(); + else + pose = pose.interpolate(pose, 0.5); + // X += pose.getX(); + // Y += pose.getY(); + + // if(X > 6) + + // Yaw += (pose.getRotation().getDegrees() + 180) % 360; + // cams++; + isTagProcessed = true; @@ -170,31 +183,38 @@ public class Vision extends Subsystem { lastLatency = latency / cams; - if(isTagProcessed || true){ - Instant now = Instant.now(); + if(isTagProcessed){ + // Instant now = Instant.now(); // double curAngle = (Yaw/cams); - double curAngle = + (((Math.random() * 2) - 1 + 360) % 360) - 180; // Generate loopover noise - - if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) <= 1){ - double diff = curAngle - lastVisionPose.getRotation().getDegrees() + rotations*360; - - if(diff > 180){ - rotations -= 1; - }else if(diff < -180){ - rotations += 1; - } - } + // Pose2d e = new Pose2d(); - - // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle + rotations*360)); - lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360)); - SmartDashboard.putNumber("curAngle", lastVisionPose.getRotation().getRadians()); - SmartDashboard.putNumber("Rotations", rotations); + + // // double curAngle = + (((Math.random() * 2) - 1 + 360) % 360) - 180; // Generate loopover noise + + + // if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) <= 1){ + // double diff = curAngle - lastVisionPose.getRotation().getDegrees() + rotations*360; + + // if(diff > 180){ + // rotations -= 1; + // }else if(diff < -180){ + // rotations += 1; + // } + // } + + + + lastVisionPose = pose; + // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); + // lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360)); + + // SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations()); + // SmartDashboard.putNumber("Rotations", rotations); lastVisionTime = now; } From d4a26dc94894bbe35375d1057fa7281cb7c6d322 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 13 Mar 2025 07:19:15 -0600 Subject: [PATCH 19/29] auto cont --- .../pathplanner/autos/2 Coral Cage 1.auto | 18 +++++- .../pathplanner/autos/2 Coral Cage 2.auto | 26 +++++++- .../pathplanner/autos/2 Coral Cage 3.auto | 26 +++++++- .../pathplanner/autos/2 Coral Cage 4.auto | 26 +++++++- .../pathplanner/autos/2 Coral Cage 5.auto | 18 +++++- .../pathplanner/autos/2 Coral Cage 6.auto | 26 +++++++- .../autos/2 Coral Center Barge Bottom.auto | 28 ++++++++- .../autos/2 Coral Center Barge Top.auto | 28 ++++++++- .../pathplanner/autos/3 Coral Cage 4.auto | 24 ++++++++ .../pathplanner/autos/3 Coral Cage 5.auto | 24 ++++++++ .../deploy/pathplanner/autos/New Auto.auto | 61 +++++++++++++++++++ src/main/deploy/pathplanner/autos/Taxi.auto | 4 +- .../paths/Bottom Feed to Reef 2.path | 18 +++--- .../pathplanner/paths/Cage 1 to Reef.path | 20 +++--- .../pathplanner/paths/Cage 2 to Reef 5.path | 18 +++--- .../pathplanner/paths/Cage 3 to Reef.path | 18 +++--- .../pathplanner/paths/Cage 4 to Reef.path | 20 +++--- .../pathplanner/paths/Cage 5 to Reef.path | 2 +- .../pathplanner/paths/Cage 6 to Reef.path | 8 +-- ....path => Center Barge to Reef 4 Left.path} | 16 ++--- ...path => Center Barge to Reef 4 Right.path} | 22 +++---- .../paths/Reef 2 to Bottom Feed.path | 16 ++--- .../paths/Reef 3 to Bottom Feed.path | 16 ++--- .../paths/Reef 4 to Bottom Feed.path | 16 ++--- .../pathplanner/paths/Reef 4 to Top Feed.path | 18 +++--- .../pathplanner/paths/Reef 5 to Top Feed.path | 16 ++--- .../pathplanner/paths/Reef 6 to Top Feed.path | 54 ++++++++++++++++ .../pathplanner/paths/Top Feed to Reef 6.path | 16 ++--- .../java/frc4388/robot/RobotContainer.java | 5 ++ 29 files changed, 472 insertions(+), 136 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto rename src/main/deploy/pathplanner/paths/{Center Barge to Reef 4.path => Center Barge to Reef 4 Left.path} (78%) rename src/main/deploy/pathplanner/paths/{Taxi.path => Center Barge to Reef 4 Right.path} (70%) create mode 100644 src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto index fd705bb..56e5665 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto @@ -31,7 +31,7 @@ { "type": "named", "data": { - "name": "align-feed" + "name": "feed-driveback" } }, { @@ -49,13 +49,25 @@ { "type": "named", "data": { - "name": "await-coral" + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" } }, { "type": "named", "data": { - "name": "place-coral-right-l4" + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" } } ] diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto index 9a62160..03c0706 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { @@ -25,7 +31,7 @@ { "type": "named", "data": { - "name": "align-feed" + "name": "feed-driveback" } }, { @@ -45,6 +51,24 @@ "data": { "name": "place-coral-left-l4" } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto index bd7da14..eeee724 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { @@ -25,7 +31,7 @@ { "type": "named", "data": { - "name": "align-feed" + "name": "feed-driveback" } }, { @@ -45,6 +51,24 @@ "data": { "name": "place-coral-right-l4" } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto index 766c054..ee7dcc4 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { @@ -25,7 +31,7 @@ { "type": "named", "data": { - "name": "align-feed" + "name": "feed-driveback" } }, { @@ -45,6 +51,24 @@ "data": { "name": "place-coral-left-l4" } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto index c538e81..611dc68 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto @@ -31,7 +31,7 @@ { "type": "named", "data": { - "name": "align-feed" + "name": "feed-driveback" } }, { @@ -49,13 +49,25 @@ { "type": "named", "data": { - "name": "await-coral" + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" } }, { "type": "named", "data": { - "name": "place-coral-left-l4" + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" } } ] diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto index c861420..10f21aa 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { @@ -25,7 +31,7 @@ { "type": "named", "data": { - "name": "align-feed" + "name": "feed-driveback" } }, { @@ -45,6 +51,24 @@ "data": { "name": "place-coral-left-l4" } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/2 Coral Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Bottom.auto index dbe71b6..83de831 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Center Barge Bottom.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Bottom.auto @@ -4,10 +4,16 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { - "pathName": "Center Barge to Reef 4" + "pathName": "Center Barge to Reef 4 Left" } }, { @@ -25,7 +31,7 @@ { "type": "named", "data": { - "name": "align-feed" + "name": "feed-driveback" } }, { @@ -45,6 +51,24 @@ "data": { "name": "place-coral-left-l4" } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/2 Coral Center Barge Top.auto b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Top.auto index 68320cc..fc445d9 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Center Barge Top.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Top.auto @@ -4,10 +4,16 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { - "pathName": "Center Barge to Reef 4" + "pathName": "Center Barge to Reef 4 Right" } }, { @@ -25,7 +31,7 @@ { "type": "named", "data": { - "name": "align-feed" + "name": "feed-driveback" } }, { @@ -45,6 +51,24 @@ "data": { "name": "place-coral-right-l4" } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto index e51e2f1..55794f3 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto @@ -28,6 +28,12 @@ "pathName": "Reef 3 to Bottom Feed" } }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, { "type": "named", "data": { @@ -52,6 +58,12 @@ "pathName": "Reef 2 to Bottom Feed" } }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, { "type": "named", "data": { @@ -75,6 +87,18 @@ "data": { "pathName": "Reef 2 to Bottom Feed" } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto index b9b9e6b..034400a 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -28,6 +28,12 @@ "pathName": "Reef 3 to Bottom Feed" } }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, { "type": "named", "data": { @@ -52,6 +58,12 @@ "pathName": "Reef 2 to Bottom Feed" } }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, { "type": "named", "data": { @@ -75,6 +87,18 @@ "data": { "pathName": "Reef 2 to Bottom Feed" } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..9333c35 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Cage 1 to Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 2 to Reef 5" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 3 to Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 4 to Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 5 to Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 6 to Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Left" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Right" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto index db06ab4..a717089 100644 --- a/src/main/deploy/pathplanner/autos/Taxi.auto +++ b/src/main/deploy/pathplanner/autos/Taxi.auto @@ -5,9 +5,9 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Taxi" + "name": "taxi" } } ] diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path index 6c86f54..35326f8 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.0807449494949497, - "y": 1.0875631313131315 + "x": 1.3040625, + "y": 1.3559375 }, "prevControl": null, "nextControl": { - "x": 2.3668181411598166, - "y": 1.8456535210784974 + "x": 2.590135691664867, + "y": 2.114027889765366 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.680625, - "y": 2.635625 + "x": 3.5221875, + "y": 2.3675 }, "prevControl": { - "x": 2.741625910703272, - "y": 2.050875079374932 + "x": 2.5831884107032717, + "y": 1.782750079374932 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": "Bottom Feed to Reef", "idealStartingState": { "velocity": 0, - "rotation": 53.32565033042684 + "rotation": 53.426969021480645 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path index 7e7a001..b9271b0 100644 --- a/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.575631313131314, - "y": 7.287689393939393 + "x": 6.9834375, + "y": 7.1084375 }, "prevControl": null, "nextControl": { - "x": 5.8378125, - "y": 6.9378125 + "x": 5.594062500000001, + "y": 6.7671875 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.344772727272727, - "y": 5.391107954545454 + "x": 5.2528125, + "y": 5.7434375 }, "prevControl": { - "x": 5.630625, - "y": 5.5728125 + "x": 5.50875, + "y": 6.145625 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -122.99770510121631 + "rotation": -118.81079374297312 }, "reversed": false, "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": -115.9743939624313 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path index 4dc63d4..eb7439a 100644 --- a/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path +++ b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.575631313127491, - "y": 6.139141414136251 + "x": 7.0078125, + "y": 6.0725 }, "prevControl": null, "nextControl": { - "x": 6.451471554401174, - "y": 5.958542570253511 + "x": 6.3984375, + "y": 6.291875 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.268371212121212, - "y": 5.417487373737374 + "x": 5.265, + "y": 5.73125 }, "prevControl": { "x": 5.8621875, - "y": 6.12125 + "y": 6.194375 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -120.86136963407526 + "rotation": -120.52970589993409 }, "reversed": false, "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": -179.99261564513898 + "rotation": -123.4533094540727 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path index 6ad2651..fdebcb4 100644 --- a/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.616287878787879, - "y": 5.092234848484848 + "x": 6.9590625, + "y": 4.9878124999999995 }, "prevControl": null, "nextControl": { - "x": 7.366483068336041, - "y": 5.1021119267533175 + "x": 6.709257689548163, + "y": 4.997689578268469 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.31375, - "y": 5.43875 + "x": 5.2528125, + "y": 5.7434375 }, "prevControl": { - "x": 5.7403125, - "y": 5.6215625 + "x": 5.728125, + "y": 5.6703125 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": 179.34296858150984 + "rotation": -124.91940201245771 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path index 6a79779..663cc6b 100644 --- a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.006439393939393, - "y": 3.1508838383838382 + "x": 6.97125, + "y": 3.244999999999999 }, "prevControl": null, "nextControl": { - "x": 6.498232323232323, - "y": 2.480050505050505 + "x": 6.166875, + "y": 2.9159374999999996 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.349684343434343, - "y": 2.6934974747474745 + "x": 5.2771875, + "y": 2.27 }, "prevControl": { - "x": 5.979861111111112, - "y": 1.992171717171717 + "x": 6.1059375, + "y": 2.5625000000000004 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 123.49004753500587 + "rotation": 120.96375653207355 }, "reversed": false, "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": 124.11447294534119 + "rotation": 118.9091836511478 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path index 0a88f46..6401f60 100644 --- a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -53,7 +53,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 124.8244891569568 + "rotation": 122.79876573246848 }, "reversed": false, "folder": "Barge to Reef", diff --git a/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path index f42fbe9..37daaac 100644 --- a/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.3015625, - "y": 2.6234375 + "x": 5.2771875, + "y": 2.294375 }, "prevControl": { - "x": 5.624116161616161, - "y": 2.205618686868687 + "x": 5.599741161616161, + "y": 1.876556186868687 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Center Barge to Reef 4.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Left.path similarity index 78% rename from src/main/deploy/pathplanner/paths/Center Barge to Reef 4.path rename to src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Left.path index 596548a..a30c744 100644 --- a/src/main/deploy/pathplanner/paths/Center Barge to Reef 4.path +++ b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Left.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.565467171717171, - "y": 4.045328282828282 + "x": 7.1296875, + "y": 4.0493749999999995 }, "prevControl": null, "nextControl": { - "x": 7.269289772727273, - "y": 4.064886363636362 + "x": 6.833510101010102, + "y": 4.06893308080808 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.1059375, - "y": 4.045328282828282 + "x": 6.3496875, + "y": 3.8421874999999996 }, "prevControl": { - "x": 6.35555109493404, - "y": 4.031433923393444 + "x": 6.599301094934041, + "y": 3.8282931405651617 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Right.path similarity index 70% rename from src/main/deploy/pathplanner/paths/Taxi.path rename to src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Right.path index c5d3778..b662f54 100644 --- a/src/main/deploy/pathplanner/paths/Taxi.path +++ b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Right.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.565467171717171, - "y": 7.348674242424242 + "x": 7.1053125, + "y": 4.061562499999999 }, "prevControl": null, "nextControl": { - "x": 7.8906231049309685, - "y": 7.339915637757156 + "x": 6.809135101010102, + "y": 4.081120580808079 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.0571875, - "y": 7.348674242424242 + "x": 6.3496875, + "y": 4.195625 }, "prevControl": { - "x": 6.306992295436264, - "y": 7.338796784398704 + "x": 6.599301094934041, + "y": 4.181730640565162 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 179.5653636269807 + "rotation": -179.96096735022735 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": -179.85896618052556 + "rotation": -179.2890804030095 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path index 6c4d421..e7219b4 100644 --- a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.749318181818182, - "y": 2.868295454545454 + "x": 3.5221875, + "y": 2.4040625 }, "prevControl": null, "nextControl": { - "x": 3.093244949494949, - "y": 2.2869318181818175 + "x": 2.866114267676767, + "y": 1.8226988636363637 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.3246843434343436, - "y": 1.5347853535353522 + "x": 1.3284375, + "y": 1.3803125 }, "prevControl": { - "x": 1.802550505050505, - "y": 2.1992045454545446 + "x": 1.8063036616161614, + "y": 2.0447316919191922 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path index fb33877..098c67d 100644 --- a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.268371212121212, - "y": 2.4597222222222217 + "x": 5.3015625, + "y": 2.2821875 }, "prevControl": null, "nextControl": { - "x": 4.670075757575758, - "y": 1.2531597222222217 + "x": 4.703267045454546, + "y": 1.075625 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.286335227272727, - "y": 1.2828125 + "x": 1.31625, + "y": 1.3559375 }, "prevControl": { - "x": 1.9715309343434337, - "y": 2.5205176767676765 + "x": 2.0014457070707063, + "y": 2.5936426767676766 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path index 42d3c4c..38a1be6 100644 --- a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 6.1059375, - "y": 4.0249999999999995 + "x": 6.3984375, + "y": 3.854374999999999 }, "prevControl": null, "nextControl": { - "x": 7.264857954545455, - "y": 2.244517045454545 + "x": 7.557357954545456, + "y": 2.073892045454545 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.1090625, - "y": 1.0146875 + "x": 1.3040625, + "y": 1.368125 }, "prevControl": { - "x": 1.9500000000000002, - "y": 2.1115625000000002 + "x": 2.1450000000000005, + "y": 2.4650000000000007 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path index 8f6baad..705c06b 100644 --- a/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 6.1059375, - "y": 4.037187499999999 + "x": 6.4228125, + "y": 4.195625 }, "prevControl": null, "nextControl": { - "x": 7.1784375, - "y": 6.4015625 + "x": 7.0809375, + "y": 6.535625 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.121401515151515, - "y": 7.003093434343434 + "x": 1.291875, + "y": 6.7428125 }, "prevControl": { - "x": 1.7184374999999998, - "y": 5.999375 + "x": 1.8889109848484849, + "y": 5.739094065656566 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": "Reef to Top Feed", "idealStartingState": { "velocity": 0, - "rotation": 179.1574757392596 + "rotation": 178.9390883097358 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path index 612587c..2accf7f 100644 --- a/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.284943181818181, - "y": 5.401079545454545 + "x": 5.3015625, + "y": 5.755625 }, "prevControl": null, "nextControl": { - "x": 4.996875, - "y": 6.8525 + "x": 5.013494318181819, + "y": 7.207045454545455 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.2230429292929292, - "y": 7.003093434343434 + "x": 1.2796875, + "y": 6.730625 }, "prevControl": { - "x": 2.34, - "y": 5.5484375 + "x": 2.3966445707070707, + "y": 5.275969065656566 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path new file mode 100644 index 0000000..b01e723 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.485625, + "y": 5.6459375 + }, + "prevControl": null, + "nextControl": { + "x": 2.5593749999999997, + "y": 6.389374999999999 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.31625, + "y": 6.730625 + }, + "prevControl": { + "x": 1.9132859848484847, + "y": 5.726906565656566 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.93780932479242 + }, + "reversed": false, + "folder": "Reef to Top Feed", + "idealStartingState": { + "velocity": 0, + "rotation": -58.96173664449721 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path b/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path index 42b5477..a8d390c 100644 --- a/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path +++ b/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.116818181818182, - "y": 6.956647727272727 + "x": 1.31625, + "y": 6.730625 }, "prevControl": null, "nextControl": { - "x": 2.123068181818182, - "y": 6.275650252525252 + "x": 2.3225000000000002, + "y": 6.049627525252525 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.6196875, - "y": 5.35122159090909 + "x": 3.5465625, + "y": 5.706875 }, "prevControl": { - "x": 2.8392584714142375, - "y": 5.792423901494171 + "x": 2.7661334714142374, + "y": 6.148077310585081 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 58768c5..9ff2715 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -388,6 +388,11 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + NamedCommands.registerCommand("taxi", new SequentialCommandGroup( + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0, -1), + new Translation2d(), 1000, true + ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy()); NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator)); From 274249b485422ae0ef6d7addc129032953a81808 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 13 Mar 2025 10:42:09 -0600 Subject: [PATCH 20/29] Add 3 coral --- .../pathplanner/autos/3 Coral Cage 1.auto | 109 ++++++++++++++++++ .../pathplanner/autos/3 Coral Cage 2.auto | 109 ++++++++++++++++++ .../pathplanner/autos/3 Coral Cage 3.auto | 109 ++++++++++++++++++ .../pathplanner/autos/3 Coral Cage 6.auto | 109 ++++++++++++++++++ .../autos/3 Coral Center Barge Bottom.auto | 109 ++++++++++++++++++ .../pathplanner/paths/Cage 4 to Reef.path | 4 +- 6 files changed, 547 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/3 Coral Cage 1.auto create mode 100644 src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto create mode 100644 src/main/deploy/pathplanner/autos/3 Coral Cage 3.auto create mode 100644 src/main/deploy/pathplanner/autos/3 Coral Cage 6.auto create mode 100644 src/main/deploy/pathplanner/autos/3 Coral Center Barge Bottom.auto diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 1.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 1.auto new file mode 100644 index 0000000..3b2b097 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 1.auto @@ -0,0 +1,109 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 1 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto new file mode 100644 index 0000000..366b60d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto @@ -0,0 +1,109 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 2 to Reef 5" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 3.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 3.auto new file mode 100644 index 0000000..97151eb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 3.auto @@ -0,0 +1,109 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 3 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 6.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 6.auto new file mode 100644 index 0000000..b379414 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 6.auto @@ -0,0 +1,109 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 6 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/3 Coral Center Barge Bottom.auto new file mode 100644 index 0000000..88b79f7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Center Barge Bottom.auto @@ -0,0 +1,109 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Left" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 4 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path index 663cc6b..c439a80 100644 --- a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path @@ -4,12 +4,12 @@ { "anchor": { "x": 6.97125, - "y": 3.244999999999999 + "y": 3.1474999999999995 }, "prevControl": null, "nextControl": { "x": 6.166875, - "y": 2.9159374999999996 + "y": 2.8184375 }, "isLocked": false, "linkedName": null From 9a159fae297f1b07b15e564560e0ec6f5117b655 Mon Sep 17 00:00:00 2001 From: Zach Wilke <83995467+C4llSqin@users.noreply.github.com> Date: Thu, 13 Mar 2025 20:17:11 -0600 Subject: [PATCH 21/29] End of practice day, autos are completely function --- .../deploy/pathplanner/paths/Bottom Feed to Reef 2.path | 4 ++-- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 8 +++++--- src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java | 2 +- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 2 +- 5 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path index 35326f8..7118995 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path @@ -20,7 +20,7 @@ "y": 2.3675 }, "prevControl": { - "x": 2.5831884107032717, + "x": 2.583188410703272, "y": 1.782750079374932 }, "nextControl": null, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 53.426969021480645 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index cd388bb..546e1b4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -290,7 +290,7 @@ public final class Constants { public static final int REEF_LIDAR_DIO_CHANNEL = 7; public static final int REVERSE_LIDAR_DIO_CHANNEL = 4; - public static final int HUMAN_PLAYER_STATION_DISTANCE = 25; + public static final int HUMAN_PLAYER_STATION_DISTANCE = 40; public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole public static final int LIDAR_MICROS_TO_CM = 10; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9ff2715..0e97233 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -398,7 +398,8 @@ public class RobotContainer { NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator)); NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, false)); + new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); + // NamedCommands.registerCommand("feed-driveback", Commands.none()); NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY)); NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( @@ -537,8 +538,9 @@ public class RobotContainer { ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - // .onTrue(new LidarAlign(m_robotSwerveDrive, m_reefLidar)); + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new DriveUntilLiDAR(m_robotSwerveDrive, + new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy()); diff --git a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java index 029d7a6..937f02c 100644 --- a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java +++ b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java @@ -40,7 +40,7 @@ public class DriveUntilLiDAR extends Command { @Override public void execute() { - swerveDrive.driveFine(leftStick, rightStick, 0.1); + swerveDrive.driveFine(leftStick, rightStick, 0.3); } @Override diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 4605f6a..8ba2b72 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -341,7 +341,7 @@ public class SwerveDrive extends Subsystem { vision.setLastOdomPose(curpose); setLastOdomSpeed(curpose, lastPose, freq); - if (vision.isTag() || true) { + if (vision.isTag()) { Pose2d pose = vision.getPose2d(); if (!robotKnowsWhereItIs) { robotKnowsWhereItIs = true; From 761c8ff912d9dddce7f3fb6e2ba8796688a5beb9 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Sat, 15 Mar 2025 08:51:50 -0600 Subject: [PATCH 22/29] End of 2nd day utah --- src/main/java/frc4388/robot/Constants.java | 8 +++-- .../java/frc4388/robot/RobotContainer.java | 4 +-- src/main/java/frc4388/utility/Trim.java | 31 ++++++++++--------- 3 files changed, 23 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 546e1b4..f44d1d1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -305,6 +305,7 @@ public final class Constants { // 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.5, 0); + // public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5); 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_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0); @@ -319,14 +320,15 @@ public final class Constants { // X is tangent to reef side // Y is normal to reef side - public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field - public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16); + public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field + public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1); public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); + // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5); public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); - public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5); + public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1); public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0e97233..1b9eb45 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -511,10 +511,10 @@ public class RobotContainer { // While Left Trigger Pressed: Trims new Trigger(() -> getDeadbandedDriverController().getPOV() == 0 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) - .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp())); + .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown())); new Trigger(() -> getDeadbandedDriverController().getPOV() == 180 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) - .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown())); + .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp())); new Trigger(() -> getDeadbandedDriverController().getPOV() == 90 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp())); diff --git a/src/main/java/frc4388/utility/Trim.java b/src/main/java/frc4388/utility/Trim.java index 7a9d017..9b06575 100644 --- a/src/main/java/frc4388/utility/Trim.java +++ b/src/main/java/frc4388/utility/Trim.java @@ -81,21 +81,22 @@ public class Trim { } public boolean load() { - try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) { - double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8)); - currentValue = fileValue; - clampModify(); - modified = false; - if (fileValue != currentValue) { - System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping..."); - modified = true; - } - return true; - } catch (Exception e) { - // e.printStackTrace(); - System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value..."); - return false; - } + // try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) { + // double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8)); + // currentValue = fileValue; + // clampModify(); + // modified = false; + // if (fileValue != currentValue) { + // System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping..."); + // modified = true; + // } + // return true; + // } catch (Exception e) { + // // e.printStackTrace(); + // System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value..."); + // return false; + // } + return false; } public void dump() { From 1f6a07751786a1c9c4160ef8b047510ce11b2687 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Sat, 15 Mar 2025 19:03:59 -0600 Subject: [PATCH 23/29] spaget: end of utah --- src/main/java/frc4388/robot/RobotContainer.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1b9eb45..8fbc18b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -116,6 +116,7 @@ public class RobotContainer { public void stop() { new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); m_robotSwerveDrive.stopModules(); + Constants.AutoConstants.Y_OFFSET_TRIM.set(0); } // ! /* Autos */ @@ -406,7 +407,9 @@ public class RobotContainer { new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0, 1), new Translation2d(), 300, true - ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); + + ), new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)), + new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); @@ -415,9 +418,12 @@ public class RobotContainer { NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right); - NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> { - m_robotElevator.transitionState(CoordinationState.PrimedFour); - })); + NamedCommands.registerCommand("prepare-l4", new SequentialCommandGroup( + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Hovering)), + // new waitElevatorRefrence(m_robotElevator), + new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour)) + )); configureButtonBindings(); configureVirtualButtonBindings(); From 4747f6233b1bf91ebac4649beb5ef1ba672e305b Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Tue, 18 Mar 2025 11:02:39 -0600 Subject: [PATCH 24/29] end of day one --- .../pathplanner/autos/3 Coral Cage 5.auto | 14 ++- .../paths/Bottom Feed to Reef 2.path | 2 +- .../pathplanner/paths/Cage 5 to Reef.path | 31 ++---- .../paths/Reef 2 to Bottom Feed.path | 2 +- .../paths/Reef 3 to Bottom Feed.path | 16 +-- .../paths/Reef 4 to Bottom Feed.path | 2 +- src/main/java/frc4388/robot/Constants.java | 43 +++++--- .../java/frc4388/robot/RobotContainer.java | 102 +++++++++++++++++- src/main/java/frc4388/robot/RobotMap.java | 5 +- .../robot/commands/DriveUntilLiDAR.java | 6 +- .../frc4388/robot/commands/GotoLastApril.java | 21 +++- .../robot/commands/MoveUntilSuply.java | 47 ++++++++ .../frc4388/robot/subsystems/Elevator.java | 8 +- 13 files changed, 243 insertions(+), 56 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/MoveUntilSuply.java diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto index 034400a..a09d168 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -19,7 +19,7 @@ { "type": "named", "data": { - "name": "place-coral-left-l4" + "name": "place-coral-right-l4" } }, { @@ -46,6 +46,12 @@ "pathName": "Bottom Feed to Reef 2" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { @@ -76,6 +82,12 @@ "pathName": "Bottom Feed to Reef 2" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path index 7118995..521f2b1 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 0.0, "rotation": 59.28109573597083 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path index 6401f60..87801e6 100644 --- a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.959532828282828, - "y": 1.5754419191919191 + "x": 6.670197826108406, + "y": 2.0396270358261894 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.349684343434343, - "y": 2.2869318181818175 + "x": 5.754837328767123, + "y": 2.492380136986301 }, "prevControl": { - "x": 5.747433712121213, - "y": 1.9321212121212112 + "x": 6.077263063386753, + "y": 2.432264238502225 }, "nextControl": null, "isLocked": false, @@ -30,18 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.8, - "y": 3.0 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 1.0, - "name": "Point Towards Zone" - } - ], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -52,14 +41,14 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": 122.79876573246848 + "velocity": 1.0, + "rotation": 119.99999999999999 }, "reversed": false, "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": 126.34745820888533 + "rotation": 123.5866498700801 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path index e7219b4..92cd109 100644 --- a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 55.37584492005105 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path index 098c67d..0d5a745 100644 --- a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 4.703267045454546, - "y": 1.075625 + "x": 4.106435503617635, + "y": 1.8391383607824139 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 1.3559375 }, "prevControl": { - "x": 2.0014457070707063, - "y": 2.5936426767676766 + "x": 2.9441094150144833, + "y": 1.668218251495801 }, "nextControl": null, "isLocked": false, @@ -33,15 +33,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 55.05578949900953 }, "reversed": false, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 121.42956561483854 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path index 38a1be6..dab21be 100644 --- a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 52.857102599919905 }, "reversed": false, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f44d1d1..404dbc9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -54,6 +54,7 @@ import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; import frc4388.utility.ReefPositionHelper; import frc4388.utility.Trim; +import frc4388.utility.configurable.ConfigurableDouble; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -193,7 +194,7 @@ public final class Constants { .withKS(0).withKV(0.124); public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); - public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(2.5, 0.2, 1); + public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1); } public static final class Configurations { @@ -298,7 +299,11 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains XY_GAINS = new Gains(5,0.6,0.0); + // public static final Gains XY_GAINS = new Gains(5,0.6,0.0); + public static final Gains XY_GAINS = new Gains(8,0,0.0); + // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP); + // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI); + // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD); // public static final Gains XY_GAINS = new Gains(3,0.3,0.0); // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); @@ -316,24 +321,37 @@ public final class Constants { public static final double MIN_XY_PID_OUTPUT = 0.0; public static final double MIN_ROT_PID_OUTPUT = 0.0; - public static final double VELOCITY_THRESHHOLD = 0.005; + public static final double VELOCITY_THRESHHOLD = 0.01; // X is tangent to reef side // Y is normal to reef side public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1); + public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18); - public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); - public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); - // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5); + public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); + public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP; + // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5); - public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); - public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1); + public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); + public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1); - public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); - public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5); + public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6); + public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5); - public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); + public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE; + + // public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); + // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); + // // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5); + + // public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); + // public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1); + + // public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + // public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5); + + // public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); public static final int L4_DRIVE_TIME = 250; //Milliseconds // public static final int L3_DRIVE_TIME = 500; @@ -437,7 +455,8 @@ public final class Constants { public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; - public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value + public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; + public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR; public static final Slot0Configs ELEVATOR_PID = new Slot0Configs() diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8fbc18b..c5086ea 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -38,6 +38,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -49,6 +50,7 @@ import frc4388.robot.commands.DriveUntilLiDAR; import frc4388.robot.commands.GotoLastApril; import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.MoveForTimeCommand; +import frc4388.robot.commands.MoveUntilSuply; import frc4388.robot.commands.WhileTrueCommand; import frc4388.robot.commands.waitElevatorRefrence; import frc4388.robot.commands.waitEndefectorRefrence; @@ -148,6 +150,48 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), + waitDebuger.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), + new ParallelDeadlineGroup( + new WaitCommand(1), + new MoveUntilSuply( + m_robotSwerveDrive, + new Translation2d(0,-0.5), + new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + ), + new InstantCommand(m_robotSwerveDrive::softStop, m_robotElevator), + + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + + new waitEndefectorRefrence(m_robotElevator), + + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + + new ConditionalCommand( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + () -> m_robotElevator.hasCoral()), + + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + ); + + /* private Command AprilLidarAlignL4Right = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + new ConditionalCommand(Commands.none(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true) + ), () -> m_robotElevator.isL4Primed()), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), @@ -204,6 +248,53 @@ public class RobotContainer { + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + ); */ + + private Command AprilLidarAlignL4Left = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + + new ConditionalCommand(Commands.none(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true) + ), () -> m_robotElevator.isL4Primed()), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), + waitDebuger.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), + new ParallelDeadlineGroup( + new WaitCommand(1), + new MoveUntilSuply( + m_robotSwerveDrive, + new Translation2d(0,-0.5), + new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + ), + new InstantCommand(m_robotSwerveDrive::softStop, m_robotElevator), + + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + + new waitEndefectorRefrence(m_robotElevator), + + + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + + new ConditionalCommand( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + () -> m_robotElevator.hasCoral()), + + + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -408,7 +499,7 @@ public class RobotContainer { new Translation2d(0, 1), new Translation2d(), 300, true - ), new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)), + ), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); @@ -421,7 +512,7 @@ public class RobotContainer { NamedCommands.registerCommand("prepare-l4", new SequentialCommandGroup( // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Hovering)), // new waitElevatorRefrence(m_robotElevator), - new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)), + // new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour)) )); @@ -545,14 +636,15 @@ public class RobotContainer { .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new DriveUntilLiDAR(m_robotSwerveDrive, - new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); + .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, + new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); + // .onTrue(new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.HALF_ROBOT_SIZE, Side.CENTER, true)); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_reefLidar)); + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_reefLidar)); // ? /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 72bafdd..577f6bf 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -55,11 +55,14 @@ public class RobotMap { /* Elevator Subsystem */ public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); + public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - void configureDriveMotorControllers() {} + void configureDriveMotorControllers() { + // endeffector.saf + } } diff --git a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java index 937f02c..cda04a7 100644 --- a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java +++ b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java @@ -45,6 +45,10 @@ public class DriveUntilLiDAR extends Command { @Override public boolean isFinished() { - return Math.abs(m_lidar.getDistance()) < mindistance; + if (Math.abs(m_lidar.getDistance()) < mindistance) { + swerveDrive.softStop(); + return true; + } + return false; } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index b11f4d0..0686440 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -50,8 +50,8 @@ public class GotoLastApril extends Command { this.vision = vision; this.distance = distance; this.side = side; - this.waitVelocityZero = waitVelocityZero; - // addRequirements(swerveDrive); + this.waitVelocityZero = waitVelocityZero && false; + addRequirements(swerveDrive); } @@ -62,6 +62,23 @@ public class GotoLastApril extends Command { @Override public void initialize() { + // double kP = AutoConstants.P_XY_GAINS.get(); + // double kI = AutoConstants.I_XY_GAINS.get(); + // double kD = AutoConstants.D_XY_GAINS.get(); + // xPID = new PID(new Gains( + // kP, + // kI, + // kD), + // 0); + // yPID = new PID(new Gains( + // kP, + // kI, + // kD), + // 0); + + // System.out.println("kP: "+kP); + // System.out.println("kI: "+kI); + // System.out.println("kD: "+kD); xPID.initialize(); yPID.initialize(); this.targetpos = ReefPositionHelper.getNearestPosition( diff --git a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java new file mode 100644 index 0000000..dd0d3e2 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java @@ -0,0 +1,47 @@ +package frc4388.robot.commands; + +import java.time.Instant; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.TimesNegativeOne; + +// Command to repeat a joystick movement for a specific time. +public class MoveUntilSuply extends Command { + private final SwerveDrive swerveDrive; + private final Translation2d leftStick; + private final Translation2d rightStick; + private final Supplier truth; + private final boolean robotRelative; + + public MoveUntilSuply( + SwerveDrive swerveDrive, + Translation2d leftStick, + Translation2d rightStick, + Supplier truth, + boolean robotRelative) { + addRequirements(swerveDrive); + + this.swerveDrive = swerveDrive; + this.leftStick = leftStick; + this.rightStick = rightStick; + this.truth = truth; + this.robotRelative = robotRelative; + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative); + } + + @Override + public boolean isFinished() { + return truth.get(); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 4526bcd..cbd10bb 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -162,7 +162,7 @@ public class Elevator extends Subsystem { case PrimedFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR); led.setMode(LEDConstants.SCORING_PATTERN); break; } @@ -255,6 +255,10 @@ public class Elevator extends Subsystem { // } // } + public boolean getEndeffectorLimit() { + return endeffectorLimitSwitch.get(); + } + private void periodicWaiting() { if (!basinBeamBreak.get()) transitionState(CoordinationState.Ready); @@ -363,7 +367,7 @@ public class Elevator extends Subsystem { } public boolean hasCoral() { - return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get(); + return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get(); } public boolean readyToMove() { From 6ac4b62089debe3f1faed7de0b4f6aaac2156336 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Tue, 18 Mar 2025 15:44:51 -0600 Subject: [PATCH 25/29] end of day 2 --- .../autos/1 Coral Center Barge Bottom.auto | 31 +++++++++++++++++++ .../autos/1 Coral Center Barge Top.auto | 31 +++++++++++++++++++ .../paths/Bottom Feed to Reef 2.path | 12 +++---- .../pathplanner/paths/Cage 1 to Reef.path | 12 +++---- .../pathplanner/paths/Cage 2 to Reef 5.path | 12 +++---- .../pathplanner/paths/Cage 3 to Reef.path | 8 ++--- .../pathplanner/paths/Cage 4 to Reef.path | 8 ++--- .../pathplanner/paths/Cage 5 to Reef.path | 8 ++--- .../pathplanner/paths/Cage 6 to Reef.path | 8 ++--- .../paths/Reef 2 to Bottom Feed.path | 16 +++++----- .../paths/Reef 3 to Bottom Feed.path | 18 +++++------ .../paths/Reef 4 to Bottom Feed.path | 20 ++++++------ .../pathplanner/paths/Reef 4 to Top Feed.path | 16 +++++----- .../pathplanner/paths/Reef 5 to Top Feed.path | 22 ++++++------- .../pathplanner/paths/Reef 6 to Top Feed.path | 8 ++--- src/main/deploy/pathplanner/settings.json | 6 ++-- .../java/frc4388/robot/RobotContainer.java | 14 +++++---- 17 files changed, 157 insertions(+), 93 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto create mode 100644 src/main/deploy/pathplanner/autos/1 Coral Center Barge Top.auto diff --git a/src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto new file mode 100644 index 0000000..7adeb88 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Left" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 Coral Center Barge Top.auto b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Top.auto new file mode 100644 index 0000000..6d81c5b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Top.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Right" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path index 521f2b1..943d856 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.590135691664867, - "y": 2.114027889765366 + "x": 2.892340424329296, + "y": 2.067688275398428 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.3675 }, "prevControl": { - "x": 2.583188410703272, - "y": 1.782750079374932 + "x": 2.589485733053583, + "y": 1.921600370835535 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path index b9271b0..710c3b1 100644 --- a/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.594062500000001, - "y": 6.7671875 + "x": 6.292598684210526, + "y": 6.699835526315789 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.2528125, - "y": 5.7434375 + "x": 5.590213815789473, + "y": 5.622203947368421 }, "prevControl": { - "x": 5.50875, - "y": 6.145625 + "x": 5.846151315789473, + "y": 6.024391447368421 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path index eb7439a..1247110 100644 --- a/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path +++ b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.3984375, - "y": 6.291875 + "x": 6.4946546052631575, + "y": 5.9878289473684205 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.265, - "y": 5.73125 + "x": 5.590213815789473, + "y": 5.593338815789473 }, "prevControl": { - "x": 5.8621875, - "y": 6.194375 + "x": 6.331085526315788, + "y": 5.958963815789473 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path index fdebcb4..5e34982 100644 --- a/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.2528125, - "y": 5.7434375 + "x": 5.619078947368421, + "y": 5.593338815789473 }, "prevControl": { - "x": 5.728125, - "y": 5.6703125 + "x": 6.094391447368421, + "y": 5.5202138157894725 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path index c439a80..c5ef9d6 100644 --- a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.2771875, - "y": 2.27 + "x": 5.55172697368421, + "y": 2.4470394736842103 }, "prevControl": { - "x": 6.1059375, - "y": 2.5625000000000004 + "x": 6.3804769736842095, + "y": 2.7395394736842107 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path index 87801e6..452dec9 100644 --- a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.754837328767123, - "y": 2.492380136986301 + "x": 5.5805921052631575, + "y": 2.4374177631578946 }, "prevControl": { - "x": 6.077263063386753, - "y": 2.432264238502225 + "x": 5.9030178398827875, + "y": 2.377301864673819 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path index 37daaac..27ace9e 100644 --- a/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.2771875, - "y": 2.294375 + "x": 5.561348684210526, + "y": 2.456661184210526 }, "prevControl": { - "x": 5.599741161616161, - "y": 1.876556186868687 + "x": 5.883902345826687, + "y": 2.038842371079213 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path index 92cd109..dbd1a43 100644 --- a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.866114267676767, - "y": 1.8226988636363637 + "x": 2.9564891755960816, + "y": 2.1114702488693777 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 1.3803125 }, "prevControl": { - "x": 1.8063036616161614, - "y": 2.0447316919191922 + "x": 2.0502196323692035, + "y": 1.7292616486680126 }, "nextControl": null, "isLocked": false, @@ -33,15 +33,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.5, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 1.0, + "velocity": 0.0, "rotation": 55.37584492005105 }, "reversed": false, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 58.10920819815426 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path index 0d5a745..46f8cb1 100644 --- a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.3015625, - "y": 2.2821875 + "x": 5.426644736842105, + "y": 2.851151315789474 }, "prevControl": null, "nextControl": { - "x": 4.106435503617635, - "y": 1.8391383607824139 + "x": 4.23151774045974, + "y": 2.4081021765718877 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.31625, - "y": 1.3559375 + "x": 1.7415296052631577, + "y": 1.5618421052631584 }, "prevControl": { - "x": 2.9441094150144833, - "y": 1.668218251495801 + "x": 3.369389020277641, + "y": 1.8741228567589594 }, "nextControl": null, "isLocked": false, @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 1.0, + "velocity": 0.0, "rotation": 55.05578949900953 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path index dab21be..7fa944f 100644 --- a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 6.3984375, - "y": 3.854374999999999 + "x": 5.9750822368421055, + "y": 3.9865131578947364 }, "prevControl": null, "nextControl": { - "x": 7.557357954545456, - "y": 2.073892045454545 + "x": 7.0046052631578934, + "y": 1.6388157894736843 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 1.368125 }, "prevControl": { - "x": 2.1450000000000005, - "y": 2.4650000000000007 + "x": 1.7992598684210526, + "y": 2.302713815789474 }, "nextControl": null, "isLocked": false, @@ -33,15 +33,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 1.0, + "velocity": 0.0, "rotation": 52.857102599919905 }, "reversed": false, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -179.46454101443553 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path index 705c06b..ef11e1e 100644 --- a/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 6.4228125, - "y": 4.195625 + "x": 5.946217105263157, + "y": 4.178947368421053 }, "prevControl": null, "nextControl": { - "x": 7.0809375, - "y": 6.535625 + "x": 6.604342105263157, + "y": 6.518947368421053 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.291875, - "y": 6.7428125 + "x": 1.452878289473684, + "y": 6.565131578947369 }, "prevControl": { - "x": 1.8889109848484849, - "y": 5.739094065656566 + "x": 2.0499142743221688, + "y": 5.561413144603935 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path index 2accf7f..2301370 100644 --- a/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.3015625, - "y": 5.755625 + "x": 5.41702302631579, + "y": 5.256578947368421 }, "prevControl": null, "nextControl": { - "x": 5.013494318181819, - "y": 7.207045454545455 + "x": 4.012253289473684, + "y": 5.958963815789473 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.2796875, - "y": 6.730625 + "x": 1.3951480263157894, + "y": 6.5747532894736835 }, "prevControl": { - "x": 2.3966445707070707, - "y": 5.275969065656566 + "x": 2.5121050970228604, + "y": 5.12009735513025 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.5, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -121.0370223454415 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path index b01e723..04ae5d9 100644 --- a/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.31625, - "y": 6.730625 + "x": 1.4143914473684207, + "y": 6.584375 }, "prevControl": { - "x": 1.9132859848484847, - "y": 5.726906565656566 + "x": 2.0114274322169052, + "y": 5.580656565656565 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index e658ac9..cf4f1eb 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -15,11 +15,11 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, + "robotMass": 58.18, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, + "driveWheelRadius": 0.0508, + "driveGearing": 6.12, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c5086ea..a67903a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -39,6 +39,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -90,6 +91,7 @@ import frc4388.utility.configurable.ConfigurableString; */ public class RobotContainer { /* RobotMap */ + public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ @@ -154,7 +156,7 @@ public class RobotContainer { waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), waitDebuger.asProxy(), - new ParallelDeadlineGroup( + new ParallelRaceGroup( new WaitCommand(1), new MoveUntilSuply( m_robotSwerveDrive, @@ -271,7 +273,7 @@ public class RobotContainer { waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), waitDebuger.asProxy(), - new ParallelDeadlineGroup( + new ParallelRaceGroup( new WaitCommand(1), new MoveUntilSuply( m_robotSwerveDrive, @@ -635,10 +637,10 @@ public class RobotContainer { ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, - new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); - // .onTrue(new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.HALF_ROBOT_SIZE, Side.CENTER, true)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, + // new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); + // // .onTrue(new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.HALF_ROBOT_SIZE, Side.CENTER, true)); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy()); From d1fb8ce7b424de689a56002c2a5cfe9903d5dcfd Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Wed, 19 Mar 2025 11:56:44 -0600 Subject: [PATCH 26/29] auto testing: afternoon of the third day --- ...om.auto => 1 Coral Center Barge Left.auto} | 8 +- ...p.auto => 1 Coral Center Barge Right.auto} | 8 +- .../pathplanner/autos/2 Coral Cage 1.auto | 8 +- .../pathplanner/autos/2 Coral Cage 2.auto | 8 +- .../pathplanner/autos/2 Coral Cage 3.auto | 8 +- .../pathplanner/autos/2 Coral Cage 4.auto | 10 +- .../pathplanner/autos/2 Coral Cage 5.auto | 10 +- .../pathplanner/autos/2 Coral Cage 6.auto | 10 +- ...op.auto => 2 Coral Center Barge Left.auto} | 2 +- ...m.auto => 2 Coral Center Barge Right.auto} | 2 +- .../pathplanner/autos/3 Coral Cage 1.auto | 109 ------------------ .../pathplanner/autos/3 Coral Cage 3.auto | 109 ------------------ .../pathplanner/autos/3 Coral Cage 4.auto | 109 ------------------ .../pathplanner/autos/3 Coral Cage 6.auto | 109 ------------------ .../deploy/pathplanner/paths/New Path.path | 54 +++++++++ src/main/deploy/pathplanner/settings.json | 6 +- .../java/frc4388/robot/RobotContainer.java | 30 +++-- .../frc4388/robot/subsystems/SwerveDrive.java | 1 + 18 files changed, 142 insertions(+), 459 deletions(-) rename src/main/deploy/pathplanner/autos/{1 Coral Center Barge Bottom.auto => 1 Coral Center Barge Left.auto} (78%) rename src/main/deploy/pathplanner/autos/{1 Coral Center Barge Top.auto => 1 Coral Center Barge Right.auto} (78%) rename src/main/deploy/pathplanner/autos/{2 Coral Center Barge Top.auto => 2 Coral Center Barge Left.auto} (98%) rename src/main/deploy/pathplanner/autos/{2 Coral Center Barge Bottom.auto => 2 Coral Center Barge Right.auto} (98%) delete mode 100644 src/main/deploy/pathplanner/autos/3 Coral Cage 1.auto delete mode 100644 src/main/deploy/pathplanner/autos/3 Coral Cage 3.auto delete mode 100644 src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto delete mode 100644 src/main/deploy/pathplanner/autos/3 Coral Cage 6.auto create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Left.auto similarity index 78% rename from src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto rename to src/main/deploy/pathplanner/autos/1 Coral Center Barge Left.auto index 7adeb88..3dd88fc 100644 --- a/src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto +++ b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Left.auto @@ -21,11 +21,17 @@ "data": { "name": "place-coral-left-l4" } + }, + { + "type": "named", + "data": { + "name": "lower-algae-removal" + } } ] } }, "resetOdom": true, - "folder": null, + "folder": "1 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 Coral Center Barge Top.auto b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Right.auto similarity index 78% rename from src/main/deploy/pathplanner/autos/1 Coral Center Barge Top.auto rename to src/main/deploy/pathplanner/autos/1 Coral Center Barge Right.auto index 6d81c5b..e0d6f82 100644 --- a/src/main/deploy/pathplanner/autos/1 Coral Center Barge Top.auto +++ b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Right.auto @@ -21,11 +21,17 @@ "data": { "name": "place-coral-right-l4" } + }, + { + "type": "named", + "data": { + "name": "lower-algae-removal" + } } ] } }, "resetOdom": true, - "folder": null, + "folder": "1 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto index 56e5665..96ded74 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto @@ -46,6 +46,12 @@ "pathName": "Top Feed to Reef 6" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { @@ -74,6 +80,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "2 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto index 03c0706..79d3540 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto @@ -46,6 +46,12 @@ "pathName": "Top Feed to Reef 6" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { @@ -74,6 +80,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "2 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto index eeee724..87b23c1 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto @@ -46,6 +46,12 @@ "pathName": "Top Feed to Reef 6" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { @@ -74,6 +80,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "2 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto index ee7dcc4..3df4f64 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto @@ -19,7 +19,7 @@ { "type": "named", "data": { - "name": "place-coral-left-l4" + "name": "place-coral-right-l4" } }, { @@ -46,6 +46,12 @@ "pathName": "Bottom Feed to Reef 2" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { @@ -74,6 +80,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "2 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto index 611dc68..0a519ac 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto @@ -19,7 +19,7 @@ { "type": "named", "data": { - "name": "place-coral-left-l4" + "name": "place-coral-right-l4" } }, { @@ -46,6 +46,12 @@ "pathName": "Bottom Feed to Reef 2" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { @@ -74,6 +80,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "2 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto index 10f21aa..68d7700 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto @@ -19,7 +19,7 @@ { "type": "named", "data": { - "name": "place-coral-left-l4" + "name": "place-coral-right-l4" } }, { @@ -46,6 +46,12 @@ "pathName": "Bottom Feed to Reef 2" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { @@ -74,6 +80,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "2 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Center Barge Top.auto b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Left.auto similarity index 98% rename from src/main/deploy/pathplanner/autos/2 Coral Center Barge Top.auto rename to src/main/deploy/pathplanner/autos/2 Coral Center Barge Left.auto index fc445d9..8351dab 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Center Barge Top.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Left.auto @@ -74,6 +74,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "2 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Right.auto similarity index 98% rename from src/main/deploy/pathplanner/autos/2 Coral Center Barge Bottom.auto rename to src/main/deploy/pathplanner/autos/2 Coral Center Barge Right.auto index 83de831..0628011 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Center Barge Bottom.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Right.auto @@ -74,6 +74,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "2 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 1.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 1.auto deleted file mode 100644 index 3b2b097..0000000 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 1.auto +++ /dev/null @@ -1,109 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "prepare-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Cage 1 to Reef" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-right-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 5 to Top Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Top Feed to Reef 6" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-right-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 6 to Top Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Top Feed to Reef 6" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-left-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 6 to Top Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 3.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 3.auto deleted file mode 100644 index 97151eb..0000000 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 3.auto +++ /dev/null @@ -1,109 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "prepare-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Cage 3 to Reef" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-right-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 5 to Top Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Top Feed to Reef 6" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-right-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 6 to Top Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Top Feed to Reef 6" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-left-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 6 to Top Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto deleted file mode 100644 index 55794f3..0000000 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto +++ /dev/null @@ -1,109 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "prepare-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Cage 4 to Reef" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-left-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 3 to Bottom Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom Feed to Reef 2" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-right-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 2 to Bottom Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom Feed to Reef 2" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-left-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 2 to Bottom Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 6.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 6.auto deleted file mode 100644 index b379414..0000000 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 6.auto +++ /dev/null @@ -1,109 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "prepare-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Cage 6 to Reef" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-left-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 3 to Bottom Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom Feed to Reef 2" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-right-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 2 to Bottom Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom Feed to Reef 2" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-left-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 2 to Bottom Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..bde395e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 4.2495147804709745, + "y": 5.984431624152738 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 4.249520880858856, + "y": 5.984529705386749 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index cf4f1eb..948f9b8 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,7 +9,11 @@ "Reef to Bottom Feed", "Top Feed to Reef" ], - "autoFolders": [], + "autoFolders": [ + "1 Coral", + "2 Coral", + "3 Coral" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a67903a..52a1b7a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -163,7 +163,7 @@ public class RobotContainer { new Translation2d(0,-0.5), new Translation2d(), m_robotElevator::getEndeffectorLimit, true) ), - new InstantCommand(m_robotSwerveDrive::softStop, m_robotElevator), + new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), @@ -172,10 +172,10 @@ public class RobotContainer { new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), - new ConditionalCommand( - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new ConditionalCommand( + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), - () -> m_robotElevator.hasCoral()), + // () -> m_robotElevator.hasCoral()), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -280,7 +280,7 @@ public class RobotContainer { new Translation2d(0,-0.5), new Translation2d(), m_robotElevator::getEndeffectorLimit, true) ), - new InstantCommand(m_robotSwerveDrive::softStop, m_robotElevator), + new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), @@ -290,10 +290,10 @@ public class RobotContainer { new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), - new ConditionalCommand( - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new ConditionalCommand( + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), - () -> m_robotElevator.hasCoral()), + // () -> m_robotElevator.hasCoral()), @@ -511,6 +511,10 @@ public class RobotContainer { NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right); + + NamedCommands.registerCommand("lower-algae-removal", lowerAlgaeRemove); + NamedCommands.registerCommand("upper-algae-removal", upperAlgaeRemove); + NamedCommands.registerCommand("prepare-l4", new SequentialCommandGroup( // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Hovering)), // new waitElevatorRefrence(m_robotElevator), @@ -830,7 +834,15 @@ public class RobotContainer { } autoChooser.onChange((filename) -> { - autoCommand = new PathPlannerAuto(filename); + if (filename.equals("Taxi")) { + autoCommand = new SequentialCommandGroup( + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0, -1), + new Translation2d(), 1000, true + ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)); + } else { + autoCommand = new PathPlannerAuto(filename); + } System.out.println("Robot Auto Changed " + filename); }); // SmartDashboard.putData(autoChooser); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 8ba2b72..0fb38ef 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -118,6 +118,7 @@ public class SwerveDrive extends Subsystem { } public void setOdoPose(Pose2d pose) { + if (pose == null) return; initalPose2d = pose; swerveDriveTrain.resetPose(pose); } From ded00059510da1891347a7c0af1b857c5921e511 Mon Sep 17 00:00:00 2001 From: Zach Wilke <83995467+C4llSqin@users.noreply.github.com> Date: Wed, 19 Mar 2025 15:26:55 -0600 Subject: [PATCH 27/29] day third --- .../pathplanner/autos/2 Coral Cage 1.auto | 2 +- .../pathplanner/autos/2 Coral Cage 2.auto | 2 +- .../pathplanner/autos/2 Coral Cage 3.auto | 2 +- .../pathplanner/autos/3 Coral Cage 2.auto | 62 +++++++++---------- .../pathplanner/paths/Cage 2 to Reef 5.path | 21 +++++-- .../pathplanner/paths/Reef 5 to Top Feed.path | 12 ++-- .../pathplanner/paths/Reef 6 to Top Feed.path | 18 +++--- .../pathplanner/paths/Top Feed to Reef 6.path | 18 +++--- 8 files changed, 74 insertions(+), 63 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto index 96ded74..ae18da3 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto @@ -19,7 +19,7 @@ { "type": "named", "data": { - "name": "place-coral-right-l4" + "name": "place-coral-left-l4" } }, { diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto index 79d3540..42d1639 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto @@ -19,7 +19,7 @@ { "type": "named", "data": { - "name": "place-coral-right-l4" + "name": "place-coral-left-l4" } }, { diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto index 87b23c1..72db1a3 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto @@ -19,7 +19,7 @@ { "type": "named", "data": { - "name": "place-coral-right-l4" + "name": "place-coral-left-l4" } }, { diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto index 366b60d..218687a 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto @@ -19,7 +19,7 @@ { "type": "named", "data": { - "name": "place-coral-right-l4" + "name": "place-coral-left-l4" } }, { @@ -46,36 +46,6 @@ "pathName": "Top Feed to Reef 6" } }, - { - "type": "named", - "data": { - "name": "place-coral-right-l4" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef 6 to Top Feed" - } - }, - { - "type": "named", - "data": { - "name": "feed-driveback" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Top Feed to Reef 6" - } - }, { "type": "named", "data": { @@ -94,6 +64,36 @@ "name": "feed-driveback" } }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path index 1247110..77f25d6 100644 --- a/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path +++ b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.4946546052631575, - "y": 5.9878289473684205 + "x": 6.902067334527623, + "y": 5.78751784036735 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 5.593338815789473 }, "prevControl": { - "x": 6.331085526315788, - "y": 5.958963815789473 + "x": 5.872115174798128, + "y": 6.170888760790882 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,18 @@ ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 5.15, + "y": 4.85 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path index 2301370..519045a 100644 --- a/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.012253289473684, - "y": 5.958963815789473 + "x": 4.4044191919191915, + "y": 5.9460227272727275 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.3951480263157894, - "y": 6.5747532894736835 + "x": 1.5076388888888888, + "y": 6.484722222222222 }, "prevControl": { - "x": 2.5121050970228604, - "y": 5.12009735513025 + "x": 3.184818220656944, + "y": 6.004734862812045 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path index 04ae5d9..77ed74f 100644 --- a/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.485625, - "y": 5.6459375 + "x": 3.5099747474747476, + "y": 5.742739898989899 }, "prevControl": null, "nextControl": { - "x": 2.5593749999999997, - "y": 6.389374999999999 + "x": 2.36577546257746, + "y": 6.2120596924932325 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 6.584375 }, "prevControl": { - "x": 2.0114274322169052, - "y": 5.580656565656565 + "x": 2.7066049155713703, + "y": 6.056626627340928 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.5, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -58.96173664449721 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path b/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path index a8d390c..74f69ec 100644 --- a/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path +++ b/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.3225000000000002, - "y": 6.049627525252525 + "x": 2.5451175614850996, + "y": 6.277541643530879 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.5465625, - "y": 5.706875 + "x": 3.540467171717172, + "y": 5.7630681818181815 }, "prevControl": { - "x": 2.7661334714142374, - "y": 6.148077310585081 + "x": 2.590802656557517, + "y": 6.105344244914677 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -52.93323259086456 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file From ca2e0cb9cda1421b754ae77b6cdf39ff55273c56 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Fri, 21 Mar 2025 13:53:12 -0600 Subject: [PATCH 28/29] end of practice denver --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 404dbc9..56d161a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -337,7 +337,7 @@ public final class Constants { public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1); public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6); - public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5); + public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5-2); public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 52a1b7a..11135de 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -625,6 +625,10 @@ public class RobotContainer { new Trigger(() -> getDeadbandedDriverController().getPOV() == 270 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust *= 2;})) + .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust /= 2;})); + new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod())) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod())); From a06b90ca0119d182cdb143f3910240fadb17839b Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Mon, 31 Mar 2025 17:57:30 -0600 Subject: [PATCH 29/29] end of denver --- src/main/java/frc4388/robot/Constants.java | 4 +- .../java/frc4388/robot/RobotContainer.java | 122 +++++++++++-- .../frc4388/robot/subsystems/SwerveDrive.java | 3 +- .../java/frc4388/robot/subsystems/Vision.java | 172 ++++++++---------- 4 files changed, 191 insertions(+), 110 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 56d161a..7f0cbfa 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -373,8 +373,8 @@ public final class Constants { // The standard deviations of our vision estimated poses, which affect correction rate // X, Y, Theta // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2 - public static final Matrix kSingleTagStdDevs = VecBuilder.fill(3, 3, 4); - public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1); } public static final class FieldConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 11135de..37a6cbb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -130,6 +130,7 @@ public class RobotContainer { private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove); // private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed); + // private Command waitDebugerManual = new waitSupplier(m_driverXbox::getYButtonPressed); private Command waitDebuger = new waitSupplier(() -> true); // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); @@ -138,7 +139,7 @@ public class RobotContainer { // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, // true, false); // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); - private Command AprilLidarAlignL4Right = new SequentialCommandGroup( + private Command AprilLidarAlignL4RightFullAuto = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new ConditionalCommand(Commands.none(), new SequentialCommandGroup( @@ -175,11 +176,57 @@ public class RobotContainer { // new ConditionalCommand( // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), - // () -> m_robotElevator.hasCoral()), + // () -> m_robotElevator.hasCoral()) new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); + private Command AprilLidarAlignL4RightSemiAuto = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + new ConditionalCommand(Commands.none(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true) + ), () -> m_robotElevator.isL4Primed()), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), + waitDebuger.asProxy(), + // new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // waitDebuger.asProxy(), + // new ParallelRaceGroup( + // new WaitCommand(1), + // new MoveUntilSuply( + // m_robotSwerveDrive, + // new Translation2d(0,-0.5), + // new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + // ), + // new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive), + // () -> m_robotElevator.hasCoral()) + + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + ); + + private Command WannaSeeMeDunk = new SequentialCommandGroup( + new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + new waitEndefectorRefrence(m_robotElevator), + new MoveForTimeCommand( + m_robotSwerveDrive, + new Translation2d(0,1), + new Translation2d(), + AutoConstants.L4_DRIVE_TIME, + true + ), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive) + ); + /* private Command AprilLidarAlignL4Right = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), @@ -253,7 +300,7 @@ public class RobotContainer { new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); */ - private Command AprilLidarAlignL4Left = new SequentialCommandGroup( + private Command AprilLidarAlignL4LeftFullAuto = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( @@ -290,8 +337,8 @@ public class RobotContainer { new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), - // new ConditionalCommand( - // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // // new ConditionalCommand( + // // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), // () -> m_robotElevator.hasCoral()), @@ -300,6 +347,51 @@ public class RobotContainer { new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); + private Command AprilLidarAlignL4LeftSemiAuto = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + + new ConditionalCommand(Commands.none(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true) + ), () -> m_robotElevator.isL4Primed()), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), + waitDebuger.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // waitDebuger.asProxy(), + // new ParallelRaceGroup( + // new WaitCommand(1), + // new MoveUntilSuply( + // m_robotSwerveDrive, + // new Translation2d(0,-0.5), + // new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + // ), + // new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + + // new waitEndefectorRefrence(m_robotElevator), + + + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + + // // // new ConditionalCommand( + // // // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + // // () -> m_robotElevator.hasCoral()), + + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + ); + private Command AprilLidarAlignL3Left = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), @@ -312,7 +404,7 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT, true), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true), waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), waitDebuger.asProxy(), @@ -504,8 +596,8 @@ public class RobotContainer { ), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); - NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); - NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); + NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4LeftFullAuto); + NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4RightFullAuto); NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left); NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right); NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); @@ -641,14 +733,14 @@ public class RobotContainer { 1, Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) ), - getDeadbandedDriverController().getRight(), 0.30 + getDeadbandedDriverController().getRight(), 0.15 ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) // .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, // new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); - // // .onTrue(new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.HALF_ROBOT_SIZE, Side.CENTER, true)); + .onTrue(WannaSeeMeDunk.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy()); @@ -667,10 +759,10 @@ public class RobotContainer { // Button box new JoystickButton(getButtonBox(), ButtonBox.Five) - .onTrue(AprilLidarAlignL4Left); + .onTrue(AprilLidarAlignL4LeftSemiAuto); new JoystickButton(getButtonBox(), ButtonBox.One) - .onTrue(AprilLidarAlignL4Right); + .onTrue(AprilLidarAlignL4RightSemiAuto); new JoystickButton(getButtonBox(), ButtonBox.Six) .onTrue(AprilLidarAlignL3Left); @@ -729,10 +821,10 @@ public class RobotContainer { // Auto Scoring new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(AprilLidarAlignL4Left); + .onTrue(AprilLidarAlignL4LeftSemiAuto); new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(AprilLidarAlignL4Right); + .onTrue(AprilLidarAlignL4RightSemiAuto); new Trigger(() -> getDeadbandedOperatorController().getLeftBumperButton() && !operatorManualMode) .onTrue(AprilLidarAlignL3Left); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 0fb38ef..c1f9335 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -351,7 +351,8 @@ public class SwerveDrive extends Subsystem { rotTarget += deltaAngle; } - swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); + vision.addVisionMeasurement(swerveDriveTrain); + // swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); //back to the ~~future~~ *past* } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index c662a40..4f34193 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import java.time.Instant; +import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -49,6 +50,7 @@ public class Vision extends Subsystem { private PhotonCamera[] cameras; private PhotonPoseEstimator[] estimators; + private List poses = new ArrayList<>(); private boolean isTagDetected = false; private boolean isTagProcessed = false; @@ -136,7 +138,8 @@ public class Vision extends Subsystem { // double Yaw = 0; double latency = 0; - Pose2d pose = null; + // Pose2d pose = null; + poses.clear(); for(int i = 0; i < cameras.length; i++){ PhotonCamera camera = cameras[i]; @@ -163,11 +166,13 @@ public class Vision extends Subsystem { // If the tag was failed to be processed if(estimatedRobotPose.isEmpty()) continue; + + poses.add(estimatedRobotPose.get()); - if(pose == null) - pose = estimatedRobotPose.get().estimatedPose.toPose2d(); - else - pose = pose.interpolate(pose, 0.5); + // if(pose == null) + // pose = estimatedRobotPose.get().estimatedPose.toPose2d(); + // else + // pose = pose.interpolate(pose, 0.5); // X += pose.getX(); // Y += pose.getY(); @@ -181,43 +186,20 @@ public class Vision extends Subsystem { } - lastLatency = latency / cams; + // lastLatency = latency / cams; - if(isTagProcessed){ - // Instant now = Instant.now(); - - // double curAngle = (Yaw/cams); - - // Pose2d e = new Pose2d(); + // if(isTagProcessed){ + // lastVisionPose = pose; + // // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); + // // lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360)); + // // SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations()); + // // SmartDashboard.putNumber("Rotations", rotations); - - // // double curAngle = + (((Math.random() * 2) - 1 + 360) % 360) - 180; // Generate loopover noise - - - // if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) <= 1){ - // double diff = curAngle - lastVisionPose.getRotation().getDegrees() + rotations*360; - - // if(diff > 180){ - // rotations -= 1; - // }else if(diff < -180){ - // rotations += 1; - // } - // } - - - - lastVisionPose = pose; - // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); - // lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360)); - - // SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations()); - // SmartDashboard.putNumber("Rotations", rotations); - - lastVisionTime = now; - } + // lastVisionTime = now; + // } } @@ -247,66 +229,66 @@ public class Vision extends Subsystem { return visionEst; // Will be empty visionEst = estimator.update(change); - updateEstimationStdDevs(visionEst, change.getTargets(), estimator); + // updateEstimationStdDevs(visionEst, change.getTargets(), estimator); return visionEst; } - /** - * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard - * deviations based on number of tags, estimation strategy, and distance from the tags. - * - * @param estimatedPose The estimated pose to guess standard deviations for. - * @param targets All targets in this camera frame - */ - private void updateEstimationStdDevs( - Optional estimatedPose, - List targets, - PhotonPoseEstimator estimator) { - if (estimatedPose.isEmpty()) { - // No pose input. Default to single-tag std devs - curStdDevs = VisionConstants.kSingleTagStdDevs; + // /** + // * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard + // * deviations based on number of tags, estimation strategy, and distance from the tags. + // * + // * @param estimatedPose The estimated pose to guess standard deviations for. + // * @param targets All targets in this camera frame + // */ + // private void updateEstimationStdDevs( + // Optional estimatedPose, + // List targets, + // PhotonPoseEstimator estimator) { + // if (estimatedPose.isEmpty()) { + // // No pose input. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; - } else { - // Pose present. Start running Heuristic - var estStdDevs = VisionConstants.kSingleTagStdDevs; - int numTags = 0; - double avgDist = 0; + // } else { + // // Pose present. Start running Heuristic + // var estStdDevs = VisionConstants.kSingleTagStdDevs; + // int numTags = 0; + // double avgDist = 0; - // Precalculation - see how many tags we found, and calculate an average-distance metric - for (var tgt : targets) { - var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) continue; + // // Precalculation - see how many tags we found, and calculate an average-distance metric + // for (var tgt : targets) { + // var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); + // if (tagPose.isEmpty()) continue; - double distance = tagPose - .get() - .toPose2d() - .getTranslation() - .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + // double distance = tagPose + // .get() + // .toPose2d() + // .getTranslation() + // .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); - if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { - numTags++; - avgDist += distance; - } - } + // if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { + // numTags++; + // avgDist += distance; + // } + // } - if (numTags == 0) { - // No tags visible. Default to single-tag std devs - curStdDevs = VisionConstants.kSingleTagStdDevs; - } else { - // One or more tags visible, run the full heuristic. - avgDist /= numTags; - // Decrease std devs if multiple targets are visible - if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; - // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) - estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); - curStdDevs = estStdDevs; - } - } - } + // if (numTags == 0) { + // // No tags visible. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; + // } else { + // // One or more tags visible, run the full heuristic. + // avgDist /= numTags; + // // Decrease std devs if multiple targets are visible + // if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; + // // Increase std devs based on (average) distance + // if (numTags == 1 && avgDist > 4) + // estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + // else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + // curStdDevs = estStdDevs; + // } + // } + // } /** * Returns the latest standard deviations of the estimated pose from {@link @@ -331,11 +313,13 @@ public class Vision extends Subsystem { // } public Pose2d getPose2d() { - if(isTagDetected && isTagProcessed) - // return lastVisionPose; - return lastPhysOdomPose; - else + if(lastPhysOdomPose != null) return lastPhysOdomPose; + return new Pose2d(); + // if(isTagDetected && isTagProcessed) + // // return lastVisionPose; + // else + // return lastPhysOdomPose; } public static double getTime() { @@ -347,7 +331,11 @@ public class Vision extends Subsystem { } - + public void addVisionMeasurement( SwerveDrivetrain drivetrain){ + for(EstimatedRobotPose pose : poses){ + drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds)); + } + }