From 920153d4ad15114387a5ed8981c61e2ecea33c89 Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Mon, 10 Mar 2025 08:24:29 -0400 Subject: [PATCH 001/106] Adding more autos --- src/main/deploy/pathplanner/autos/A_R2.auto | 19 +++++++++++++++++++ src/main/deploy/pathplanner/autos/A_R3.auto | 2 +- src/main/deploy/pathplanner/autos/A_R4.auto | 19 +++++++++++++++++++ .../autos/{Leave A.auto => LeaveA.auto} | 2 +- src/main/deploy/pathplanner/autos/LeaveB.auto | 19 +++++++++++++++++++ src/main/deploy/pathplanner/autos/LeaveC.auto | 19 +++++++++++++++++++ .../deploy/pathplanner/autos/TestAuto.auto | 2 +- src/main/deploy/pathplanner/settings.json | 6 +++++- 8 files changed, 84 insertions(+), 4 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/A_R2.auto create mode 100644 src/main/deploy/pathplanner/autos/A_R4.auto rename src/main/deploy/pathplanner/autos/{Leave A.auto => LeaveA.auto} (91%) create mode 100644 src/main/deploy/pathplanner/autos/LeaveB.auto create mode 100644 src/main/deploy/pathplanner/autos/LeaveC.auto diff --git a/src/main/deploy/pathplanner/autos/A_R2.auto b/src/main/deploy/pathplanner/autos/A_R2.auto new file mode 100644 index 0000000..2cbc044 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A_R2.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A_R2" + } + } + ] + } + }, + "resetOdom": true, + "folder": "StartToReef", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A_R3.auto b/src/main/deploy/pathplanner/autos/A_R3.auto index a2d8abf..2cbc044 100644 --- a/src/main/deploy/pathplanner/autos/A_R3.auto +++ b/src/main/deploy/pathplanner/autos/A_R3.auto @@ -14,6 +14,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "StartToReef", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A_R4.auto b/src/main/deploy/pathplanner/autos/A_R4.auto new file mode 100644 index 0000000..9ff659e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A_R4.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A_R4" + } + } + ] + } + }, + "resetOdom": true, + "folder": "StartToReef", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Leave A.auto b/src/main/deploy/pathplanner/autos/LeaveA.auto similarity index 91% rename from src/main/deploy/pathplanner/autos/Leave A.auto rename to src/main/deploy/pathplanner/autos/LeaveA.auto index d8b7a51..c4245a5 100644 --- a/src/main/deploy/pathplanner/autos/Leave A.auto +++ b/src/main/deploy/pathplanner/autos/LeaveA.auto @@ -14,6 +14,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "LeaveStart", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeaveB.auto b/src/main/deploy/pathplanner/autos/LeaveB.auto new file mode 100644 index 0000000..0865a86 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeaveB.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeaveB" + } + } + ] + } + }, + "resetOdom": true, + "folder": "LeaveStart", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeaveC.auto b/src/main/deploy/pathplanner/autos/LeaveC.auto new file mode 100644 index 0000000..ef442e4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeaveC.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeaveC" + } + } + ] + } + }, + "resetOdom": true, + "folder": "LeaveStart", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TestAuto.auto b/src/main/deploy/pathplanner/autos/TestAuto.auto index ba05359..b266b0f 100644 --- a/src/main/deploy/pathplanner/autos/TestAuto.auto +++ b/src/main/deploy/pathplanner/autos/TestAuto.auto @@ -14,6 +14,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Test", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 566711c..0fd10e4 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,7 +9,11 @@ "Reef to HP", "Start to Reef" ], - "autoFolders": [], + "autoFolders": [ + "LeaveStart", + "StartToReef", + "Test" + ], "defaultMaxVel": 5.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, From 5e5b8e87b64c7d839db64ea38d03f60fb606093c Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Wed, 12 Mar 2025 17:45:38 -0400 Subject: [PATCH 002/106] Some drive tweaks --- .../java/frc/robot/subsystems/drive/TunerConstants.java | 8 ++++---- .../java/frc/robot/subsystems/vision/VisionConstants.java | 7 ++++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java index 5538d02..3817ebc 100644 --- a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java @@ -127,7 +127,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 1; private static final int kFrontLeftSteerMotorId = 2; private static final int kFrontLeftEncoderId = 3; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.3748); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.0239); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -138,7 +138,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 4; private static final int kFrontRightSteerMotorId = 5; private static final int kFrontRightEncoderId = 6; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.170); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.308); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -149,7 +149,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 8; private static final int kBackLeftEncoderId = 9; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.2615); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.4429); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -160,7 +160,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 10; private static final int kBackRightSteerMotorId = 11; private static final int kBackRightEncoderId = 12; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.1426); + private static final Angle kBackRightEncoderOffset = Rotations.of(0.4294); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index e4ce08f..62024df 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -45,10 +45,11 @@ public class VisionConstants { public static Transform3d robotToCamera5 = new Transform3d(Units.inchesToMeters(-13.75), Units.inchesToMeters(-11.25), Units.inchesToMeters(9.25), new Rotation3d(0.0, -Math.PI/4, Math.PI)); - // Camera 6 + + // Camera 6 public static Transform3d robotToCamera6 = - new Transform3d(Units.inchesToMeters(-13.75), Units.inchesToMeters(11.25), Units.inchesToMeters(9.25), - new Rotation3d(0.0, 0, Math.PI)); + new Transform3d(Units.inchesToMeters(13.75), Units.inchesToMeters(11.5), Units.inchesToMeters(9.25), + new Rotation3d(0.0, 0, 0)); // Camera 7 From ca9c80ea5e6934b1299468037f13290a09158590 Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Wed, 12 Mar 2025 17:49:17 -0400 Subject: [PATCH 003/106] camera config --- src/main/java/frc/robot/RobotContainer.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6a470a5..16b7b87 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -120,10 +120,7 @@ public RobotContainer() { vision = new Vision( drive::addVisionMeasurement, - new VisionIOPhotonVision(camera5Name, robotToCamera5), - new VisionIOPhotonVision(camera6Name, robotToCamera6), - new VisionIOPhotonVision(camera7Name, robotToCamera7), - new VisionIOPhotonVision(camera8Name, robotToCamera8) + new VisionIOPhotonVision(camera6Name, robotToCamera6) ); break; From b34ce81538fd35ce27a1175b69f7cf16f3bc83df Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Wed, 12 Mar 2025 18:52:18 -0400 Subject: [PATCH 004/106] first part of merge --- .../java/frc/robot/HardwareConstants.java | 3 + src/main/java/frc/robot/RobotContainer.java | 63 +++++-- .../java/frc/robot/subsystems/arm/Arm.java | 134 +++++++++++---- .../frc/robot/subsystems/arm/ArmCommands.java | 98 +++++++++-- .../frc/robot/subsystems/arm/io/ArmIO.java | 1 - .../robot/subsystems/arm/io/RealArmIO.java | 63 +++++-- .../frc/robot/subsystems/climber/Climber.java | 28 ++++ .../subsystems/climber/ClimberCommands.java | 22 +++ .../subsystems/climber/io/ClimberIO.java | 14 ++ .../subsystems/climber/io/RealClimberIO.java | 30 ++++ .../robot/subsystems/elevator/Elevator.java | 154 +++++++----------- .../elevator/io/RealElevatorIO.java | 9 +- .../java/frc/robot/subsystems/leds/LEDs.java | 25 ++- .../robot/subsystems/leds/LEDsCommands.java | 24 ++- .../MultiSubsystemCommands.java | 149 +++++++++++++++-- .../frc/robot/subsystems/presets/Preset.java | 4 +- 16 files changed, 618 insertions(+), 203 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/climber/Climber.java create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberCommands.java create mode 100644 src/main/java/frc/robot/subsystems/climber/io/ClimberIO.java create mode 100644 src/main/java/frc/robot/subsystems/climber/io/RealClimberIO.java diff --git a/src/main/java/frc/robot/HardwareConstants.java b/src/main/java/frc/robot/HardwareConstants.java index 8a37a03..4b86c7c 100644 --- a/src/main/java/frc/robot/HardwareConstants.java +++ b/src/main/java/frc/robot/HardwareConstants.java @@ -24,11 +24,14 @@ public class CAN { public static final int ARM_MTR_ID = 14; public static final int INTAKE_MTR_ID = 15; + public static final int CLIMBER_MTR_ID = 30; public static final int PRIMARY_ELEVATOR_ID = 16; public static final int SECONDARY_ELEVATOR_ID = 17; public static final int CANDLE_ID = 50; + + public static final int CORAL_LASERCAN_ID = 20; } public class DIO { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 16b7b87..c8d3ab1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,9 @@ import frc.robot.subsystems.arm.ArmCommands; import frc.robot.subsystems.arm.Arm.ArmPosition; import frc.robot.subsystems.arm.io.RealArmIO; +import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.climber.ClimberCommands; +import frc.robot.subsystems.climber.io.RealClimberIO; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveCommands; import frc.robot.subsystems.drive.Telemetry; @@ -43,7 +46,10 @@ import frc.robot.subsystems.elevator.ElevatorCommands; import frc.robot.subsystems.elevator.Elevator.ElevatorPosition; import frc.robot.subsystems.elevator.io.RealElevatorIO; +import frc.robot.subsystems.leds.LEDs; +import frc.robot.subsystems.leds.LEDsCommands; import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.OverallPosition; import frc.robot.subsystems.presets.Preset; import frc.robot.subsystems.vision.Vision; @@ -59,17 +65,20 @@ public class RobotContainer { private final Drive drive; private final Elevator elevatorSubsystem = new Elevator(new RealElevatorIO()); private final Arm armSubsystem = new Arm(new RealArmIO()); + private final LEDs LEDSubsystem = new LEDs(); private final ElevatorCommands elevatorCommands = new ElevatorCommands(elevatorSubsystem); private final ArmCommands armCommands = new ArmCommands(armSubsystem); - private final Vision vision; - private final CommandXboxController joystick = new CommandXboxController(0); + private final LEDsCommands LEDCommands = new LEDsCommands(LEDSubsystem); + + private final Climber climber = new Climber(new RealClimberIO()); + private final ClimberCommands ClimberCommands = new ClimberCommands(climber); private final MultiSubsystemCommands multiSubsystemCommands = new MultiSubsystemCommands(elevatorSubsystem, armSubsystem, elevatorCommands, armCommands); - /** I am single :( - * hi zoe :3 - */ + private final Vision vision; + private final CommandXboxController joystick = new CommandXboxController(0); + private final Preset preset = new Preset(); private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed @@ -89,13 +98,16 @@ public class RobotContainer { private final JoystickButton StowButton = new JoystickButton(buttonbox1, 3); private final JoystickButton L3Button = new JoystickButton(buttonbox1, 4); private final JoystickButton L4Button = new JoystickButton(buttonbox1, 5); - private final JoystickButton LoadingButton = new JoystickButton(buttonbox1, 6); - private final JoystickButton button7 = new JoystickButton(buttonbox1, 7); + private final JoystickButton intakeButton = new JoystickButton(buttonbox1, 7); private final JoystickButton L4_scoreButton = new JoystickButton(buttonbox1, 8); - private final JoystickButton button9 = new JoystickButton(buttonbox1, 9); + private final JoystickButton spitButton = new JoystickButton(buttonbox1, 9); + private final JoystickButton gamepieceModeToggle = new JoystickButton(buttonbox1, 10); private final GenericHID buttonbox2 = new GenericHID(2); + private final JoystickButton manualIntakeButton = new JoystickButton(buttonbox2, 1); private final JoystickButton presetButton = new JoystickButton(buttonbox2, 2); + private final JoystickButton incrementButton = new JoystickButton(buttonbox2, 4); + private final JoystickButton decrementButton = new JoystickButton(buttonbox2, 7); private final LoggedDashboardChooser autoChooser; @@ -193,7 +205,7 @@ public RobotContainer() { // AutoAlign to Intake + Intake NamedCommands.registerCommand("Intake", - multiSubsystemCommands.intake()); + multiSubsystemCommands.loadGamepiece()); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -224,10 +236,25 @@ private void configureBindings() { () -> -joystick.getRightX())); - // joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); - // joystick.b().whileTrue(drivetrain - // .applyRequest(() -> point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())))); + //climberstick.start().and(climberstick.y()).onTrue(getAutonomousCommand()) + + // reset the field-centric heading on left bumper press + // joystick.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); + + // drive.registerTelemetry(logger::telemeterize); + + intakeButton.onTrue(multiSubsystemCommands.loadGamepiece().raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving())); + spitButton.onTrue(armCommands.spit()); + + L1Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L1)); + L2Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L2)); + StowButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.Stow)); + L3Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L3)); + L4Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4)); + L4_scoreButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4_Score)); + gamepieceModeToggle.whileTrue(multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE)); + gamepieceModeToggle.whileFalse(multiSubsystemCommands.setGamepieceMode(GamepieceMode.CORAL)); // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. joystick.back().and(joystick.y()).whileTrue(drive.sysIdDynamic(Direction.kForward)); @@ -237,14 +264,14 @@ private void configureBindings() { // Driver Right Bumper: Approach Nearest Right-Side Reef Branch joystick.rightBumper() - .whileTrue( - joystickApproach( - () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.RIGHT))); + .whileTrue( + joystickApproach( + () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.RIGHT))); - // Driver Left Bumper: Approach Nearest Left-Side Reef Branch + // Driver Left Bumper: Approach Nearest Left-Side Reef Branch joystick.leftBumper() - .whileTrue( - joystickApproach( + .whileTrue( + joystickApproach( () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.LEFT))); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 869cb97..9707c2c 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -3,18 +3,15 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Volt; -import java.util.function.BooleanSupplier; +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -22,79 +19,152 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.arm.io.ArmIO; -import frc.robot.subsystems.arm.io.ArmIO.ArmIOInputs; +import frc.robot.subsystems.arm.io.ArmIOInputsAutoLogged; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; public class Arm extends SubsystemBase { public enum ArmPosition { - Stow(40), - Loading(30), - L4_Score(45); + Stow(100, 100), + Loading(128, 55), + L4_Score(65, 45), + Algae_Score(60, 60); - double angle; + double coralAngle, algaeAngle; - ArmPosition(double angle) { - this.angle = angle; + ArmPosition(double coralAngle, double algaeAngle) { + this.coralAngle = coralAngle; + this.algaeAngle = algaeAngle; + } + + double getAngle(GamepieceMode mode) { + return (mode == GamepieceMode.ALGAE) ? this.algaeAngle : this.coralAngle; } } public final Trigger _hasPiece = new Trigger(() -> hasPiece()); private ArmIO _io; - private ArmIOInputs _inputs; + private ArmIOInputsAutoLogged _inputs; private boolean _prevLightSensorVal; + private boolean _hasGamepiece; + private int _intakeSpikeCounter; + private ArmPosition _currentPos; + private ArmPosition _desiredPos; + private MultiSubsystemCommands.GamepieceMode _currentMode; private ProfiledPIDController _armPidController; - private static final double KP = 0; - private static final double KI = 0; - private static final double KD = 0; - private static final double PROFILE_VEL = 0; - private static final double PROFILE_ACC = 0; + private static final double KP = 0.06;//0.09; + private static final double KI = 0; //0.01; + private static final double KD = 0.005; + private static final double PROFILE_VEL = 160; + private static final double PROFILE_ACC = 145; + + public static final double HAS_ALGAE_CURRENT = 40; + + private static final double ARM_FEEDFORWARD_COEFF = 0.4; SysIdRoutine routine = new SysIdRoutine(new Config(), new SysIdRoutine.Mechanism(this::setArmVoltage, this::populateLog, this)); public Arm(ArmIO io) { _io = io; - _inputs = new ArmIOInputs(); + _inputs = new ArmIOInputsAutoLogged(); _armPidController = new ProfiledPIDController(KP, KI, KD, new Constraints(PROFILE_VEL, PROFILE_ACC)); - + _armPidController.setTolerance(7); } public void setArmSetpoint(ArmPosition setpoint) { - _armPidController.setGoal(setpoint.angle); + _armPidController.reset(_inputs._armEncoderPositionDegrees); + _armPidController.setGoal(setpoint.getAngle(_currentMode)); + _desiredPos = setpoint; } public void setIntakeSpeed(double speed) { + if (_currentMode == GamepieceMode.ALGAE) speed *= -1; _io.setIntakeMotorSpeed(speed); } + public void spit() { + double speed = (_currentMode == GamepieceMode.ALGAE) ? -0.5 : 0.5; + setIntakeSpeed(speed); + } + + public void clearHasGamepiece() { + _hasGamepiece = false; + } + + public void setHasGamepiece() { + _hasGamepiece = true; + } + + public void clearIntakeSpikeCounter() { + _intakeSpikeCounter = 0; + } + + public double getIntakeSpikeCounter() { + return _intakeSpikeCounter; + } + public void setArmVoltage(Voltage voltage) { _io.setArmMotorVoltage(voltage); } + public double getIntakeCurrent() { + return _inputs._intakeMotorCurrent; + } + public void resetIntakeEncoders() { _io.resetIntakeEncoders(); } public boolean intakeAtDesiredRotations() { - return _inputs._intakeMotorPositionRotations >= 2; + return _inputs._intakeMotorPositionRotations <= -2; } public boolean hasPiece() { - boolean current = _inputs._lightSensorState; - boolean hasPiece = _prevLightSensorVal && !current; - _prevLightSensorVal = current; - return hasPiece; + boolean hasPiece = false; + if (_currentMode == GamepieceMode.CORAL) { + boolean currentState = _inputs._lightSensorState; + // hasPiece = _prevLightSensorVal && !currentState; + hasPiece = currentState; + _prevLightSensorVal = currentState; + } + + if (hasPiece) _hasGamepiece = true; + + return _hasGamepiece; + } + + public boolean lightSensorSeesGamepiece() { + return _inputs._lightSensorState; } - public boolean armAtSetpoint() { - return _armPidController.atGoal(); + public boolean isAtSetpoint() { + boolean atSetpoint = _armPidController.atGoal(); + if (atSetpoint) + _currentPos = _desiredPos; + return atSetpoint; + } + + public ArmPosition getCurrentPos() { + return _currentPos; } public void runArmPID() { - _io.setArmMotorSpeed(_armPidController.calculate(_inputs._armEncoderPositionDegrees)); + double out = _armPidController.calculate(_inputs._armEncoderPositionDegrees) + + (ARM_FEEDFORWARD_COEFF * Math.cos(Units.degreesToRadians(_inputs._armEncoderPositionDegrees))); + _io.setArmMotorVoltage(Voltage.ofBaseUnits(out, Volt)); + } + + public GamepieceMode getCurrentMode() { + return _currentMode; + } + + public void setCurrentMode(GamepieceMode mode) { + _currentMode = mode; } public void populateLog(SysIdRoutineLog log) { @@ -116,5 +186,13 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { @Override public void periodic() { _io.updateInputs(_inputs); + Logger.processInputs("Arm", _inputs); + + Logger.recordOutput("Arm/desiredPos", _armPidController.getSetpoint().position); + Logger.recordOutput("Arm/hasPiece", _hasGamepiece); + Logger.recordOutput("Arm/atSetpoint", isAtSetpoint()); + Logger.recordOutput("Arm/currentPosEnum", _currentPos); + Logger.recordOutput("Arm/desiredPosEnum", _desiredPos); + Logger.recordOutput("Arm/intakeSpikeCounter", _intakeSpikeCounter); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index a8242b1..2ba8c8a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -2,8 +2,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.arm.Arm.ArmPosition; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; @@ -17,36 +19,41 @@ public ArmCommands(Arm arm) { public Command spit() { return new StartEndCommand( () -> { - _arm.setIntakeSpeed(0.5); + if (_arm.getCurrentPos() == ArmPosition.L4_Score) + _arm.setArmSetpoint(ArmPosition.Stow); + _arm.spit(); }, () -> { _arm.setIntakeSpeed(0); + _arm.clearHasGamepiece(); }, _arm).withTimeout(1); + } public Command setArmPosition(ArmPosition setpoint) { - - return new InstantCommand(); - /* if (setpoint == ArmPosition.L4_Score) { return new InstantCommand( - () -> { - _arm.setArmSetpoint(setpoint); - }, - _arm).alongWith(intakeForNumberOfRotations()); + () -> { + _arm.setArmSetpoint(setpoint); + }, + _arm).andThen(intakeForNumberOfRotations()); } return new InstantCommand( () -> { _arm.setArmSetpoint(setpoint); }, - _arm); */ + _arm); } public Command intake() { + return Commands.either(intakeAlgae(), intakeCoral(), () -> _arm.getCurrentMode() == GamepieceMode.ALGAE); + } + + private Command intakeCoral() { return new StartEndCommand( () -> { - _arm.setIntakeSpeed(0.5); + _arm.setIntakeSpeed(0.35); }, () -> { _arm.setIntakeSpeed(0); @@ -54,6 +61,65 @@ public Command intake() { _arm).until(() -> _arm.hasPiece()); } + private Command intakeAlgae() { + return new Command() { + double intakeSpikeCounter = 0; + + @Override + public void initialize() { + intakeSpikeCounter = 0; + _arm.setIntakeSpeed(0.5); + } + + @Override + public void execute() { + if (_arm.getIntakeCurrent() >= Arm.HAS_ALGAE_CURRENT) { + intakeSpikeCounter++; + } + } + + @Override + public void end(boolean interrupted) { + _arm.setIntakeSpeed(0.05); + } + + @Override + public boolean isFinished() { + return intakeSpikeCounter > 15; + } + }; + } + + public Command moveGamepieceToLightSensor() { + return new Command() { + boolean movingDown = true; + + @Override + public void initialize() { + // If we see the gamepiece, we want to move further down in the intake + // If we don't, it's too far down and needs to go back up + System.out.println("AAAAAAAAAAAAAAAAAAAAa"); + movingDown = _arm.lightSensorSeesGamepiece(); + } + + @Override + public void execute() { + double speed = (movingDown) ? 0.065 : -0.065; + _arm.setIntakeSpeed(speed); + } + + @Override + public void end(boolean interrupted) { + _arm.setIntakeSpeed(0); + } + + @Override + public boolean isFinished() { + return (movingDown) ? !_arm.lightSensorSeesGamepiece() : _arm.lightSensorSeesGamepiece(); + } + }; + } + public Command runArmPID() { return Commands.run(() -> { _arm.runArmPID(); @@ -63,15 +129,15 @@ public Command runArmPID() { public Command intakeForNumberOfRotations() { return new StartEndCommand(() -> { _arm.resetIntakeEncoders(); - _arm.setIntakeSpeed(-0.3); - }, - () -> { - _arm.setIntakeSpeed(0); + _arm.setIntakeSpeed(-0.1); }, - _arm).until(() -> _arm.intakeAtDesiredRotations()); + () -> { + _arm.setIntakeSpeed(0); + }, + _arm).until(() -> _arm.intakeAtDesiredRotations()); } public Command waitUntilAtSetpoint() { - return new WaitUntilCommand(_arm::armAtSetpoint); + return new WaitUntilCommand(_arm::isAtSetpoint); } } diff --git a/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java index 7f4633b..f4c50f7 100644 --- a/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java @@ -31,5 +31,4 @@ public default void setArmMotorSpeed(double speed) {} public default void resetIntakeEncoders() {} public default void setArmMotorVoltage(Voltage voltage) {} - } diff --git a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java index b0e01c3..5484a2f 100644 --- a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java @@ -1,9 +1,17 @@ package frc.robot.subsystems.arm.io; import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; + +import au.grapplerobotics.ConfigurationFailedException; +import au.grapplerobotics.LaserCan; + import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; @@ -12,22 +20,42 @@ public class RealArmIO implements ArmIO { - public double POS_AT_90 = 0.0; - public double POS_AT_0 = 0.0; - public double ENCODER_CONVERSION = (POS_AT_90 - POS_AT_0) * 90; - - private double INTAKE_ROTATION_CONVERSION = 1; + private static final double POS_AT_90 = 0.711; + private static final double POS_AT_0 = 0.458; + private static final double ENCODER_CONVERSION = (POS_AT_90 - POS_AT_0) / 90.0; + private static final double LASERCAN_DISTANCE_MM = 50; + + private double INTAKE_ROTATION_CONVERSION = 1; private SparkFlex _armMotor; - private DigitalInput _lightSensor; - private SparkMax _intakeMotor; + private LaserCan _laserCan; + private SparkFlex _intakeMotor; private SparkAbsoluteEncoder _armEncoder; public RealArmIO() { _armMotor = new SparkFlex(CAN.ARM_MTR_ID, MotorType.kBrushless); - _intakeMotor = new SparkMax(CAN.INTAKE_MTR_ID, MotorType.kBrushless); - _lightSensor = new DigitalInput(DIO.LIGHT_SENSOR_CHANNEL); - _armEncoder = _intakeMotor.getAbsoluteEncoder(); + _intakeMotor = new SparkFlex(CAN.INTAKE_MTR_ID, MotorType.kBrushless); + _armEncoder = _armMotor.getAbsoluteEncoder(); + + SparkFlexConfig armConfig = new SparkFlexConfig(); + armConfig.idleMode(IdleMode.kBrake); + armConfig.inverted(true); + armConfig.voltageCompensation(12); + _armMotor.configure(armConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + SparkFlexConfig intakeConfig = new SparkFlexConfig(); + intakeConfig.idleMode(IdleMode.kBrake); + _intakeMotor.configure(intakeConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + _laserCan = new LaserCan(CAN.CORAL_LASERCAN_ID); + // Optionally initialise the settings of the LaserCAN, if you haven't already + // done so in GrappleHook + try { + _laserCan.setRangingMode(LaserCan.RangingMode.SHORT); + _laserCan.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + } catch (ConfigurationFailedException e) { + System.out.println("Configuration failed! " + e); + } } public void updateInputs(ArmIOInputs inputs) { @@ -35,13 +63,15 @@ public void updateInputs(ArmIOInputs inputs) { inputs._armMotorCurrent = _armMotor.getOutputCurrent(); inputs._armMotorVoltage = _armMotor.getAppliedOutput() * _armMotor.getBusVoltage(); - inputs._lightSensorState = !_lightSensor.get(); + LaserCan.Measurement measurement = _laserCan.getMeasurement(); + if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) + inputs._lightSensorState = measurement.distance_mm <= LASERCAN_DISTANCE_MM; inputs._intakeMotorVelocityRotationsPerMin = _intakeMotor.get(); inputs._intakeMotorCurrent = _intakeMotor.getOutputCurrent(); inputs._intakeMotorVoltage = _intakeMotor.getAppliedOutput() * _armMotor.getBusVoltage(); - inputs._intakeMotorPositionRotations = _intakeMotor.getEncoder().getPosition() * INTAKE_ROTATION_CONVERSION; - - inputs._armEncoderPositionDegrees = _armEncoder.getPosition() * ENCODER_CONVERSION; + inputs._intakeMotorPositionRotations = _intakeMotor.getEncoder().getPosition() * INTAKE_ROTATION_CONVERSION; + + inputs._armEncoderPositionDegrees = (_armEncoder.getPosition() - POS_AT_0) / ENCODER_CONVERSION; inputs._armEncoderVelocity = _armEncoder.getVelocity(); } @@ -49,7 +79,7 @@ public void setArmMotorSpeed(double speed) { _armMotor.set(speed); } - public void setIntakeSpeed(double speed) { + public void setIntakeMotorSpeed(double speed) { _intakeMotor.set(speed); } @@ -60,4 +90,5 @@ public void resetIntakeEncoders() { public void setArmMotorVoltage(Voltage voltage) { _armMotor.setVoltage(voltage); } + } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java new file mode 100644 index 0000000..f4d0d79 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.climber; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.climber.io.ClimberIO; +import frc.robot.subsystems.climber.io.ClimberIOInputsAutoLogged; + +public class Climber extends SubsystemBase { + + private ClimberIO _io; + private ClimberIOInputsAutoLogged _inputs; + + public Climber(ClimberIO io) { + _io = io; + _inputs = new ClimberIOInputsAutoLogged(); + } + + public void setClimberSpeed(double speed) { + _io.setClimberMotorSpeed(speed); + } + + @Override + public void periodic() { + _io.updateInputs(_inputs); + Logger.processInputs("Climber", _inputs); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java new file mode 100644 index 0000000..5474c0f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.climber; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; + +public class ClimberCommands { + private Climber _climber; + + public ClimberCommands(Climber climber) { + _climber = climber; + } + + public Command runClimber(DoubleSupplier ySupplier) { + return Commands.run( + () -> { + _climber.setClimberSpeed(ySupplier.getAsDouble()); + }, + _climber); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/io/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/io/ClimberIO.java new file mode 100644 index 0000000..09f4e65 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/io/ClimberIO.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.climber.io; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimberIO { + @AutoLog + public static class ClimberIOInputs { + public double _climberMotorSpeed = 0.0; + } + + public default void updateInputs(ClimberIOInputs inputs) {} + + public default void setClimberMotorSpeed(double speed) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/io/RealClimberIO.java b/src/main/java/frc/robot/subsystems/climber/io/RealClimberIO.java new file mode 100644 index 0000000..533d31d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/io/RealClimberIO.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.climber.io; + +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import frc.robot.HardwareConstants.CAN; + +public class RealClimberIO implements ClimberIO{ + private SparkFlex _climberMotor; + + public RealClimberIO() { + _climberMotor = new SparkFlex(CAN.CLIMBER_MTR_ID, MotorType.kBrushless); + + SparkFlexConfig climberConfig = new SparkFlexConfig(); + climberConfig.idleMode(IdleMode.kBrake); + _climberMotor.configure(climberConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void updateInputs(ClimberIOInputs inputs) { + inputs._climberMotorSpeed = _climberMotor.get(); + } + + public void setClimberMotorSpeed(double speed) { + _climberMotor.set(speed); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index facbe08..78c3cc9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -7,117 +7,89 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.Volt; import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.VelocityUnit; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import frc.robot.HardwareConstants.CAN; + import frc.robot.subsystems.arm.Arm.ArmPosition; -import frc.robot.subsystems.arm.io.ArmIO.ArmIOInputs; import frc.robot.subsystems.elevator.io.ElevatorIO; -import frc.robot.subsystems.elevator.io.ElevatorIO.ElevatorIOInputs; +import frc.robot.subsystems.elevator.io.ElevatorIOInputsAutoLogged; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; public class Elevator extends SubsystemBase { public enum ElevatorPosition { - L1(1), - L2(2), - L3(3), - L4(4), - Stow(5); + L1(5, 5), + L2(11, 20), + L3(27, 38), + L4(50.5, 50.5), + Stow(0.5, 0.5); - public final double setpoint; + double coralHeight, algaeHeight; - private ElevatorPosition(double setpoint) { - this.setpoint = setpoint; + ElevatorPosition(double coralHeight, double algaeHeight) { + this.coralHeight = coralHeight; + this.algaeHeight = algaeHeight; + } + + double getHeight(GamepieceMode mode) { + return (mode == GamepieceMode.ALGAE) ? this.algaeHeight : this.coralHeight; } } private static final double CALIBRATION_SPEED = -0.1; - private static final double HARD_STOP_CURRENT_LIMIT = 30; + private static final double HARD_STOP_CURRENT_LIMIT = 37; private static final double INCREMENT_CONSTANT = 1; private static final double DECREMENT_CONSTANT = 1; - private static final double ELEVATOR_MOTOR_KP = 0.21; // 0.08 - private static final double ELEVATOR_MOTOR_KI = 0; // 0.001 - private static final double ELEVATOR_MOTOR_KD = 0; // 0.002 - private static final double ELEVATOR_PID_VEL = 120; - private static final double ELEVATOR_PID_ACC = 125; - - private static final double ELEVATOR_MOTOR_TOLERANCE = 0.0; + private static final double ELEVATOR_MOTOR_KP = 0.8; //0.75; + private static final double ELEVATOR_MOTOR_KI = 0;//0.15; + private static final double ELEVATOR_MOTOR_KD = 0; + private static final double ELEVATOR_PID_VEL = 220; + private static final double ELEVATOR_PID_ACC = 215; - private static final double ELEVATOR_MAX_INCHES = 14.0; - private static final double ELEVATOR_MAX_ROTATIONS = 28.67; + private static final double ELEVATOR_MAX_INCHES = 52; //48; + private static final double ELEVATOR_MAX_ROTATIONS = 87; // 36.4; private static final double MOTOR_CONVERSION = ELEVATOR_MAX_INCHES / ELEVATOR_MAX_ROTATIONS; - // In pounds - private static final double ELEVATOR_STAGE1_WEIGHT = 3.75; - private static final double ELEVATOR_STAGE2_WEIGHT = 0; - - // In inches - private static final double SPOOL_DIAMETER = 1.6; - - // In pound - inches - private static final double STALL_TORQUE = 62.75; - - // In amps - private static final double STALL_CURRENT = 366; - - // In Ohms - private static final double RESISTANCE = 0.0185; - - private static final double GEAR_RATIO = 7.75; - private static final double NUMBER_OF_MOTORS = 2; - - private static final double FEEDFORWARD_CONSTANT = ((ELEVATOR_STAGE1_WEIGHT + (2 * ELEVATOR_STAGE2_WEIGHT)) - * SPOOL_DIAMETER * STALL_CURRENT * RESISTANCE) / (NUMBER_OF_MOTORS * GEAR_RATIO * STALL_TORQUE); + private static final double FEEDFORWARD_CONSTANT = 0.225; private boolean _isCalibrated; private ElevatorPosition _currentPos; + private ElevatorPosition _desiredPos; + private ElevatorPosition _prevPos; + private MultiSubsystemCommands.GamepieceMode _currentMode; private ProfiledPIDController _elevatorMotorPID; private ElevatorIO _io; - private ElevatorIOInputs _inputs; - - SysIdRoutine routine = new SysIdRoutine(new Config(), - new SysIdRoutine.Mechanism(this::setElevatorVoltage, this::populateLog, this)); + private ElevatorIOInputsAutoLogged _inputs; public Elevator(ElevatorIO io) { _io = io; - _inputs = new ElevatorIOInputs(); + _inputs = new ElevatorIOInputsAutoLogged(); _elevatorMotorPID = new ProfiledPIDController(ELEVATOR_MOTOR_KP, ELEVATOR_MOTOR_KI, ELEVATOR_MOTOR_KD, new Constraints(ELEVATOR_PID_VEL, ELEVATOR_PID_ACC)); - _elevatorMotorPID.setTolerance(ELEVATOR_MOTOR_TOLERANCE); - _elevatorMotorPID.setGoal(0.25); + _elevatorMotorPID.setTolerance(0.5); + + _currentPos = ElevatorPosition.Stow; + _desiredPos = ElevatorPosition.Stow; + _prevPos = ElevatorPosition.Stow; } // Runs the motors down at the calibration speed @@ -134,21 +106,25 @@ public void setElevatorVoltage(Voltage voltage) { } public void setSetpoint(ElevatorPosition setpoint) { - setSetpoint(setpoint.setpoint); - _currentPos = setpoint; + _prevPos = _currentPos; + _desiredPos = setpoint; + setSetpoint(setpoint.getHeight(_currentMode)); } // Sets the setpoint of the PID public void setSetpoint(double setpoint) { if (setpoint < 0.25 || setpoint > ELEVATOR_MAX_INCHES) return; - _elevatorMotorPID.setGoal(setpoint); _elevatorMotorPID.reset(getCurrentPosInches()); + _elevatorMotorPID.setGoal(setpoint); } // Checks if it is at the setpoint public boolean isAtSetpoint() { - return _elevatorMotorPID.atSetpoint(); + boolean atSetpoint = Math.abs(_elevatorMotorPID.getGoal().position - getCurrentPosInches()) <= 0.5; + if (atSetpoint) + _currentPos = _desiredPos; + return atSetpoint; } // Increases elevator position @@ -174,7 +150,7 @@ public void resetEncoders() { // Runs motors with PID public void runMotorsWithPID() { - _io.setElevatorSpeed(_elevatorMotorPID.calculate(getCurrentPosInches())); + _io.setElevatorVoltage(Voltage.ofBaseUnits(_elevatorMotorPID.calculate(getCurrentPosInches()) + FEEDFORWARD_CONSTANT, Volt)); } public boolean isCalibrated() { @@ -187,47 +163,33 @@ public void setIsCalibrated(boolean isCalibrated) { // Converts the motors position from weird units to normal people inches public double getCurrentPosInches() { - return _inputs._elevatorPosition * MOTOR_CONVERSION; + return (_inputs._elevatorPosition * MOTOR_CONVERSION) - 0.25; } public ElevatorPosition getCurrentPos() { return _currentPos; } - public boolean canMoveToPos(ElevatorPosition currentElevator, ArmPosition desiredArm) { - switch (currentElevator) { - case L1: - case L2: - case L3: - return (desiredArm != ArmPosition.L4_Score) || (desiredArm != ArmPosition.Loading); - case L4: - return desiredArm != ArmPosition.Loading; - case Stow: - return desiredArm != ArmPosition.L4_Score; - default: - return false; - } + public ElevatorPosition getPrevPos() { + return _prevPos; } - public void populateLog(SysIdRoutineLog log) { - log.motor("elevator_primary") - .voltage(Voltage.ofBaseUnits(_inputs._elevatorMotorVoltage, Volt)) - .linearPosition(Distance.ofBaseUnits(Units.inchesToMeters(getCurrentPosInches()), Meters)) - .linearVelocity( - LinearVelocity.ofBaseUnits(_inputs._elevatorVelocity * SPOOL_DIAMETER * Math.PI / 60, MetersPerSecond)); + public GamepieceMode getCurrentMode() { + return _currentMode; } - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return routine.quasistatic(direction); + public void setCurrentMode(GamepieceMode mode) { + _currentMode = mode; } - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return routine.dynamic(direction); + public double getElevatorMaxHeight() { + return ELEVATOR_MAX_INCHES; } @Override public void periodic() { _io.updateInputs(_inputs); + Logger.processInputs("Elevator", _inputs); // This method will be called once per scheduler run Logger.recordOutput("Elevator/currentPos", getCurrentPosInches()); @@ -236,8 +198,10 @@ public void periodic() { Logger.recordOutput("Elevator/Current", _inputs._elevatorMotorCurrent); Logger.recordOutput("Elevator/motorSpeed", _inputs._elevatorSpeed); Logger.recordOutput("Elevator/CurrentSetpoint", _elevatorMotorPID.getSetpoint().position); - - // Logger.recordOutput("Elevator/motorSpeed", _primaryMotor.get()); + Logger.recordOutput("Elevator/atSetpoint", isAtSetpoint()); + Logger.recordOutput("Elevator/currentPosEnum", _currentPos); + Logger.recordOutput("Elevator/desiredPosEnum", _desiredPos); + Logger.recordOutput("Elevator/currentGamepieceMode", _currentMode); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java index e8275a1..1bf6639 100644 --- a/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java @@ -11,8 +11,6 @@ import com.revrobotics.spark.config.SparkFlexConfig; import frc.robot.HardwareConstants.CAN; -import frc.robot.subsystems.arm.io.ArmIO; -import frc.robot.subsystems.elevator.io.ElevatorIO.ElevatorIOInputs; public class RealElevatorIO implements ElevatorIO { private SparkFlex _primaryMotor; @@ -26,11 +24,12 @@ public RealElevatorIO() { SparkFlexConfig primaryConfig = new SparkFlexConfig(); primaryConfig.inverted(true); primaryConfig.idleMode(IdleMode.kCoast); - _primaryMotor.configure(primaryConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + _primaryMotor.configure(primaryConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); SparkFlexConfig secondaryConfig = new SparkFlexConfig(); - secondaryConfig.follow(CAN.PRIMARY_ELEVATOR_ID); - _secondaryMotor.configure(secondaryConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + secondaryConfig.follow(CAN.PRIMARY_ELEVATOR_ID, true); + secondaryConfig.idleMode(IdleMode.kCoast); + _secondaryMotor.configure(secondaryConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } public void updateInputs(ElevatorIOInputs inputs) { diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java index a2614b3..11da210 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java @@ -6,9 +6,11 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.HardwareConstants; import frc.robot.HardwareConstants.CAN; +import frc.robot.subsystems.elevator.Elevator; import com.ctre.phoenix.led.RainbowAnimation; import com.ctre.phoenix.led.StrobeAnimation; @@ -20,10 +22,12 @@ public class LEDs extends SubsystemBase { CANdle _candle = new CANdle(CAN.CANDLE_ID); - static int _numLEDs = 8; + static int _numLEDs = 60; //leds/height * currentheight boolean _alreadyRunning = false; LEDAnimation _currentAnimation = LEDAnimation.None; + Elevator _elevator; + public enum LEDAnimation { None(null, null, 0), @@ -43,7 +47,9 @@ public enum LEDAnimation { SolidTeal(new LEDColor(0, 225, 174), null, 0), - SolidCoral(new LEDColor(255, 80, 15), null, 0); + SolidCoral(new LEDColor(255, 80, 15), null, 0), + + SolidRed(new LEDColor(255, 0, 0), null, 0); LEDColor _color; Animation _animation; @@ -81,6 +87,11 @@ public void runAnimation(LEDAnimation animation) { _candle.clearAnimation(0); _candle.setLEDs(0, 0, 0); _candle.animate(animation.getAnimation()); + } else if (animation == LEDAnimation.SolidRed) { + _candle.clearAnimation(0); + LEDColor color = animation.getColor(); + _candle.setLEDs(color.getR(), color.getG(), color.getB(),0, 0, + (int) Math.round(_numLEDs/_elevator.getElevatorMaxHeight() * _elevator.getCurrentPosInches())); } else if (animation.getAnimation() == null) { LEDColor color = animation.getColor(); _candle.clearAnimation(0); @@ -119,7 +130,7 @@ public void pickingUpCoral() { public void elevatorOrArmIsMoving() { if (!_alreadyRunning) { - runAnimation(LEDAnimation.BlinkDarkBlue); + runAnimation(LEDAnimation.SolidDarkBlue); _alreadyRunning = false; } } @@ -145,6 +156,13 @@ public void robotHasClimbed() { } } + public void disabledAnimation1() { + if (!_alreadyRunning) { + runAnimation(LEDAnimation.PartyMode); + _alreadyRunning = false; + } + } + public void reset() { _candle.clearAnimation(0); _alreadyRunning = false; @@ -152,6 +170,7 @@ public void reset() { @Override public void periodic() { + Logger.recordOutput("LEDs/alreadyRunning", _alreadyRunning); Logger.recordOutput("LEDs/CurrentAnimation", _currentAnimation.toString()); } } diff --git a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java index a664aca..1649a9d 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java @@ -14,6 +14,7 @@ public LEDsCommands(LEDs leds) { _leds = leds; } + //Runs when intake button is pushed public Command intaking() { return new StartEndCommand( () -> { @@ -25,6 +26,7 @@ public Command intaking() { _leds); } + //Runs when we have just acquired a piece public Command hasPiece() { return new StartEndCommand( () -> { @@ -33,9 +35,10 @@ public Command hasPiece() { () -> { _leds.reset(); }, - _leds); + _leds).withTimeout(3); } + // Runs when we have no gamepiece and we are toggled to pick up coral public Command pickingUpCoral() { return new StartEndCommand( () -> { @@ -47,6 +50,7 @@ public Command pickingUpCoral() { _leds); } + // Runs when we have no gamepiece and we are toggled to pick up coral public Command pickingUpAlgae() { return new StartEndCommand( () -> { @@ -58,6 +62,7 @@ public Command pickingUpAlgae() { _leds); } + //We have a piece and hasPiece animation has already run public Command elevatorOrArmIsMoving() { return new StartEndCommand( () -> { @@ -69,6 +74,7 @@ public Command elevatorOrArmIsMoving() { _leds); } + //Runs when robot has finished climbing public Command robotHasClimbed() { return new StartEndCommand( () -> { @@ -79,15 +85,21 @@ public Command robotHasClimbed() { }, _leds); } - - public Command elevatorAndArmAtSetpoints() { + + public Command disabledAnimation1() { return new StartEndCommand( () -> { - _leds.elevatorAndArmAtSetpoints(); - }, + _leds.disabledAnimation1(); + }, () -> { _leds.reset(); }, - _leds); + _leds) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }; } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 4fcb7fd..04b5ae5 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -1,7 +1,11 @@ package frc.robot.subsystems.multisubsystemcommands; +import java.util.function.Supplier; + import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.Arm.ArmPosition; import frc.robot.subsystems.arm.ArmCommands; @@ -12,7 +16,8 @@ public class MultiSubsystemCommands { public enum OverallPosition { Stow(ElevatorPosition.Stow, ArmPosition.Stow), - Loading(ElevatorPosition.Stow, ArmPosition.Loading), + Coral_Loading(ElevatorPosition.Stow, ArmPosition.Loading), + Algae_Loading(ElevatorPosition.L2, ArmPosition.Loading), L1(ElevatorPosition.L1, ArmPosition.Stow), L2(ElevatorPosition.L2, ArmPosition.Stow), L3(ElevatorPosition.L3, ArmPosition.Stow), @@ -36,6 +41,11 @@ ArmPosition getArmPosition() { } } + public enum GamepieceMode { + ALGAE, + CORAL; + } + private Elevator _elevator; private Arm _arm; private ElevatorCommands _elevatorCommands; @@ -52,7 +62,16 @@ public MultiSubsystemCommands(Elevator elevator, Arm arm, ElevatorCommands eleva public Command setOverallSetpoint(OverallPosition setpoint) { return _elevatorCommands.setElevatorSetpoint(setpoint.getElevatorPosition()) .alongWith(_armCommands.setArmPosition(setpoint.getArmPosition())) - .unless(() -> !_elevator.canMoveToPos(_elevator.getCurrentPos(), setpoint.getArmPosition())); + .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), setpoint.getElevatorPosition(), + _arm.getCurrentPos(), setpoint.getArmPosition())); + } + + public Command setGamepieceMode(GamepieceMode mode) { + return new InstantCommand( + () -> { + _elevator.setCurrentMode(mode); + _arm.setCurrentMode(mode); + }, _elevator, _arm); } public Command waitForOverallMechanism() { @@ -72,26 +91,130 @@ private Command score(OverallPosition setpoint) { } } - public Command scoreGamepieceAtPosition(OverallPosition setpoint) { - - return new InstantCommand(); + public Command scoreGamepieceAtPosition(Supplier setpoint) { + return scoreGamepieceAtPosition(setpoint.get()); + } - /* - if (setpoint == OverallPosition.Stow || setpoint == OverallPosition.Loading + public Command scoreGamepieceAtPosition(OverallPosition setpoint) { + if (setpoint == OverallPosition.Stow || setpoint == OverallPosition.Coral_Loading || setpoint == OverallPosition.L4_Score) { throw new RuntimeException("scoreGamepieceAtPosition cannot run to stow,loading,or L4_score"); } return setOverallSetpoint(setpoint) .andThen(waitForOverallMechanism()) .andThen(score(setpoint)) - .andThen(setOverallSetpoint(OverallPosition.Stow)); */ + .andThen(setOverallSetpoint(OverallPosition.Stow)); + } + + public Command loadGamepiece() { + return Commands.either(loadAlgae(), loadCoral(), () -> _arm.getCurrentMode() == GamepieceMode.ALGAE); } - public Command intake() { - return setOverallSetpoint(OverallPosition.Loading) + public Command loadCoral() { + return setOverallSetpoint(OverallPosition.Coral_Loading) .andThen(waitForOverallMechanism()) .andThen(_armCommands.intake()) - .andThen(setOverallSetpoint(OverallPosition.Stow)); + .andThen(new WaitCommand(0.25)) + // .andThen(setOverallSetpoint(OverallPosition.Stow)) + .andThen(_armCommands.moveGamepieceToLightSensor()) + .andThen(new WaitCommand(0.25)) + .andThen(_armCommands.moveGamepieceToLightSensor().unless(() -> _arm.lightSensorSeesGamepiece())) + .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), ElevatorPosition.Stow, + _arm.getCurrentPos(), ArmPosition.Loading)); + } -} - \ No newline at end of file + + public Command loadAlgae() { + return _armCommands.setArmPosition(ArmPosition.Loading) + .andThen(waitForOverallMechanism()) + .andThen(_armCommands.intake()) + .andThen(_armCommands.setArmPosition(ArmPosition.Stow)) + .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), ElevatorPosition.L2, + _arm.getCurrentPos(), ArmPosition.Loading)); + } + + public boolean canMoveToPos(ElevatorPosition currentElevator, ElevatorPosition desiredElevator, + ArmPosition currentArm, ArmPosition desiredArm) { + boolean canMoveArm = false; + boolean canMoveElevator = false; + + if (_arm.getCurrentMode() == GamepieceMode.CORAL) { + switch (currentElevator) { + case L1: + case L2: + case L3: + canMoveArm = (desiredArm != ArmPosition.L4_Score) && (desiredArm != ArmPosition.Loading); + break; + case L4: + canMoveArm = (desiredArm != ArmPosition.Loading); + break; + case Stow: + canMoveArm = (desiredArm != ArmPosition.L4_Score); + break; + default: + canMoveArm = false; + break; + } + + switch (desiredElevator) { + case L1: + case L2: + case L3: + canMoveElevator = (currentArm != ArmPosition.L4_Score) && (currentArm != ArmPosition.Loading); + break; + case L4: + canMoveElevator = (currentArm != ArmPosition.Loading); + break; + case Stow: + canMoveElevator = (currentArm != ArmPosition.L4_Score); + break; + default: + canMoveElevator = false; + break; + } + } else { + switch (currentElevator) { + case L1: + case L4: + canMoveArm = false; + break; + case L2: + case L3: + canMoveArm = desiredArm != ArmPosition.Algae_Score; + break; + case Stow: + canMoveArm = desiredArm != ArmPosition.Loading; + break; + default: + canMoveArm = false; + break; + } + + switch (desiredElevator) { + case L1: + case L4: + canMoveElevator = false; + break; + case L2: + case L3: + canMoveElevator = currentArm != ArmPosition.Algae_Score; + break; + case Stow: + canMoveElevator = currentArm != ArmPosition.Loading; + break; + default: + canMoveElevator = false; + break; + } + + if (currentArm == ArmPosition.Loading) { + canMoveElevator = desiredElevator == _elevator.getPrevPos(); + canMoveArm = canMoveElevator; + } + } + System.out.println("Arm: " + canMoveArm + " Elevator: " + canMoveElevator); + + return canMoveArm && canMoveElevator && _arm.isAtSetpoint() && _elevator.isAtSetpoint(); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/presets/Preset.java b/src/main/java/frc/robot/subsystems/presets/Preset.java index 9b4dac4..65c8fda 100644 --- a/src/main/java/frc/robot/subsystems/presets/Preset.java +++ b/src/main/java/frc/robot/subsystems/presets/Preset.java @@ -10,13 +10,13 @@ public enum ReefSide { Right; } - private OverallPosition _level = OverallPosition.Stow; + private OverallPosition _level = OverallPosition.L1; private ReefSide _side = ReefSide.Right; private boolean _isLevelValid = false; private boolean _isSideValid = false; private void setPresetLevel(OverallPosition level) { - if (level == OverallPosition.Stow || level == OverallPosition.Loading || level == OverallPosition.L4_Score) { + if (level == OverallPosition.Stow || level == OverallPosition.Coral_Loading || level == OverallPosition.L4_Score) { throw new RuntimeException("Invalid Preset Level/Position"); } From c99c95614aaba6a79e22c15c8fe92a3f6f6475fd Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Wed, 12 Mar 2025 19:04:23 -0400 Subject: [PATCH 005/106] possibly merged --- src/main/java/frc/robot/RobotContainer.java | 71 ++++++++++++++------ vendordeps/libgrapplefrc2025.json | 74 +++++++++++++++++++++ 2 files changed, 125 insertions(+), 20 deletions(-) create mode 100644 vendordeps/libgrapplefrc2025.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c8d3ab1..202eb6b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -42,6 +43,7 @@ import frc.robot.subsystems.drive.io.ModuleIO; import frc.robot.subsystems.drive.io.ModuleIOSim; import frc.robot.subsystems.drive.io.ModuleIOTalonFX; +import frc.robot.subsystems.elevator.CmdElevatorCalibrate; import frc.robot.subsystems.elevator.Elevator; import frc.robot.subsystems.elevator.ElevatorCommands; import frc.robot.subsystems.elevator.Elevator.ElevatorPosition; @@ -255,6 +257,7 @@ private void configureBindings() { gamepieceModeToggle.whileTrue(multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE)); gamepieceModeToggle.whileFalse(multiSubsystemCommands.setGamepieceMode(GamepieceMode.CORAL)); + // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. joystick.back().and(joystick.y()).whileTrue(drive.sysIdDynamic(Direction.kForward)); @@ -275,7 +278,17 @@ private void configureBindings() { () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.LEFT))); + // Resets the preset when we don't have a piece. + armSubsystem._hasPiece.onFalse(preset.resetPreset().andThen(LEDCommands.pickingUpCoral())); + + // Sets the level preset + presetButton.and(L1Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L1)); + presetButton.and(L2Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L2)); + presetButton.and(L3Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L3)); + presetButton.and(L4Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L4)); + incrementButton.onTrue(elevatorCommands.incrementElevatorPosition()); + decrementButton.onTrue(elevatorCommands.decrementElevatorPosition()); /* // Driver Left Bumper and Algae mode: Approach Nearest Reef Face joystick.rightBumper() @@ -297,26 +310,6 @@ private void configureBindings() { // // reset the field-centric heading on left bumper press // joystick.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); - // drive.registerTelemetry(logger::telemeterize); - - // Elevator sys id routines - button7.whileTrue(elevatorSubsystem.sysIdDynamic(Direction.kForward)); - button9.whileTrue(elevatorSubsystem.sysIdDynamic(Direction.kReverse)); - - L1Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L1)); - L2Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L2)); - StowButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.Stow)); - L3Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L3)); - L4Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4)); - LoadingButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.Loading)); - L4_scoreButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4_Score)); - - - // Runs the preset to score unless the preset is invalid. - /* - joystick.rightBumper().onTrue( - multiSubsystemCommands.scoreGamepieceAtPosition(preset.getLevel()).unless(() -> !preset.isPresetValid())); - */ // Resets the preset when we don't have a piece. armSubsystem._hasPiece.onFalse(preset.resetPreset()); @@ -336,5 +329,43 @@ public Command getAutonomousCommand() { public Drive getDrive() { return drive; } + public void calibrateAndStartPIDs() { + // PID commands: we only want one of them so start/stop works correctly + Command elevatorPIDCommand = elevatorCommands.runElevatorPID(); + Command armPIDCommand = armCommands.runArmPID(); + // Start elevator pid + if (elevatorSubsystem.isCalibrated()) { + elevatorCommands.runElevatorPID(); + if (!CommandScheduler.getInstance().isScheduled(elevatorPIDCommand)) { + CommandScheduler.getInstance().schedule(elevatorPIDCommand); + } + } else { + Command calibCommand = new CmdElevatorCalibrate(elevatorSubsystem).andThen(elevatorPIDCommand); + CommandScheduler.getInstance().schedule(calibCommand); + } + + // Start arm pid + if (!CommandScheduler.getInstance().isScheduled(armPIDCommand)) { + CommandScheduler.getInstance().schedule(armPIDCommand); + } + + // Set initial positions + CommandScheduler.getInstance().schedule(elevatorCommands.setElevatorSetpoint(ElevatorPosition.Stow)); + CommandScheduler.getInstance().schedule(armCommands.setArmPosition(ArmPosition.Stow)); + + CommandScheduler.getInstance().schedule(multiSubsystemCommands.setGamepieceMode(GamepieceMode.CORAL)); + } + + public void startIdleAnimations() { + Command disabled1 = LEDCommands.disabledAnimation1(); + if (!CommandScheduler.getInstance().isScheduled(disabled1)) + CommandScheduler.getInstance().schedule(disabled1); + } + + public void startEnabledLEDs() { + Command initialLEDs = LEDCommands.pickingUpCoral(); + if (!CommandScheduler.getInstance().isScheduled(initialLEDs)) + CommandScheduler.getInstance().schedule(initialLEDs); + } } diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2025.json new file mode 100644 index 0000000..a49af31 --- /dev/null +++ b/vendordeps/libgrapplefrc2025.json @@ -0,0 +1,74 @@ +{ + "fileName": "libgrapplefrc2025.json", + "name": "libgrapplefrc", + "version": "2025.1.3", + "frcYear": "2025", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2025.1.3" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.1.3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2025.1.3", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.1.3", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From 589aeb2909fcab33ebec85e6f51e51546a561a51 Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Wed, 12 Mar 2025 19:11:17 -0400 Subject: [PATCH 006/106] merge2 --- src/main/java/frc/robot/Robot.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 44c6726..b37eff5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -149,8 +149,6 @@ public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); System.out.println(m_autonomousCommand); - - System.out.println("!!!!!!!!!!Autonomous init!!!!!!!!!!!"); if (m_autonomousCommand != null) { System.out.println("FOUND A COMMAND"); m_autonomousCommand.schedule(); From 84bf10862e53dbacc857dcc47b78ce39ad47c1ff Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Wed, 12 Mar 2025 19:15:45 -0400 Subject: [PATCH 007/106] it helps to start the PIDs --- src/main/java/frc/robot/Robot.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b37eff5..c62ff49 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -81,11 +81,11 @@ public Robot() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - } @Override public void disabledInit() { + m_robotContainer.startIdleAnimations(); } @Override @@ -150,9 +150,11 @@ public void autonomousInit() { System.out.println(m_autonomousCommand); if (m_autonomousCommand != null) { - System.out.println("FOUND A COMMAND"); m_autonomousCommand.schedule(); } + + m_robotContainer.calibrateAndStartPIDs(); + m_robotContainer.startEnabledLEDs(); } @Override @@ -167,6 +169,8 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_robotContainer.calibrateAndStartPIDs(); + m_robotContainer.startEnabledLEDs(); } @Override From 7d7ef916510c77e66ce4c8c9efcbfbe1350c275d Mon Sep 17 00:00:00 2001 From: area5188 Date: Wed, 12 Mar 2025 20:39:20 -0400 Subject: [PATCH 008/106] Added auto display --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 12 ++++++++---- .../frc/robot/subsystems/drive/TunerConstants.java | 2 +- 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c62ff49..446a98b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -86,6 +86,7 @@ public void robotPeriodic() { @Override public void disabledInit() { m_robotContainer.startIdleAnimations(); + SmartDashboard.putData("Auto Trajectory", m_autoTraj); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 202eb6b..d3a6818 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -111,9 +111,13 @@ public class RobotContainer { private final JoystickButton incrementButton = new JoystickButton(buttonbox2, 4); private final JoystickButton decrementButton = new JoystickButton(buttonbox2, 7); + private final JoystickButton dynamic = new JoystickButton(buttonbox2, 8); + private final JoystickButton qstatic = new JoystickButton(buttonbox2, 9); + private final LoggedDashboardChooser autoChooser; // public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + private double speedMultiplier = 0.9; private final Telemetry logger = new Telemetry(MaxSpeed); @@ -260,10 +264,10 @@ private void configureBindings() { // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. - joystick.back().and(joystick.y()).whileTrue(drive.sysIdDynamic(Direction.kForward)); - joystick.back().and(joystick.x()).whileTrue(drive.sysIdDynamic(Direction.kReverse)); - joystick.start().and(joystick.y()).whileTrue(drive.sysIdQuasistatic(Direction.kForward)); - joystick.start().and(joystick.x()).whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); + dynamic.and(joystick.y()).whileTrue(drive.sysIdDynamic(Direction.kForward)); + dynamic.and(joystick.x()).whileTrue(drive.sysIdDynamic(Direction.kReverse)); + qstatic.and(joystick.y()).whileTrue(drive.sysIdQuasistatic(Direction.kForward)); + qstatic.and(joystick.x()).whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); // Driver Right Bumper: Approach Nearest Right-Side Reef Branch joystick.rightBumper() diff --git a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java index 3817ebc..b015024 100644 --- a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java @@ -29,7 +29,7 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() .withKP(2.3).withKI(0).withKD(0) - .withKS(0).withKV(0.124); + .withKS(0.08).withKV(0.124); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors From 65a3a8dfa5ee1e435ba47d84ecbd40e14d8bdc39 Mon Sep 17 00:00:00 2001 From: area5188 Date: Thu, 13 Mar 2025 18:28:57 -0400 Subject: [PATCH 009/106] testing 3/13 --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/arm/Arm.java | 2 +- .../java/frc/robot/subsystems/arm/ArmCommands.java | 1 - .../java/frc/robot/subsystems/elevator/Elevator.java | 2 +- .../MultiSubsystemCommands.java | 12 +++++++++--- .../frc/robot/subsystems/vision/VisionConstants.java | 2 +- 6 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d3a6818..8f35e9d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -256,7 +256,7 @@ private void configureBindings() { L2Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L2)); StowButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.Stow)); L3Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L3)); - L4Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4)); + L4Button.onTrue(multiSubsystemCommands.goToL4()); L4_scoreButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4_Score)); gamepieceModeToggle.whileTrue(multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE)); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 9707c2c..6be604f 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -121,7 +121,7 @@ public void resetIntakeEncoders() { } public boolean intakeAtDesiredRotations() { - return _inputs._intakeMotorPositionRotations <= -2; + return _inputs._intakeMotorPositionRotations <= -1.5; } public boolean hasPiece() { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 2ba8c8a..f46c87e 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -98,7 +98,6 @@ public Command moveGamepieceToLightSensor() { public void initialize() { // If we see the gamepiece, we want to move further down in the intake // If we don't, it's too far down and needs to go back up - System.out.println("AAAAAAAAAAAAAAAAAAAAa"); movingDown = _arm.lightSensorSeesGamepiece(); } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 78c3cc9..932bea0 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -34,7 +34,7 @@ public enum ElevatorPosition { L1(5, 5), L2(11, 20), L3(27, 38), - L4(50.5, 50.5), + L4(52, 52), Stow(0.5, 0.5); double coralHeight, algaeHeight; diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 04b5ae5..70f7a36 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -115,7 +115,7 @@ public Command loadCoral() { .andThen(waitForOverallMechanism()) .andThen(_armCommands.intake()) .andThen(new WaitCommand(0.25)) - // .andThen(setOverallSetpoint(OverallPosition.Stow)) + .andThen(setOverallSetpoint(OverallPosition.Stow)) .andThen(_armCommands.moveGamepieceToLightSensor()) .andThen(new WaitCommand(0.25)) .andThen(_armCommands.moveGamepieceToLightSensor().unless(() -> _arm.lightSensorSeesGamepiece())) @@ -125,12 +125,18 @@ public Command loadCoral() { } public Command loadAlgae() { - return _armCommands.setArmPosition(ArmPosition.Loading) + return _armCommands.setArmPosition(ArmPosition.Loading) .andThen(waitForOverallMechanism()) .andThen(_armCommands.intake()) .andThen(_armCommands.setArmPosition(ArmPosition.Stow)) .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), ElevatorPosition.L2, - _arm.getCurrentPos(), ArmPosition.Loading)); + _arm.getCurrentPos(), ArmPosition.Loading)); + } + + public Command goToL4() { + return setOverallSetpoint(OverallPosition.L4) + .andThen(waitForOverallMechanism()) + .andThen(setOverallSetpoint(OverallPosition.L4_Score)); } public boolean canMoveToPos(ElevatorPosition currentElevator, ElevatorPosition desiredElevator, diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 62024df..a04a9b4 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -48,7 +48,7 @@ public class VisionConstants { // Camera 6 public static Transform3d robotToCamera6 = - new Transform3d(Units.inchesToMeters(13.75), Units.inchesToMeters(11.5), Units.inchesToMeters(9.25), + new Transform3d(Units.inchesToMeters(13.75), Units.inchesToMeters(-11.5), Units.inchesToMeters(9.25), new Rotation3d(0.0, 0, 0)); From c014c38cbce6e0aededce7193f9d1ad18224c7be Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Thu, 13 Mar 2025 19:56:34 -0400 Subject: [PATCH 010/106] added Chris' autos --- src/main/deploy/pathplanner/autos/B-L2.auto | 37 +++++++++++++ src/main/deploy/pathplanner/paths/R4_P.path | 59 +++++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 13 +++++ 3 files changed, 109 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/B-L2.auto create mode 100644 src/main/deploy/pathplanner/paths/R4_P.path diff --git a/src/main/deploy/pathplanner/autos/B-L2.auto b/src/main/deploy/pathplanner/autos/B-L2.auto new file mode 100644 index 0000000..6d96e83 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/B-L2.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "B_R4" + } + }, + { + "type": "named", + "data": { + "name": "L2" + } + }, + { + "type": "named", + "data": { + "name": "AlgaeL3" + } + }, + { + "type": "path", + "data": { + "pathName": "R4_P" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R4_P.path b/src/main/deploy/pathplanner/paths/R4_P.path new file mode 100644 index 0000000..92dca2c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/R4_P.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.1, + "y": 4.05 + }, + "prevControl": null, + "nextControl": { + "x": 7.228887668477433, + "y": 0.6385793840777492 + }, + "isLocked": false, + "linkedName": "R4" + }, + { + "anchor": { + "x": 7.2735, + "y": 0.905 + }, + "prevControl": { + "x": 7.265791075075885, + "y": 1.1548811166865443 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8f35e9d..4276e9d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -212,6 +212,19 @@ public RobotContainer() { // AutoAlign to Intake + Intake NamedCommands.registerCommand("Intake", multiSubsystemCommands.loadGamepiece()); + + // AutoAlign + Algae Removal + NamedCommands.registerCommand("AlgaeL2", + multiSubsystemCommands.setOverallSetpoint(OverallPosition.L2) + .andThen(multiSubsystemCommands.waitForOverallMechanism()) + .andThen(multiSubsystemCommands.loadAlgae())); + + // AutoAlign + Algae Removal + NamedCommands.registerCommand("AlgaeL3", + multiSubsystemCommands.setOverallSetpoint(OverallPosition.L3) + .andThen(multiSubsystemCommands.waitForOverallMechanism()) + .andThen(multiSubsystemCommands.loadAlgae())); + autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); From 2a4ad5f5a027b09485fe5ef3cd363579f47a20b9 Mon Sep 17 00:00:00 2001 From: area5188 Date: Thu, 13 Mar 2025 21:01:37 -0400 Subject: [PATCH 011/106] More changes from 3/13 --- src/main/java/frc/robot/RobotContainer.java | 20 +++++++++---------- .../java/frc/robot/subsystems/arm/Arm.java | 10 +++++++--- .../frc/robot/subsystems/arm/ArmCommands.java | 2 +- .../robot/subsystems/elevator/Elevator.java | 11 ++++++++-- .../MultiSubsystemCommands.java | 6 ++++++ 5 files changed, 33 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8f35e9d..1561823 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -84,7 +84,7 @@ public class RobotContainer { private final Preset preset = new Preset(); private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max + private double MaxAngularRate = RotationsPerSecond.of(0.5).in(RadiansPerSecond); // 1/2 of a rotation per second max // angular velocity /* Setting up bindings for necessary control of the swerve drive platform */ @@ -108,8 +108,8 @@ public class RobotContainer { private final GenericHID buttonbox2 = new GenericHID(2); private final JoystickButton manualIntakeButton = new JoystickButton(buttonbox2, 1); private final JoystickButton presetButton = new JoystickButton(buttonbox2, 2); - private final JoystickButton incrementButton = new JoystickButton(buttonbox2, 4); - private final JoystickButton decrementButton = new JoystickButton(buttonbox2, 7); + private final JoystickButton incrementElevatorButton = new JoystickButton(buttonbox2, 4); + private final JoystickButton decrementElevatorButton = new JoystickButton(buttonbox2, 7); private final JoystickButton dynamic = new JoystickButton(buttonbox2, 8); private final JoystickButton qstatic = new JoystickButton(buttonbox2, 9); @@ -249,7 +249,7 @@ private void configureBindings() { // drive.registerTelemetry(logger::telemeterize); - intakeButton.onTrue(multiSubsystemCommands.loadGamepiece().raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving())); + intakeButton.onTrue(multiSubsystemCommands.loadGamepiece());//.raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving())); spitButton.onTrue(armCommands.spit()); L1Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L1)); @@ -264,10 +264,10 @@ private void configureBindings() { // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. - dynamic.and(joystick.y()).whileTrue(drive.sysIdDynamic(Direction.kForward)); - dynamic.and(joystick.x()).whileTrue(drive.sysIdDynamic(Direction.kReverse)); - qstatic.and(joystick.y()).whileTrue(drive.sysIdQuasistatic(Direction.kForward)); - qstatic.and(joystick.x()).whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); + // dynamic.and(joystick.y()).whileTrue(drive.sysIdDynamic(Direction.kForward)); + // dynamic.and(joystick.x()).whileTrue(drive.sysIdDynamic(Direction.kReverse)); + // qstatic.and(joystick.y()).whileTrue(drive.sysIdQuasistatic(Direction.kForward)); + // qstatic.and(joystick.x()).whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); // Driver Right Bumper: Approach Nearest Right-Side Reef Branch joystick.rightBumper() @@ -291,8 +291,8 @@ private void configureBindings() { presetButton.and(L3Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L3)); presetButton.and(L4Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L4)); - incrementButton.onTrue(elevatorCommands.incrementElevatorPosition()); - decrementButton.onTrue(elevatorCommands.decrementElevatorPosition()); + incrementElevatorButton.onTrue(elevatorCommands.incrementElevatorPosition()); + decrementElevatorButton.onTrue(elevatorCommands.decrementElevatorPosition()); /* // Driver Left Bumper and Algae mode: Approach Nearest Reef Face joystick.rightBumper() diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 6be604f..a247539 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -48,6 +48,7 @@ public enum ArmPosition { private ArmIOInputsAutoLogged _inputs; private boolean _prevLightSensorVal; private boolean _hasGamepiece; + private boolean _atSetpoint; private int _intakeSpikeCounter; private ArmPosition _currentPos; private ArmPosition _desiredPos; @@ -57,7 +58,7 @@ public enum ArmPosition { private static final double KP = 0.06;//0.09; private static final double KI = 0; //0.01; - private static final double KD = 0.005; + private static final double KD = 0.01; private static final double PROFILE_VEL = 160; private static final double PROFILE_ACC = 145; @@ -73,13 +74,14 @@ public Arm(ArmIO io) { _inputs = new ArmIOInputsAutoLogged(); _armPidController = new ProfiledPIDController(KP, KI, KD, new Constraints(PROFILE_VEL, PROFILE_ACC)); - _armPidController.setTolerance(7); + _armPidController.setTolerance(4); } public void setArmSetpoint(ArmPosition setpoint) { _armPidController.reset(_inputs._armEncoderPositionDegrees); _armPidController.setGoal(setpoint.getAngle(_currentMode)); _desiredPos = setpoint; + _atSetpoint = false; } public void setIntakeSpeed(double speed) { @@ -144,8 +146,10 @@ public boolean lightSensorSeesGamepiece() { public boolean isAtSetpoint() { boolean atSetpoint = _armPidController.atGoal(); - if (atSetpoint) + if (atSetpoint) { _currentPos = _desiredPos; + _atSetpoint = true; + } return atSetpoint; } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index f46c87e..51fcebf 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -103,7 +103,7 @@ public void initialize() { @Override public void execute() { - double speed = (movingDown) ? 0.065 : -0.065; + double speed = (movingDown) ? 0.07 : -0.07; _arm.setIntakeSpeed(speed); } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 932bea0..69e8a88 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -35,7 +35,8 @@ public enum ElevatorPosition { L2(11, 20), L3(27, 38), L4(52, 52), - Stow(0.5, 0.5); + Stow(0.5, 0.5), + Manual(0, 0); double coralHeight, algaeHeight; @@ -69,6 +70,7 @@ public enum ElevatorPosition { private static final double FEEDFORWARD_CONSTANT = 0.225; private boolean _isCalibrated; + private boolean _atSetpoint; private ElevatorPosition _currentPos; private ElevatorPosition _desiredPos; private ElevatorPosition _prevPos; @@ -115,6 +117,7 @@ public void setSetpoint(ElevatorPosition setpoint) { public void setSetpoint(double setpoint) { if (setpoint < 0.25 || setpoint > ELEVATOR_MAX_INCHES) return; + _atSetpoint = false; _elevatorMotorPID.reset(getCurrentPosInches()); _elevatorMotorPID.setGoal(setpoint); } @@ -122,18 +125,22 @@ public void setSetpoint(double setpoint) { // Checks if it is at the setpoint public boolean isAtSetpoint() { boolean atSetpoint = Math.abs(_elevatorMotorPID.getGoal().position - getCurrentPosInches()) <= 0.5; - if (atSetpoint) + if (atSetpoint) { _currentPos = _desiredPos; + _atSetpoint = true; + } return atSetpoint; } // Increases elevator position public void incrementElevatorPosition() { + _currentPos = ElevatorPosition.Manual; setSetpoint(_elevatorMotorPID.getGoal().position + INCREMENT_CONSTANT); } // Decreases elevator position public void decrementElevatorPosition() { + _currentPos = ElevatorPosition.Manual; setSetpoint(_elevatorMotorPID.getGoal().position - DECREMENT_CONSTANT); } diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 70f7a36..b953b26 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -174,6 +174,9 @@ public boolean canMoveToPos(ElevatorPosition currentElevator, ElevatorPosition d case Stow: canMoveElevator = (currentArm != ArmPosition.L4_Score); break; + case Manual: + canMoveElevator = currentArm == ArmPosition.Stow; + break; default: canMoveElevator = false; break; @@ -208,6 +211,9 @@ public boolean canMoveToPos(ElevatorPosition currentElevator, ElevatorPosition d case Stow: canMoveElevator = currentArm != ArmPosition.Loading; break; + case Manual: + canMoveElevator = currentArm == ArmPosition.Stow; + break; default: canMoveElevator = false; break; From 347529dda22bed98d5890f4d882bff82bea981f1 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Fri, 14 Mar 2025 12:37:02 -0400 Subject: [PATCH 012/106] Added elevator auto adjust --- .../robot/subsystems/elevator/Elevator.java | 28 ++++++++++++++----- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 69e8a88..61893b0 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -9,6 +9,8 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volt; +import java.util.HashMap; + import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -53,8 +55,8 @@ public enum ElevatorPosition { private static final double CALIBRATION_SPEED = -0.1; private static final double HARD_STOP_CURRENT_LIMIT = 37; - private static final double INCREMENT_CONSTANT = 1; - private static final double DECREMENT_CONSTANT = 1; + private static final int INCREMENT_CONSTANT = 1; + private static final int DECREMENT_CONSTANT = 1; private static final double ELEVATOR_MOTOR_KP = 0.8; //0.75; private static final double ELEVATOR_MOTOR_KI = 0;//0.15; @@ -75,6 +77,7 @@ public enum ElevatorPosition { private ElevatorPosition _desiredPos; private ElevatorPosition _prevPos; private MultiSubsystemCommands.GamepieceMode _currentMode; + private HashMap _manualAdjustments; private ProfiledPIDController _elevatorMotorPID; @@ -92,6 +95,8 @@ public Elevator(ElevatorIO io) { _currentPos = ElevatorPosition.Stow; _desiredPos = ElevatorPosition.Stow; _prevPos = ElevatorPosition.Stow; + + _manualAdjustments = new HashMap<>(); } // Runs the motors down at the calibration speed @@ -110,7 +115,8 @@ public void setElevatorVoltage(Voltage voltage) { public void setSetpoint(ElevatorPosition setpoint) { _prevPos = _currentPos; _desiredPos = setpoint; - setSetpoint(setpoint.getHeight(_currentMode)); + String key = getManualAdjustKey(); + setSetpoint(setpoint.getHeight(_currentMode) + _manualAdjustments.getOrDefault(key, 0)); } // Sets the setpoint of the PID @@ -132,16 +138,24 @@ public boolean isAtSetpoint() { return atSetpoint; } + private String getManualAdjustKey() { + return _currentPos.toString() + _currentMode.toString(); + } + // Increases elevator position public void incrementElevatorPosition() { - _currentPos = ElevatorPosition.Manual; - setSetpoint(_elevatorMotorPID.getGoal().position + INCREMENT_CONSTANT); + String key = getManualAdjustKey(); + Integer offset = _manualAdjustments.getOrDefault(key, 0) + INCREMENT_CONSTANT; + _manualAdjustments.put(key, offset); + setSetpoint(_currentPos); } // Decreases elevator position public void decrementElevatorPosition() { - _currentPos = ElevatorPosition.Manual; - setSetpoint(_elevatorMotorPID.getGoal().position - DECREMENT_CONSTANT); + String key = getManualAdjustKey(); + Integer offset = _manualAdjustments.getOrDefault(key, 0) + DECREMENT_CONSTANT; + _manualAdjustments.put(key, offset); + setSetpoint(_currentPos); } // Checks if above limit From 574ef9ad71e78dfb734a3f16f3397fb667b03d0b Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Fri, 14 Mar 2025 12:57:14 -0400 Subject: [PATCH 013/106] Removed subsystem reqirements for manual adjusts --- .../java/frc/robot/subsystems/elevator/ElevatorCommands.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java index 02025b2..10dc88a 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java @@ -24,7 +24,7 @@ public Command decrementElevatorPosition() { () -> { _elevator.decrementElevatorPosition(); - } , _elevator); + }); } public Command incrementElevatorPosition(){ @@ -32,7 +32,7 @@ public Command incrementElevatorPosition(){ () -> { _elevator.incrementElevatorPosition(); - }, _elevator); + }); } public Command setElevatorSetpoint(ElevatorPosition setpoint) { From ded0d7062afcacc6f89c11c6065922612607afa8 Mon Sep 17 00:00:00 2001 From: area5188 Date: Fri, 14 Mar 2025 14:31:26 -0400 Subject: [PATCH 014/106] Manual elevator adjustment tested --- src/main/java/frc/robot/RobotContainer.java | 18 +++++++-------- .../java/frc/robot/subsystems/arm/Arm.java | 2 ++ .../robot/subsystems/elevator/Elevator.java | 23 +++++++++++-------- 3 files changed, 25 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3f3a44d..4ff4e4b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,10 +67,10 @@ public class RobotContainer { private final Drive drive; private final Elevator elevatorSubsystem = new Elevator(new RealElevatorIO()); private final Arm armSubsystem = new Arm(new RealArmIO()); - private final LEDs LEDSubsystem = new LEDs(); + // private final LEDs LEDSubsystem = new LEDs(); private final ElevatorCommands elevatorCommands = new ElevatorCommands(elevatorSubsystem); private final ArmCommands armCommands = new ArmCommands(armSubsystem); - private final LEDsCommands LEDCommands = new LEDsCommands(LEDSubsystem); + // private final LEDsCommands LEDCommands = new LEDsCommands(LEDSubsystem); private final Climber climber = new Climber(new RealClimberIO()); private final ClimberCommands ClimberCommands = new ClimberCommands(climber); @@ -296,7 +296,7 @@ private void configureBindings() { // Resets the preset when we don't have a piece. - armSubsystem._hasPiece.onFalse(preset.resetPreset().andThen(LEDCommands.pickingUpCoral())); + // armSubsystem._hasPiece.onFalse(preset.resetPreset().andThen(LEDCommands.pickingUpCoral())); // Sets the level preset presetButton.and(L1Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L1)); @@ -374,15 +374,15 @@ public void calibrateAndStartPIDs() { } public void startIdleAnimations() { - Command disabled1 = LEDCommands.disabledAnimation1(); - if (!CommandScheduler.getInstance().isScheduled(disabled1)) - CommandScheduler.getInstance().schedule(disabled1); + // Command disabled1 = LEDCommands.disabledAnimation1(); + // if (!CommandScheduler.getInstance().isScheduled(disabled1)) + // CommandScheduler.getInstance().schedule(disabled1); } public void startEnabledLEDs() { - Command initialLEDs = LEDCommands.pickingUpCoral(); - if (!CommandScheduler.getInstance().isScheduled(initialLEDs)) - CommandScheduler.getInstance().schedule(initialLEDs); + // Command initialLEDs = LEDCommands.pickingUpCoral(); + // if (!CommandScheduler.getInstance().isScheduled(initialLEDs)) + // CommandScheduler.getInstance().schedule(initialLEDs); } } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index a247539..c4b3333 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -73,6 +73,8 @@ public Arm(ArmIO io) { _io = io; _inputs = new ArmIOInputsAutoLogged(); + _currentMode = GamepieceMode.CORAL; + _armPidController = new ProfiledPIDController(KP, KI, KD, new Constraints(PROFILE_VEL, PROFILE_ACC)); _armPidController.setTolerance(4); } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 61893b0..e1f3f69 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -95,6 +95,7 @@ public Elevator(ElevatorIO io) { _currentPos = ElevatorPosition.Stow; _desiredPos = ElevatorPosition.Stow; _prevPos = ElevatorPosition.Stow; + _currentMode = GamepieceMode.CORAL; _manualAdjustments = new HashMap<>(); } @@ -139,23 +140,27 @@ public boolean isAtSetpoint() { } private String getManualAdjustKey() { - return _currentPos.toString() + _currentMode.toString(); + return _desiredPos.toString() + _currentMode.toString(); } // Increases elevator position public void incrementElevatorPosition() { - String key = getManualAdjustKey(); - Integer offset = _manualAdjustments.getOrDefault(key, 0) + INCREMENT_CONSTANT; - _manualAdjustments.put(key, offset); - setSetpoint(_currentPos); + if (_currentPos == _desiredPos) { + String key = getManualAdjustKey(); + Integer offset = _manualAdjustments.getOrDefault(key, 0) + INCREMENT_CONSTANT; + _manualAdjustments.put(key, offset); + setSetpoint(_currentPos); + } } // Decreases elevator position public void decrementElevatorPosition() { - String key = getManualAdjustKey(); - Integer offset = _manualAdjustments.getOrDefault(key, 0) + DECREMENT_CONSTANT; - _manualAdjustments.put(key, offset); - setSetpoint(_currentPos); + if (_currentPos == _desiredPos) { + String key = getManualAdjustKey(); + Integer offset = _manualAdjustments.getOrDefault(key, 0) - DECREMENT_CONSTANT; + _manualAdjustments.put(key, offset); + setSetpoint(_currentPos); + } } // Checks if above limit From 31b7d67bb0932dca47f528becfc633f510484ca2 Mon Sep 17 00:00:00 2001 From: area5188 Date: Fri, 14 Mar 2025 20:06:16 -0400 Subject: [PATCH 015/106] Adding drive practice changes --- src/main/java/frc/robot/RobotContainer.java | 40 ++++++--------------- 1 file changed, 10 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4ff4e4b..c124fa2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -95,19 +95,19 @@ public class RobotContainer { private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); private final GenericHID buttonbox1 = new GenericHID(1); - private final JoystickButton L1Button = new JoystickButton(buttonbox1, 1); - private final JoystickButton L2Button = new JoystickButton(buttonbox1, 2); - private final JoystickButton StowButton = new JoystickButton(buttonbox1, 3); - private final JoystickButton L3Button = new JoystickButton(buttonbox1, 4); - private final JoystickButton L4Button = new JoystickButton(buttonbox1, 5); + private final GenericHID buttonbox2 = new GenericHID(2); + + private final JoystickButton StowButton = new JoystickButton(buttonbox2, 5); + private final JoystickButton L1Button = new JoystickButton(buttonbox2, 2); + private final JoystickButton L2Button = new JoystickButton(buttonbox1, 8); + private final JoystickButton L3Button = new JoystickButton(buttonbox1,5); + private final JoystickButton L4Button = new JoystickButton(buttonbox1, 2); + private final JoystickButton intakeButton = new JoystickButton(buttonbox1, 7); - private final JoystickButton L4_scoreButton = new JoystickButton(buttonbox1, 8); private final JoystickButton spitButton = new JoystickButton(buttonbox1, 9); + private final JoystickButton gamepieceModeToggle = new JoystickButton(buttonbox1, 10); - private final GenericHID buttonbox2 = new GenericHID(2); - private final JoystickButton manualIntakeButton = new JoystickButton(buttonbox2, 1); - private final JoystickButton presetButton = new JoystickButton(buttonbox2, 2); private final JoystickButton incrementElevatorButton = new JoystickButton(buttonbox2, 4); private final JoystickButton decrementElevatorButton = new JoystickButton(buttonbox2, 7); @@ -265,12 +265,11 @@ private void configureBindings() { intakeButton.onTrue(multiSubsystemCommands.loadGamepiece());//.raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving())); spitButton.onTrue(armCommands.spit()); + StowButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.Stow)); L1Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L1)); L2Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L2)); - StowButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.Stow)); L3Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L3)); L4Button.onTrue(multiSubsystemCommands.goToL4()); - L4_scoreButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4_Score)); gamepieceModeToggle.whileTrue(multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE)); gamepieceModeToggle.whileFalse(multiSubsystemCommands.setGamepieceMode(GamepieceMode.CORAL)); @@ -294,16 +293,6 @@ private void configureBindings() { joystickApproach( () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.LEFT))); - - // Resets the preset when we don't have a piece. - // armSubsystem._hasPiece.onFalse(preset.resetPreset().andThen(LEDCommands.pickingUpCoral())); - - // Sets the level preset - presetButton.and(L1Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L1)); - presetButton.and(L2Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L2)); - presetButton.and(L3Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L3)); - presetButton.and(L4Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L4)); - incrementElevatorButton.onTrue(elevatorCommands.incrementElevatorPosition()); decrementElevatorButton.onTrue(elevatorCommands.decrementElevatorPosition()); /* @@ -326,16 +315,7 @@ private void configureBindings() { // // reset the field-centric heading on left bumper press // joystick.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); - - - // Resets the preset when we don't have a piece. - armSubsystem._hasPiece.onFalse(preset.resetPreset()); - // Sets the level preset - presetButton.and(L1Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L1)); - presetButton.and(L2Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L2)); - presetButton.and(L3Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L3)); - presetButton.and(L4Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L4)); } From c20e39f3146fa77ea064747dccdac6bb9ffaa28c Mon Sep 17 00:00:00 2001 From: shlap Date: Sat, 15 Mar 2025 08:55:54 -0400 Subject: [PATCH 016/106] moved autos around --- src/main/deploy/pathplanner/autos/LeaveA.auto | 2 +- src/main/deploy/pathplanner/paths/TestPath.path | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/LeaveA.auto b/src/main/deploy/pathplanner/autos/LeaveA.auto index c4245a5..d8b7a51 100644 --- a/src/main/deploy/pathplanner/autos/LeaveA.auto +++ b/src/main/deploy/pathplanner/autos/LeaveA.auto @@ -14,6 +14,6 @@ } }, "resetOdom": true, - "folder": "LeaveStart", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TestPath.path b/src/main/deploy/pathplanner/paths/TestPath.path index 42d6faf..641d703 100644 --- a/src/main/deploy/pathplanner/paths/TestPath.path +++ b/src/main/deploy/pathplanner/paths/TestPath.path @@ -20,8 +20,8 @@ "y": 6.0 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 3.752151639344262, + "y": 6.6563012295081965 }, "nextControl": null, "isLocked": false, From ca2cd949caa5d172b86d736613361ad645858180 Mon Sep 17 00:00:00 2001 From: shlap Date: Sat, 15 Mar 2025 09:50:35 -0400 Subject: [PATCH 017/106] new autos --- src/main/deploy/pathplanner/autos/R3_L4.auto | 31 +++++++++++ .../deploy/pathplanner/autos/Score+Algae.auto | 37 +++++++++++++ src/main/deploy/pathplanner/paths/A_R3.path | 8 +-- src/main/deploy/pathplanner/paths/B_R3.path | 8 +-- src/main/deploy/pathplanner/paths/C_R3.path | 8 +-- .../pathplanner/paths/CoralAlgae_R3.path | 54 +++++++++++++++++++ src/main/deploy/pathplanner/paths/HP1_R3.path | 8 +-- src/main/deploy/pathplanner/paths/HP2_R3.path | 8 +-- src/main/deploy/pathplanner/paths/R3_HP1.path | 8 +-- src/main/deploy/pathplanner/paths/R3_HP2.path | 8 +-- src/main/java/frc/robot/RobotContainer.java | 7 +++ 11 files changed, 157 insertions(+), 28 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/R3_L4.auto create mode 100644 src/main/deploy/pathplanner/autos/Score+Algae.auto create mode 100644 src/main/deploy/pathplanner/paths/CoralAlgae_R3.path diff --git a/src/main/deploy/pathplanner/autos/R3_L4.auto b/src/main/deploy/pathplanner/autos/R3_L4.auto new file mode 100644 index 0000000..7bebc35 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/R3_L4.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A_R3" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Score+Algae.auto b/src/main/deploy/pathplanner/autos/Score+Algae.auto new file mode 100644 index 0000000..a83d321 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Score+Algae.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A_R3" + } + }, + { + "type": "named", + "data": { + "name": "L2" + } + }, + { + "type": "path", + "data": { + "pathName": "CoralAlgae_R3" + } + }, + { + "type": "named", + "data": { + "name": "AlgaeL3" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index 3e052d8..7460f91 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.27, - "y": 5.43 + "x": 5.166700819672132, + "y": 5.5414446721311466 }, "prevControl": { - "x": 5.045108479704744, - "y": 5.299100830755238 + "x": 4.941809299376876, + "y": 5.410545502886385 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/B_R3.path b/src/main/deploy/pathplanner/paths/B_R3.path index 71b925e..0ca5cc2 100644 --- a/src/main/deploy/pathplanner/paths/B_R3.path +++ b/src/main/deploy/pathplanner/paths/B_R3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.27, - "y": 5.43 + "x": 5.166700819672132, + "y": 5.5414446721311466 }, "prevControl": { - "x": 6.309522336579676, - "y": 5.75024510478078 + "x": 6.206223156251808, + "y": 5.861689776911927 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C_R3.path b/src/main/deploy/pathplanner/paths/C_R3.path index 4e34e79..0bafd8e 100644 --- a/src/main/deploy/pathplanner/paths/C_R3.path +++ b/src/main/deploy/pathplanner/paths/C_R3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.27, - "y": 5.43 + "x": 5.166700819672132, + "y": 5.5414446721311466 }, "prevControl": { - "x": 6.30677323496538, - "y": 5.292850243595853 + "x": 6.203474054637512, + "y": 5.404294915727 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/CoralAlgae_R3.path b/src/main/deploy/pathplanner/paths/CoralAlgae_R3.path new file mode 100644 index 0000000..c78d839 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CoralAlgae_R3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.166700819672132, + "y": 5.5414446721311466 + }, + "prevControl": null, + "nextControl": { + "x": 5.376465786078236, + "y": 5.405434897230054 + }, + "isLocked": false, + "linkedName": "R3" + }, + { + "anchor": { + "x": 5.324783571442977, + "y": 5.43542116633939 + }, + "prevControl": { + "x": 5.105176780835683, + "y": 5.554890231453185 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "R3Algae" + } + ], + "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": -121.5 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -121.5 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP1_R3.path b/src/main/deploy/pathplanner/paths/HP1_R3.path index cf08bd7..1f5a8d7 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R3.path +++ b/src/main/deploy/pathplanner/paths/HP1_R3.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 5.27, - "y": 5.43 + "x": 5.166700819672132, + "y": 5.5414446721311466 }, "prevControl": { - "x": 5.032070058765731, - "y": 5.506742055382024 + "x": 4.928770878437863, + "y": 5.618186727513171 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R3.path b/src/main/deploy/pathplanner/paths/HP2_R3.path index 228d14e..0dfd16a 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R3.path +++ b/src/main/deploy/pathplanner/paths/HP2_R3.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 5.27, - "y": 5.43 + "x": 5.166700819672132, + "y": 5.5414446721311466 }, "prevControl": { - "x": 5.332471627267896, - "y": 6.459744247891212 + "x": 5.229172446940028, + "y": 6.571188920022359 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R3_HP1.path b/src/main/deploy/pathplanner/paths/R3_HP1.path index 3e64fc9..bffcc76 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/R3_HP1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.27, - "y": 5.43 + "x": 5.166700819672132, + "y": 5.5414446721311466 }, "prevControl": null, "nextControl": { - "x": 5.269121552272354, - "y": 5.770187198760307 + "x": 5.165822371944486, + "y": 5.881631870891454 }, "isLocked": false, "linkedName": "R3" diff --git a/src/main/deploy/pathplanner/paths/R3_HP2.path b/src/main/deploy/pathplanner/paths/R3_HP2.path index 7c49a68..5e0e0f8 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP2.path +++ b/src/main/deploy/pathplanner/paths/R3_HP2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.27, - "y": 5.43 + "x": 5.166700819672132, + "y": 5.5414446721311466 }, "prevControl": null, "nextControl": { - "x": 4.54124935998884, - "y": 6.801706917455025 + "x": 4.437950179660972, + "y": 6.913151589586172 }, "isLocked": false, "linkedName": "R3" diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8f35e9d..f154213 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -213,6 +213,13 @@ public RobotContainer() { NamedCommands.registerCommand("Intake", multiSubsystemCommands.loadGamepiece()); + NamedCommands.registerCommand("AlgaeL2", + multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE) + .andThen(multiSubsystemCommands.loadAlgae())); + + NamedCommands.registerCommand("AlgaeL3", + multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE) + .andThen(multiSubsystemCommands.loadAlgae())); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); From 28bbf8d552df01839db124b958265473d9374d09 Mon Sep 17 00:00:00 2001 From: area5188 Date: Sat, 15 Mar 2025 10:03:46 -0400 Subject: [PATCH 018/106] Added algae align --- src/main/java/frc/robot/RobotContainer.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c124fa2..96a58c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -293,6 +293,13 @@ private void configureBindings() { joystickApproach( () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.LEFT))); + + // a -button approach reef + joystick.a() + .whileTrue( + joystickApproach( + () -> FieldConstants.getNearestReefFace(drive.getPose()))); + incrementElevatorButton.onTrue(elevatorCommands.incrementElevatorPosition()); decrementElevatorButton.onTrue(elevatorCommands.decrementElevatorPosition()); /* From 1a779c90a71162c67f8384b3a8c8281e560c2b0f Mon Sep 17 00:00:00 2001 From: shlap Date: Sat, 15 Mar 2025 10:08:54 -0400 Subject: [PATCH 019/106] Removed unneccesary autos --- src/main/deploy/pathplanner/autos/B-L2.auto | 37 ------------- src/main/deploy/pathplanner/paths/R4_P.path | 59 --------------------- 2 files changed, 96 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/B-L2.auto delete mode 100644 src/main/deploy/pathplanner/paths/R4_P.path diff --git a/src/main/deploy/pathplanner/autos/B-L2.auto b/src/main/deploy/pathplanner/autos/B-L2.auto deleted file mode 100644 index 6d96e83..0000000 --- a/src/main/deploy/pathplanner/autos/B-L2.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "B_R4" - } - }, - { - "type": "named", - "data": { - "name": "L2" - } - }, - { - "type": "named", - "data": { - "name": "AlgaeL3" - } - }, - { - "type": "path", - "data": { - "pathName": "R4_P" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R4_P.path b/src/main/deploy/pathplanner/paths/R4_P.path deleted file mode 100644 index 92dca2c..0000000 --- a/src/main/deploy/pathplanner/paths/R4_P.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 6.1, - "y": 4.05 - }, - "prevControl": null, - "nextControl": { - "x": 7.228887668477433, - "y": 0.6385793840777492 - }, - "isLocked": false, - "linkedName": "R4" - }, - { - "anchor": { - "x": 7.2735, - "y": 0.905 - }, - "prevControl": { - "x": 7.265791075075885, - "y": 1.1548811166865443 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -90.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0.0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file From 3423fd6dc6af983b6e96962db4eefdab693464f6 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Sat, 15 Mar 2025 13:22:47 -0400 Subject: [PATCH 020/106] Removed all requires from commands --- src/main/deploy/pathplanner/autos/A_R3.auto | 10 +++++-- src/main/java/frc/robot/Robot.java | 30 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 18 +++++------ .../frc/robot/subsystems/arm/ArmCommands.java | 17 +++++------ .../robot/subsystems/elevator/Elevator.java | 1 + .../subsystems/elevator/ElevatorCommands.java | 4 +-- 6 files changed, 56 insertions(+), 24 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/A_R3.auto b/src/main/deploy/pathplanner/autos/A_R3.auto index 2cbc044..55ca871 100644 --- a/src/main/deploy/pathplanner/autos/A_R3.auto +++ b/src/main/deploy/pathplanner/autos/A_R3.auto @@ -7,13 +7,19 @@ { "type": "path", "data": { - "pathName": "A_R2" + "pathName": "A_R3" + } + }, + { + "type": "named", + "data": { + "name": "L2" } } ] } }, "resetOdom": true, - "folder": "StartToReef", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 446a98b..e3fb6e5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,7 +6,10 @@ import java.io.IOException; import java.util.ArrayList; +import java.util.HashMap; import java.util.List; +import java.util.Map; +import java.util.function.BiConsumer; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -74,6 +77,33 @@ public Robot() { Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may // be added. + + // Log active commands + Map commandCounts = new HashMap<>(); + BiConsumer logCommandFunction = + (Command command, Boolean active) -> { + String name = command.getName(); + int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); + commandCounts.put(name, count); + Logger.recordOutput( + "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); + Logger.recordOutput("CommandsAll/" + name, count > 0); + }; + CommandScheduler.getInstance() + .onCommandInitialize( + (Command command) -> { + logCommandFunction.accept(command, true); + }); + CommandScheduler.getInstance() + .onCommandFinish( + (Command command) -> { + logCommandFunction.accept(command, false); + }); + CommandScheduler.getInstance() + .onCommandInterrupt( + (Command command) -> { + logCommandFunction.accept(command, false); + }); m_robotContainer = new RobotContainer(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3f3a44d..4ff4e4b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,10 +67,10 @@ public class RobotContainer { private final Drive drive; private final Elevator elevatorSubsystem = new Elevator(new RealElevatorIO()); private final Arm armSubsystem = new Arm(new RealArmIO()); - private final LEDs LEDSubsystem = new LEDs(); + // private final LEDs LEDSubsystem = new LEDs(); private final ElevatorCommands elevatorCommands = new ElevatorCommands(elevatorSubsystem); private final ArmCommands armCommands = new ArmCommands(armSubsystem); - private final LEDsCommands LEDCommands = new LEDsCommands(LEDSubsystem); + // private final LEDsCommands LEDCommands = new LEDsCommands(LEDSubsystem); private final Climber climber = new Climber(new RealClimberIO()); private final ClimberCommands ClimberCommands = new ClimberCommands(climber); @@ -296,7 +296,7 @@ private void configureBindings() { // Resets the preset when we don't have a piece. - armSubsystem._hasPiece.onFalse(preset.resetPreset().andThen(LEDCommands.pickingUpCoral())); + // armSubsystem._hasPiece.onFalse(preset.resetPreset().andThen(LEDCommands.pickingUpCoral())); // Sets the level preset presetButton.and(L1Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L1)); @@ -374,15 +374,15 @@ public void calibrateAndStartPIDs() { } public void startIdleAnimations() { - Command disabled1 = LEDCommands.disabledAnimation1(); - if (!CommandScheduler.getInstance().isScheduled(disabled1)) - CommandScheduler.getInstance().schedule(disabled1); + // Command disabled1 = LEDCommands.disabledAnimation1(); + // if (!CommandScheduler.getInstance().isScheduled(disabled1)) + // CommandScheduler.getInstance().schedule(disabled1); } public void startEnabledLEDs() { - Command initialLEDs = LEDCommands.pickingUpCoral(); - if (!CommandScheduler.getInstance().isScheduled(initialLEDs)) - CommandScheduler.getInstance().schedule(initialLEDs); + // Command initialLEDs = LEDCommands.pickingUpCoral(); + // if (!CommandScheduler.getInstance().isScheduled(initialLEDs)) + // CommandScheduler.getInstance().schedule(initialLEDs); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 51fcebf..1a444eb 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -26,8 +26,7 @@ public Command spit() { () -> { _arm.setIntakeSpeed(0); _arm.clearHasGamepiece(); - }, - _arm).withTimeout(1); + }).withTimeout(1); } @@ -36,14 +35,14 @@ public Command setArmPosition(ArmPosition setpoint) { return new InstantCommand( () -> { _arm.setArmSetpoint(setpoint); - }, - _arm).andThen(intakeForNumberOfRotations()); + } + ).andThen(intakeForNumberOfRotations()); } return new InstantCommand( () -> { _arm.setArmSetpoint(setpoint); - }, - _arm); + } + ); } public Command intake() { @@ -57,8 +56,7 @@ private Command intakeCoral() { }, () -> { _arm.setIntakeSpeed(0); - }, - _arm).until(() -> _arm.hasPiece()); + }).until(() -> _arm.hasPiece()); } private Command intakeAlgae() { @@ -132,8 +130,7 @@ public Command intakeForNumberOfRotations() { }, () -> { _arm.setIntakeSpeed(0); - }, - _arm).until(() -> _arm.intakeAtDesiredRotations()); + }).until(() -> _arm.intakeAtDesiredRotations()); } public Command waitUntilAtSetpoint() { diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 61893b0..ce5d039 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -95,6 +95,7 @@ public Elevator(ElevatorIO io) { _currentPos = ElevatorPosition.Stow; _desiredPos = ElevatorPosition.Stow; _prevPos = ElevatorPosition.Stow; + _currentMode = GamepieceMode.CORAL; _manualAdjustments = new HashMap<>(); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java index 10dc88a..c687ab8 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java @@ -23,7 +23,6 @@ public Command decrementElevatorPosition() { return new InstantCommand( () -> { _elevator.decrementElevatorPosition(); - }); } @@ -31,7 +30,6 @@ public Command incrementElevatorPosition(){ return new InstantCommand( () -> { _elevator.incrementElevatorPosition(); - }); } @@ -39,7 +37,7 @@ public Command setElevatorSetpoint(ElevatorPosition setpoint) { return new InstantCommand( () -> { _elevator.setSetpoint(setpoint); - }, _elevator); + }); } public Command waitUntilAtSetpoint() { From 652884879d149b9e5bfca819720dfd6009fcf6a5 Mon Sep 17 00:00:00 2001 From: area5188 Date: Sat, 15 Mar 2025 14:12:09 -0400 Subject: [PATCH 021/106] Widened tolerances for elevator and added wait for algae intake --- src/main/java/frc/robot/Robot.java | 92 +++++++++---------- src/main/java/frc/robot/RobotContainer.java | 9 +- .../frc/robot/subsystems/arm/ArmCommands.java | 12 ++- .../robot/subsystems/elevator/Elevator.java | 4 +- 4 files changed, 63 insertions(+), 54 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 446a98b..7c52db3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -46,7 +46,7 @@ public class Robot extends LoggedRobot { public static final Translation2d fieldCenter = new Translation2d(fieldLength / 2, fieldWidth / 2); - private final RobotContainer m_robotContainer; + private final RobotContainer m_robotContainer = new RobotContainer(); public Robot() { @@ -75,7 +75,6 @@ public Robot() { Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may // be added. - m_robotContainer = new RobotContainer(); } @Override @@ -86,59 +85,58 @@ public void robotPeriodic() { @Override public void disabledInit() { m_robotContainer.startIdleAnimations(); - SmartDashboard.putData("Auto Trajectory", m_autoTraj); + // SmartDashboard.putData("Auto Trajectory", m_autoTraj); } @Override public void disabledPeriodic() { - { var m_alliance = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; // Get currently selected command - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - // Check if is the same as the last one - if (m_autonomousCommand != m_lastAutonomousCommand && m_autonomousCommand != null) { - // Check if its contained in the list of our autos - if (AutoBuilder.getAllAutoNames().contains(m_autonomousCommand.getName())) { - // Clear the current path - m_pathsToShow.clear(); - // Grabs all paths from the auto - try { - for (PathPlannerPath path : PathPlannerAuto - .getPathGroupFromAutoFile(m_autonomousCommand.getName())) { - // Adds all poses to master list - m_pathsToShow.addAll(path.getPathPoses()); - } - } catch (IOException | ParseException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - // Check to see which alliance we are on Red Alliance - if (m_alliance) { - for (int i = 0; i < m_pathsToShow.size(); i++) { - m_pathsToShow.set(i, - m_pathsToShow.get(i).rotateAround(fieldCenter, Rotation2d.k180deg)); - } - } - // Displays all poses on Field2d widget - m_autoTraj.getObject("traj").setPoses(m_pathsToShow); - } - } - m_lastAutonomousCommand = m_autonomousCommand; - - if (!m_pathsToShow.isEmpty()) { - var firstPose = m_pathsToShow.get(0); - Logger.recordOutput("Alignment/StartPose", firstPose); - SmartDashboard.putBoolean("Alignment/Translation", - firstPose.getTranslation().getDistance( - m_robotContainer.getDrive().getPose().getTranslation()) <= Units - .inchesToMeters(1.5)); - SmartDashboard.putBoolean("Alignment/Rotation", - firstPose.getRotation().minus(m_robotContainer.getDrive().getPose().getRotation()) - .getDegrees() < 1); - } - } + // m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + // // Check if is the same as the last one + // if (m_autonomousCommand != m_lastAutonomousCommand && m_autonomousCommand != null) { + // // Check if its contained in the list of our autos + // if (AutoBuilder.getAllAutoNames().contains(m_autonomousCommand.getName())) { + // // Clear the current path + // m_pathsToShow.clear(); + // // Grabs all paths from the auto + // try { + // for (PathPlannerPath path : PathPlannerAuto + // .getPathGroupFromAutoFile(m_autonomousCommand.getName())) { + // // Adds all poses to master list + // m_pathsToShow.addAll(path.getPathPoses()); + // } + // } catch (IOException | ParseException e) { + // // TODO Auto-generated catch block + // e.printStackTrace(); + // } + // // Check to see which alliance we are on Red Alliance + // if (m_alliance) { + // for (int i = 0; i < m_pathsToShow.size(); i++) { + // m_pathsToShow.set(i, + // m_pathsToShow.get(i).rotateAround(fieldCenter, Rotation2d.k180deg)); + // } + // } + // // Displays all poses on Field2d widget + // m_autoTraj.getObject("traj").setPoses(m_pathsToShow); + // } + // } + // m_lastAutonomousCommand = m_autonomousCommand; + + // if (!m_pathsToShow.isEmpty()) { + // var firstPose = m_pathsToShow.get(0); + // Logger.recordOutput("Alignment/StartPose", firstPose); + // SmartDashboard.putBoolean("Alignment/Translation", + // firstPose.getTranslation().getDistance( + // m_robotContainer.getDrive().getPose().getTranslation()) <= Units + // .inchesToMeters(1.5)); + // SmartDashboard.putBoolean("Alignment/Rotation", + // firstPose.getRotation().minus(m_robotContainer.getDrive().getPose().getRotation()) + // .getDegrees() < 1); + // } + // } } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2f15053..4dadaca 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -102,6 +102,7 @@ public class RobotContainer { private final JoystickButton L2Button = new JoystickButton(buttonbox1, 8); private final JoystickButton L3Button = new JoystickButton(buttonbox1,5); private final JoystickButton L4Button = new JoystickButton(buttonbox1, 2); + private final JoystickButton L4ScoreButton = new JoystickButton(buttonbox1, 3); private final JoystickButton intakeButton = new JoystickButton(buttonbox1, 7); private final JoystickButton spitButton = new JoystickButton(buttonbox1, 9); @@ -226,7 +227,7 @@ public RobotContainer() { .andThen(multiSubsystemCommands.loadAlgae())); - NamedCommands.registerCommand("AlgaeL2", + NamedCommands.registerCommand("AlgaeL2", multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE) .andThen(multiSubsystemCommands.loadAlgae())); @@ -234,6 +235,9 @@ public RobotContainer() { multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE) .andThen(multiSubsystemCommands.loadAlgae())); + NamedCommands.registerCommand("Stow", + multiSubsystemCommands.setOverallSetpoint(OverallPosition.Stow)); + autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); // hide the joystick missing warnings @@ -276,7 +280,8 @@ private void configureBindings() { L1Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L1)); L2Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L2)); L3Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L3)); - L4Button.onTrue(multiSubsystemCommands.goToL4()); + L4Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4)); + L4ScoreButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4_Score)); gamepieceModeToggle.whileTrue(multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE)); gamepieceModeToggle.whileFalse(multiSubsystemCommands.setGamepieceMode(GamepieceMode.CORAL)); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 51fcebf..3056a9f 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.arm; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.FunctionalCommand; @@ -64,28 +66,32 @@ private Command intakeCoral() { private Command intakeAlgae() { return new Command() { double intakeSpikeCounter = 0; + int counter = 0; @Override public void initialize() { intakeSpikeCounter = 0; + counter = 0; _arm.setIntakeSpeed(0.5); } @Override public void execute() { - if (_arm.getIntakeCurrent() >= Arm.HAS_ALGAE_CURRENT) { + counter++; + if (_arm.getIntakeCurrent() >= Arm.HAS_ALGAE_CURRENT && counter > 12) { intakeSpikeCounter++; } + Logger.recordOutput("Arm/intakeSpikeCount", intakeSpikeCounter); } @Override public void end(boolean interrupted) { - _arm.setIntakeSpeed(0.05); + _arm.setIntakeSpeed(0.08); } @Override public boolean isFinished() { - return intakeSpikeCounter > 15; + return intakeSpikeCounter > 3; } }; } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index e1f3f69..20ea474 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -90,7 +90,7 @@ public Elevator(ElevatorIO io) { _elevatorMotorPID = new ProfiledPIDController(ELEVATOR_MOTOR_KP, ELEVATOR_MOTOR_KI, ELEVATOR_MOTOR_KD, new Constraints(ELEVATOR_PID_VEL, ELEVATOR_PID_ACC)); - _elevatorMotorPID.setTolerance(0.5); + _elevatorMotorPID.setTolerance(1); _currentPos = ElevatorPosition.Stow; _desiredPos = ElevatorPosition.Stow; @@ -131,7 +131,7 @@ public void setSetpoint(double setpoint) { // Checks if it is at the setpoint public boolean isAtSetpoint() { - boolean atSetpoint = Math.abs(_elevatorMotorPID.getGoal().position - getCurrentPosInches()) <= 0.5; + boolean atSetpoint = Math.abs(_elevatorMotorPID.getGoal().position - getCurrentPosInches()) <= 1; if (atSetpoint) { _currentPos = _desiredPos; _atSetpoint = true; From 32db8fe272d5fc40ca4e73b2eaa7ace05bc1a13b Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Sat, 15 Mar 2025 15:04:22 -0400 Subject: [PATCH 022/106] Pushing auto + teleop fixes --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 13 ++++++++- .../frc/robot/subsystems/arm/ArmCommands.java | 9 ++++-- .../frc/robot/subsystems/autos/Autos.java | 29 +++++++++++++++++++ .../robot/subsystems/elevator/Elevator.java | 1 + 5 files changed, 49 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/autos/Autos.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e3fb6e5..f9c8ae8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -106,6 +106,7 @@ public Robot() { }); m_robotContainer = new RobotContainer(); + } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4ff4e4b..d60bed0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,14 +6,17 @@ import static edu.wpi.first.units.Units.*; +import java.io.IOException; import java.lang.invoke.VarHandle.AccessMode; import java.util.function.Supplier; +import org.json.simple.parser.ParseException; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.util.FileVersionException; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; @@ -31,6 +34,7 @@ import frc.robot.subsystems.arm.ArmCommands; import frc.robot.subsystems.arm.Arm.ArmPosition; import frc.robot.subsystems.arm.io.RealArmIO; +import frc.robot.subsystems.autos.Autos; import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.climber.ClimberCommands; import frc.robot.subsystems.climber.io.RealClimberIO; @@ -77,6 +81,7 @@ public class RobotContainer { private final MultiSubsystemCommands multiSubsystemCommands = new MultiSubsystemCommands(elevatorSubsystem, armSubsystem, elevatorCommands, armCommands); + private final Autos autos = new Autos(multiSubsystemCommands); private final Vision vision; private final CommandXboxController joystick = new CommandXboxController(0); @@ -340,7 +345,13 @@ private void configureBindings() { public Command getAutonomousCommand() { - return autoChooser.get(); + try { + return autos.A3_L4(); + } catch (FileVersionException | IOException | ParseException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + }//autoChooser.get(); + return null; } public Drive getDrive() { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 1a444eb..5d1d97f 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -26,7 +26,7 @@ public Command spit() { () -> { _arm.setIntakeSpeed(0); _arm.clearHasGamepiece(); - }).withTimeout(1); + }, _arm).withTimeout(1); } @@ -56,11 +56,11 @@ private Command intakeCoral() { }, () -> { _arm.setIntakeSpeed(0); - }).until(() -> _arm.hasPiece()); + }, _arm).until(() -> _arm.hasPiece()); } private Command intakeAlgae() { - return new Command() { + Command c = new Command() { double intakeSpikeCounter = 0; @Override @@ -86,6 +86,9 @@ public boolean isFinished() { return intakeSpikeCounter > 15; } }; + c.addRequirements(_arm); + + return c; } public Command moveGamepieceToLightSensor() { diff --git a/src/main/java/frc/robot/subsystems/autos/Autos.java b/src/main/java/frc/robot/subsystems/autos/Autos.java new file mode 100644 index 0000000..4462081 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/autos/Autos.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems.autos; + +import java.io.IOException; + +import org.json.simple.parser.ParseException; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.FileVersionException; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.OverallPosition; + +public class Autos { + MultiSubsystemCommands _msc; + + public Autos(MultiSubsystemCommands msc) { + _msc = msc; + } + + public Command A3_L4() throws FileVersionException, IOException, ParseException { + return AutoBuilder.followPath(PathPlannerPath.fromPathFile("A_R3")) + .andThen(_msc.setOverallSetpoint(OverallPosition.L3)); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index ce5d039..064e96c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -21,6 +21,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; From da249584a06e920c1abbed72f4ab6263a9b0dfe6 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Sat, 15 Mar 2025 15:48:27 -0400 Subject: [PATCH 023/106] Autos should be fixed --- src/main/java/frc/robot/RobotContainer.java | 17 +++-------- .../java/frc/robot/subsystems/arm/Arm.java | 2 ++ .../frc/robot/subsystems/autos/Autos.java | 29 ------------------- .../robot/subsystems/elevator/Elevator.java | 2 ++ 4 files changed, 8 insertions(+), 42 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/autos/Autos.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8228bcf..0073742 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -339,13 +339,10 @@ private void configureBindings() { public Command getAutonomousCommand() { - try { - return autos.A3_L4(); - } catch (FileVersionException | IOException | ParseException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - }//autoChooser.get(); - return null; + // try { + + return autoChooser.get(); + // return null; } public Drive getDrive() { @@ -370,12 +367,6 @@ public void calibrateAndStartPIDs() { if (!CommandScheduler.getInstance().isScheduled(armPIDCommand)) { CommandScheduler.getInstance().schedule(armPIDCommand); } - - // Set initial positions - CommandScheduler.getInstance().schedule(elevatorCommands.setElevatorSetpoint(ElevatorPosition.Stow)); - CommandScheduler.getInstance().schedule(armCommands.setArmPosition(ArmPosition.Stow)); - - CommandScheduler.getInstance().schedule(multiSubsystemCommands.setGamepieceMode(GamepieceMode.CORAL)); } public void startIdleAnimations() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index c4b3333..d39bd58 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -77,6 +77,8 @@ public Arm(ArmIO io) { _armPidController = new ProfiledPIDController(KP, KI, KD, new Constraints(PROFILE_VEL, PROFILE_ACC)); _armPidController.setTolerance(4); + + setArmSetpoint(ArmPosition.Stow); } public void setArmSetpoint(ArmPosition setpoint) { diff --git a/src/main/java/frc/robot/subsystems/autos/Autos.java b/src/main/java/frc/robot/subsystems/autos/Autos.java deleted file mode 100644 index 4462081..0000000 --- a/src/main/java/frc/robot/subsystems/autos/Autos.java +++ /dev/null @@ -1,29 +0,0 @@ -package frc.robot.subsystems.autos; - -import java.io.IOException; - -import org.json.simple.parser.ParseException; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.FileVersionException; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.elevator.Elevator; -import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; -import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.OverallPosition; - -public class Autos { - MultiSubsystemCommands _msc; - - public Autos(MultiSubsystemCommands msc) { - _msc = msc; - } - - public Command A3_L4() throws FileVersionException, IOException, ParseException { - return AutoBuilder.followPath(PathPlannerPath.fromPathFile("A_R3")) - .andThen(_msc.setOverallSetpoint(OverallPosition.L3)); - } -} diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index c777d50..a87c60d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -99,6 +99,8 @@ public Elevator(ElevatorIO io) { _currentMode = GamepieceMode.CORAL; _manualAdjustments = new HashMap<>(); + + setSetpoint(ElevatorPosition.Stow); } // Runs the motors down at the calibration speed From e9b7727350901aea4840517ead2da24cd9553ea9 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Sat, 15 Mar 2025 16:15:12 -0400 Subject: [PATCH 024/106] Fixed build errors --- src/main/java/frc/robot/RobotContainer.java | 5 +++-- src/main/java/frc/robot/subsystems/arm/Arm.java | 2 -- src/main/java/frc/robot/subsystems/elevator/Elevator.java | 2 -- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0073742..ac09feb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,7 +34,6 @@ import frc.robot.subsystems.arm.ArmCommands; import frc.robot.subsystems.arm.Arm.ArmPosition; import frc.robot.subsystems.arm.io.RealArmIO; -import frc.robot.subsystems.autos.Autos; import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.climber.ClimberCommands; import frc.robot.subsystems.climber.io.RealClimberIO; @@ -81,7 +80,6 @@ public class RobotContainer { private final MultiSubsystemCommands multiSubsystemCommands = new MultiSubsystemCommands(elevatorSubsystem, armSubsystem, elevatorCommands, armCommands); - private final Autos autos = new Autos(multiSubsystemCommands); private final Vision vision; private final CommandXboxController joystick = new CommandXboxController(0); @@ -367,6 +365,9 @@ public void calibrateAndStartPIDs() { if (!CommandScheduler.getInstance().isScheduled(armPIDCommand)) { CommandScheduler.getInstance().schedule(armPIDCommand); } + + CommandScheduler.getInstance().schedule(elevatorCommands.setElevatorSetpoint(ElevatorPosition.Stow)); + CommandScheduler.getInstance().schedule(armCommands.setArmPosition(ArmPosition.Stow)); } public void startIdleAnimations() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index d39bd58..c4b3333 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -77,8 +77,6 @@ public Arm(ArmIO io) { _armPidController = new ProfiledPIDController(KP, KI, KD, new Constraints(PROFILE_VEL, PROFILE_ACC)); _armPidController.setTolerance(4); - - setArmSetpoint(ArmPosition.Stow); } public void setArmSetpoint(ArmPosition setpoint) { diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index a87c60d..c777d50 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -99,8 +99,6 @@ public Elevator(ElevatorIO io) { _currentMode = GamepieceMode.CORAL; _manualAdjustments = new HashMap<>(); - - setSetpoint(ElevatorPosition.Stow); } // Runs the motors down at the calibration speed From c5bc0d4c94d61cadc7cbffe89af2faa5c6d34097 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Sat, 15 Mar 2025 16:33:34 -0400 Subject: [PATCH 025/106] More updates --- src/main/deploy/pathplanner/autos/A_R2.auto | 19 ------------ src/main/deploy/pathplanner/autos/A_R3.auto | 2 +- src/main/deploy/pathplanner/autos/A_R4.auto | 19 ------------ src/main/deploy/pathplanner/autos/HP2_R6.auto | 19 ------------ src/main/deploy/pathplanner/autos/LeaveA.auto | 2 +- src/main/deploy/pathplanner/autos/R3_L4.auto | 31 ------------------- src/main/java/frc/robot/RobotContainer.java | 4 ++- .../java/frc/robot/subsystems/arm/Arm.java | 2 +- .../robot/subsystems/elevator/Elevator.java | 4 +-- 9 files changed, 8 insertions(+), 94 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/A_R2.auto delete mode 100644 src/main/deploy/pathplanner/autos/A_R4.auto delete mode 100644 src/main/deploy/pathplanner/autos/HP2_R6.auto delete mode 100644 src/main/deploy/pathplanner/autos/R3_L4.auto diff --git a/src/main/deploy/pathplanner/autos/A_R2.auto b/src/main/deploy/pathplanner/autos/A_R2.auto deleted file mode 100644 index 2cbc044..0000000 --- a/src/main/deploy/pathplanner/autos/A_R2.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "A_R2" - } - } - ] - } - }, - "resetOdom": true, - "folder": "StartToReef", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A_R3.auto b/src/main/deploy/pathplanner/autos/A_R3.auto index 55ca871..055c9e7 100644 --- a/src/main/deploy/pathplanner/autos/A_R3.auto +++ b/src/main/deploy/pathplanner/autos/A_R3.auto @@ -13,7 +13,7 @@ { "type": "named", "data": { - "name": "L2" + "name": "L4" } } ] diff --git a/src/main/deploy/pathplanner/autos/A_R4.auto b/src/main/deploy/pathplanner/autos/A_R4.auto deleted file mode 100644 index 9ff659e..0000000 --- a/src/main/deploy/pathplanner/autos/A_R4.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "A_R4" - } - } - ] - } - }, - "resetOdom": true, - "folder": "StartToReef", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HP2_R6.auto b/src/main/deploy/pathplanner/autos/HP2_R6.auto deleted file mode 100644 index c9b3428..0000000 --- a/src/main/deploy/pathplanner/autos/HP2_R6.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "HP2_R6" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeaveA.auto b/src/main/deploy/pathplanner/autos/LeaveA.auto index d8b7a51..c4245a5 100644 --- a/src/main/deploy/pathplanner/autos/LeaveA.auto +++ b/src/main/deploy/pathplanner/autos/LeaveA.auto @@ -14,6 +14,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "LeaveStart", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/R3_L4.auto b/src/main/deploy/pathplanner/autos/R3_L4.auto deleted file mode 100644 index 7bebc35..0000000 --- a/src/main/deploy/pathplanner/autos/R3_L4.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "A_R3" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "named", - "data": { - "name": "Stow" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ac09feb..0f47a1c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -105,6 +105,7 @@ public class RobotContainer { private final JoystickButton L2Button = new JoystickButton(buttonbox1, 8); private final JoystickButton L3Button = new JoystickButton(buttonbox1,5); private final JoystickButton L4Button = new JoystickButton(buttonbox1, 2); + private final JoystickButton L4ScoreButton = new JoystickButton(buttonbox1, 3); private final JoystickButton intakeButton = new JoystickButton(buttonbox1, 7); private final JoystickButton spitButton = new JoystickButton(buttonbox1, 9); @@ -279,7 +280,8 @@ private void configureBindings() { L1Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L1)); L2Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L2)); L3Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L3)); - L4Button.onTrue(multiSubsystemCommands.goToL4()); + L4Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4)); + L4ScoreButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4_Score)); gamepieceModeToggle.whileTrue(multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE)); gamepieceModeToggle.whileFalse(multiSubsystemCommands.setGamepieceMode(GamepieceMode.CORAL)); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index c4b3333..02a03be 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -76,7 +76,7 @@ public Arm(ArmIO io) { _currentMode = GamepieceMode.CORAL; _armPidController = new ProfiledPIDController(KP, KI, KD, new Constraints(PROFILE_VEL, PROFILE_ACC)); - _armPidController.setTolerance(4); + _armPidController.setTolerance(5); } public void setArmSetpoint(ArmPosition setpoint) { diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index c777d50..bb1bf27 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -91,7 +91,7 @@ public Elevator(ElevatorIO io) { _elevatorMotorPID = new ProfiledPIDController(ELEVATOR_MOTOR_KP, ELEVATOR_MOTOR_KI, ELEVATOR_MOTOR_KD, new Constraints(ELEVATOR_PID_VEL, ELEVATOR_PID_ACC)); - _elevatorMotorPID.setTolerance(0.5); + _elevatorMotorPID.setTolerance(1); _currentPos = ElevatorPosition.Stow; _desiredPos = ElevatorPosition.Stow; @@ -132,7 +132,7 @@ public void setSetpoint(double setpoint) { // Checks if it is at the setpoint public boolean isAtSetpoint() { - boolean atSetpoint = Math.abs(_elevatorMotorPID.getGoal().position - getCurrentPosInches()) <= 0.5; + boolean atSetpoint = Math.abs(_elevatorMotorPID.getGoal().position - getCurrentPosInches()) <= 1; if (atSetpoint) { _currentPos = _desiredPos; _atSetpoint = true; From 42c4a357d541b1429c0a346f225f4e41b5be2313 Mon Sep 17 00:00:00 2001 From: shlap Date: Sat, 15 Mar 2025 18:01:48 -0400 Subject: [PATCH 026/106] Committed Stuff --- src/main/deploy/pathplanner/autos/LeaveD.auto | 19 +++++++ src/main/deploy/pathplanner/paths/A_R1.path | 4 +- src/main/deploy/pathplanner/paths/A_R2.path | 4 +- src/main/deploy/pathplanner/paths/A_R3.path | 10 ++-- src/main/deploy/pathplanner/paths/A_R4.path | 4 +- src/main/deploy/pathplanner/paths/B_R2.path | 4 +- src/main/deploy/pathplanner/paths/B_R3.path | 4 +- src/main/deploy/pathplanner/paths/B_R4.path | 4 +- src/main/deploy/pathplanner/paths/B_R5.path | 4 +- src/main/deploy/pathplanner/paths/C_R3.path | 4 +- src/main/deploy/pathplanner/paths/C_R4.path | 4 +- src/main/deploy/pathplanner/paths/C_R5.path | 4 +- src/main/deploy/pathplanner/paths/C_R6.path | 4 +- src/main/deploy/pathplanner/paths/LeaveA.path | 10 ++-- src/main/deploy/pathplanner/paths/LeaveB.path | 12 ++--- src/main/deploy/pathplanner/paths/LeaveC.path | 10 ++-- src/main/deploy/pathplanner/paths/LeaveD.path | 54 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 7 ++- .../robot/subsystems/elevator/Elevator.java | 4 +- 19 files changed, 124 insertions(+), 46 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/LeaveD.auto create mode 100644 src/main/deploy/pathplanner/paths/LeaveD.path diff --git a/src/main/deploy/pathplanner/autos/LeaveD.auto b/src/main/deploy/pathplanner/autos/LeaveD.auto new file mode 100644 index 0000000..a34b3a7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeaveD.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeaveD" + } + } + ] + } + }, + "resetOdom": true, + "folder": "LeaveStart", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A_R1.path b/src/main/deploy/pathplanner/paths/A_R1.path index b13a00e..5794171 100644 --- a/src/main/deploy/pathplanner/paths/A_R1.path +++ b/src/main/deploy/pathplanner/paths/A_R1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.779246464530422, "y": 7.262 }, "prevControl": null, "nextControl": { - "x": 7.586495221239114, + "x": 7.224741685769536, "y": 6.582720823328432 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/A_R2.path b/src/main/deploy/pathplanner/paths/A_R2.path index 138c08e..08ac6d1 100644 --- a/src/main/deploy/pathplanner/paths/A_R2.path +++ b/src/main/deploy/pathplanner/paths/A_R2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.779246464530422, "y": 7.262 }, "prevControl": null, "nextControl": { - "x": 8.648750608172906, + "x": 8.286997072703329, "y": 7.408445022150453 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index 7460f91..5d0b6df 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.779246464530422, "y": 7.262 }, "prevControl": null, "nextControl": { - "x": 8.467610613658996, - "y": 7.461889236623081 + "x": 7.442242366169765, + "y": 7.027920081967213 }, "isLocked": false, "linkedName": "A" @@ -20,8 +20,8 @@ "y": 5.5414446721311466 }, "prevControl": { - "x": 4.941809299376876, - "y": 5.410545502886385 + "x": 5.502356557377048, + "y": 5.68529713114754 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/A_R4.path b/src/main/deploy/pathplanner/paths/A_R4.path index 37e0d52..6f846c3 100644 --- a/src/main/deploy/pathplanner/paths/A_R4.path +++ b/src/main/deploy/pathplanner/paths/A_R4.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.779246464530422, "y": 7.262 }, "prevControl": null, "nextControl": { - "x": 7.5362335241468985, + "x": 7.17447998867732, "y": 6.3175303768448945 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/B_R2.path b/src/main/deploy/pathplanner/paths/B_R2.path index 1a9d6ff..b99e3f1 100644 --- a/src/main/deploy/pathplanner/paths/B_R2.path +++ b/src/main/deploy/pathplanner/paths/B_R2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.1566598360655735, "y": 6.209 }, "prevControl": null, "nextControl": { - "x": 4.821990291262136, + "x": 3.8376501273277093, "y": 6.1207766990291255 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/B_R3.path b/src/main/deploy/pathplanner/paths/B_R3.path index 0ca5cc2..ab8241d 100644 --- a/src/main/deploy/pathplanner/paths/B_R3.path +++ b/src/main/deploy/pathplanner/paths/B_R3.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.1566598360655735, "y": 6.209 }, "prevControl": null, "nextControl": { - "x": 6.506300290646091, + "x": 5.521960126711664, "y": 5.905521108644577 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/B_R4.path b/src/main/deploy/pathplanner/paths/B_R4.path index 4510ba8..c4a508d 100644 --- a/src/main/deploy/pathplanner/paths/B_R4.path +++ b/src/main/deploy/pathplanner/paths/B_R4.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.1566598360655735, "y": 6.209 }, "prevControl": null, "nextControl": { - "x": 7.1975633821443274, + "x": 6.213223218209901, "y": 5.212845335404368 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/B_R5.path b/src/main/deploy/pathplanner/paths/B_R5.path index 95defe6..e134d07 100644 --- a/src/main/deploy/pathplanner/paths/B_R5.path +++ b/src/main/deploy/pathplanner/paths/B_R5.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.1566598360655735, "y": 6.209 }, "prevControl": null, "nextControl": { - "x": 7.06293676106582, + "x": 6.078596597131393, "y": 4.827990479111984 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C_R3.path b/src/main/deploy/pathplanner/paths/C_R3.path index 0bafd8e..9d3a50c 100644 --- a/src/main/deploy/pathplanner/paths/C_R3.path +++ b/src/main/deploy/pathplanner/paths/C_R3.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.779, "y": 5.058 }, "prevControl": null, "nextControl": { - "x": 7.101052505681702, + "x": 6.739052505681702, "y": 5.235142414101164 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C_R4.path b/src/main/deploy/pathplanner/paths/C_R4.path index 01c8c00..387507c 100644 --- a/src/main/deploy/pathplanner/paths/C_R4.path +++ b/src/main/deploy/pathplanner/paths/C_R4.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.779, "y": 5.058 }, "prevControl": null, "nextControl": { - "x": 7.201422880265144, + "x": 6.839422880265144, "y": 4.594606377694494 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index 0f821b9..e44f415 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.779, "y": 5.058 }, "prevControl": null, "nextControl": { - "x": 8.39747272260748, + "x": 8.03547272260748, "y": 5.149606717229121 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C_R6.path b/src/main/deploy/pathplanner/paths/C_R6.path index 3152363..c3a42d3 100644 --- a/src/main/deploy/pathplanner/paths/C_R6.path +++ b/src/main/deploy/pathplanner/paths/C_R6.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.779, "y": 5.058 }, "prevControl": null, "nextControl": { - "x": 7.395457580845862, + "x": 7.0334575808458615, "y": 4.270077123187671 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LeaveA.path b/src/main/deploy/pathplanner/paths/LeaveA.path index 99efd9f..2c15e54 100644 --- a/src/main/deploy/pathplanner/paths/LeaveA.path +++ b/src/main/deploy/pathplanner/paths/LeaveA.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.779246464530422, "y": 7.262 }, "prevControl": null, "nextControl": { - "x": 8.390999846987176, - "y": 7.261723402117175 + "x": 7.520884554371662, + "y": 7.254098699313813 }, "isLocked": false, "linkedName": "A" @@ -20,8 +20,8 @@ "y": 7.262 }, "prevControl": { - "x": 7.786891826231032, - "y": 7.2620629067231155 + "x": 7.749604617834939, + "y": 7.276054705812421 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LeaveB.path b/src/main/deploy/pathplanner/paths/LeaveB.path index eed1340..bd95189 100644 --- a/src/main/deploy/pathplanner/paths/LeaveB.path +++ b/src/main/deploy/pathplanner/paths/LeaveB.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.1566598360655735, "y": 6.209 }, "prevControl": null, "nextControl": { - "x": 8.390999846987176, - "y": 6.208723402117175 + "x": 6.906773561742195, + "y": 6.201460112415871 }, "isLocked": false, "linkedName": "B" }, { "anchor": { - "x": 7.5, + "x": 6.58125, "y": 6.209 }, "prevControl": { - "x": 7.786891826231032, - "y": 6.209062906723116 + "x": 6.831049987149319, + "y": 6.199001679130972 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LeaveC.path b/src/main/deploy/pathplanner/paths/LeaveC.path index 17b95ca..e89d596 100644 --- a/src/main/deploy/pathplanner/paths/LeaveC.path +++ b/src/main/deploy/pathplanner/paths/LeaveC.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.141, + "x": 7.779, "y": 5.058 }, "prevControl": null, "nextControl": { - "x": 8.390999846987176, - "y": 5.057723402117175 + "x": 7.530706640433861, + "y": 5.0288382511608125 }, "isLocked": false, "linkedName": "C" @@ -20,8 +20,8 @@ "y": 5.058 }, "prevControl": { - "x": 7.786891826231032, - "y": 5.058062906723116 + "x": 7.7497678481494265, + "y": 5.0687713523201605 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LeaveD.path b/src/main/deploy/pathplanner/paths/LeaveD.path new file mode 100644 index 0000000..ceb46ab --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeaveD.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.2165983606557385, + "y": 4.019006147540983 + }, + "prevControl": null, + "nextControl": { + "x": 6.856967213114754, + "y": 4.030993852459016 + }, + "isLocked": false, + "linkedName": "Mid" + }, + { + "anchor": { + "x": 6.809016393442622, + "y": 4.019006147540983 + }, + "prevControl": { + "x": 7.057487327111027, + "y": 4.046614029059696 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Leave Start", + "idealStartingState": { + "velocity": 0, + "rotation": 178.51213247117218 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f47a1c..70a2c2a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; @@ -211,7 +212,8 @@ public RobotContainer() { // AutoAlignToReef + Move to L4 + Score NamedCommands.registerCommand("L4", - multiSubsystemCommands.scoreGamepieceAtPosition(OverallPosition.L4)); + //multiSubsystemCommands.scoreGamepieceAtPosition(OverallPosition.L4)); + elevatorCommands.setElevatorSetpoint(ElevatorPosition.L3)); // AutoAlign to Intake + Intake NamedCommands.registerCommand("Intake", @@ -238,6 +240,9 @@ public RobotContainer() { multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE) .andThen(multiSubsystemCommands.loadAlgae())); + NamedCommands.registerCommand("Stow", + new PrintCommand("AAA")); + autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); // hide the joystick missing warnings diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index bb1bf27..feadfcf 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -117,8 +117,8 @@ public void setElevatorVoltage(Voltage voltage) { public void setSetpoint(ElevatorPosition setpoint) { _prevPos = _currentPos; _desiredPos = setpoint; - String key = getManualAdjustKey(); - setSetpoint(setpoint.getHeight(_currentMode) + _manualAdjustments.getOrDefault(key, 0)); + // String key = getManualAdjustKey(); + // setSetpoint(setpoint.getHeight(_currentMode)); } // Sets the setpoint of the PID From 01be0a59ea5371261695876765a86ecf0b1f712e Mon Sep 17 00:00:00 2001 From: area5188 Date: Sat, 15 Mar 2025 18:05:45 -0400 Subject: [PATCH 027/106] removed extra robot container constructor --- src/main/java/frc/robot/Robot.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fff82f2..ac52791 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -104,9 +104,6 @@ public Robot() { (Command command) -> { logCommandFunction.accept(command, false); }); - - m_robotContainer = new RobotContainer(); - } @Override From 05116ae94d59ca23a73e3682bbbec08ffc51fc41 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Sat, 15 Mar 2025 18:37:50 -0400 Subject: [PATCH 028/106] Fixed merge error --- src/main/java/frc/robot/subsystems/elevator/Elevator.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index feadfcf..bb1bf27 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -117,8 +117,8 @@ public void setElevatorVoltage(Voltage voltage) { public void setSetpoint(ElevatorPosition setpoint) { _prevPos = _currentPos; _desiredPos = setpoint; - // String key = getManualAdjustKey(); - // setSetpoint(setpoint.getHeight(_currentMode)); + String key = getManualAdjustKey(); + setSetpoint(setpoint.getHeight(_currentMode) + _manualAdjustments.getOrDefault(key, 0)); } // Sets the setpoint of the PID From 739adfc0b2488ca552d4c88125a84997c79b2113 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Tue, 18 Mar 2025 18:09:03 -0400 Subject: [PATCH 029/106] added stash --- .../frc/robot/subsystems/arm/ArmCommands.java | 4 ++++ .../subsystems/elevator/ElevatorCommands.java | 19 ++++++++++++------- .../MultiSubsystemCommands.java | 7 ++----- 3 files changed, 18 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 0372dce..27ff1f8 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -145,4 +145,8 @@ public Command intakeForNumberOfRotations() { public Command waitUntilAtSetpoint() { return new WaitUntilCommand(_arm::isAtSetpoint); } + + public Command moveArm(ArmPosition pos) { + return setArmPosition(pos).andThen(waitUntilAtSetpoint()); + } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java index c687ab8..e079160 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java @@ -16,32 +16,37 @@ public ElevatorCommands(Elevator elevator) { public Command runElevatorPID() { return Commands.run(() -> { _elevator.runMotorsWithPID(); - } ); + }); } public Command decrementElevatorPosition() { return new InstantCommand( () -> { _elevator.decrementElevatorPosition(); - }); + + } , _elevator); } - - public Command incrementElevatorPosition(){ + + public Command incrementElevatorPosition() { return new InstantCommand( () -> { _elevator.incrementElevatorPosition(); - }); + + }, _elevator); } public Command setElevatorSetpoint(ElevatorPosition setpoint) { return new InstantCommand( () -> { _elevator.setSetpoint(setpoint); - }); + }, _elevator); } public Command waitUntilAtSetpoint() { return new WaitUntilCommand(_elevator::isAtSetpoint); } -} + public Command moveElevator(ElevatorPosition pos) { + return setElevatorSetpoint(pos).andThen(waitUntilAtSetpoint()); + } +} diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index b953b26..9552201 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -59,11 +59,8 @@ public MultiSubsystemCommands(Elevator elevator, Arm arm, ElevatorCommands eleva _armCommands = armCommands; } - public Command setOverallSetpoint(OverallPosition setpoint) { - return _elevatorCommands.setElevatorSetpoint(setpoint.getElevatorPosition()) - .alongWith(_armCommands.setArmPosition(setpoint.getArmPosition())) - .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), setpoint.getElevatorPosition(), - _arm.getCurrentPos(), setpoint.getArmPosition())); + public Command moveToPosition(OverallPosition setpoint) { + return _armCommands.moveArm(setpoint.) } public Command setGamepieceMode(GamepieceMode mode) { From 0d30a1dbc9c59404c1b4001d9799449fa89d6850 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Tue, 18 Mar 2025 18:44:18 -0400 Subject: [PATCH 030/106] Transient pos should be implemented, need to test --- src/main/java/frc/robot/RobotContainer.java | 35 ++-- .../java/frc/robot/subsystems/arm/Arm.java | 3 +- .../MultiSubsystemCommands.java | 159 ++---------------- 3 files changed, 28 insertions(+), 169 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cdca876..8b75f9d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -217,31 +217,18 @@ public RobotContainer() { // AutoAlign to Intake + Intake NamedCommands.registerCommand("Intake", - multiSubsystemCommands.loadGamepiece()); + multiSubsystemCommands.loadCoral()); // AutoAlign + Algae Removal NamedCommands.registerCommand("AlgaeL2", - multiSubsystemCommands.setOverallSetpoint(OverallPosition.L2) - .andThen(multiSubsystemCommands.waitForOverallMechanism()) - .andThen(multiSubsystemCommands.loadAlgae())); + multiSubsystemCommands.loadAlgae(OverallPosition.L2)); // AutoAlign + Algae Removal NamedCommands.registerCommand("AlgaeL3", - multiSubsystemCommands.setOverallSetpoint(OverallPosition.L3) - .andThen(multiSubsystemCommands.waitForOverallMechanism()) - .andThen(multiSubsystemCommands.loadAlgae())); - - - NamedCommands.registerCommand("AlgaeL2", - multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE) - .andThen(multiSubsystemCommands.loadAlgae())); - - NamedCommands.registerCommand("AlgaeL3", - multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE) - .andThen(multiSubsystemCommands.loadAlgae())); + multiSubsystemCommands.loadAlgae(OverallPosition.L3)); NamedCommands.registerCommand("Stow", - multiSubsystemCommands.setOverallSetpoint(OverallPosition.Stow)); + multiSubsystemCommands.moveToPosition(OverallPosition.Stow)); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -278,15 +265,15 @@ private void configureBindings() { // drive.registerTelemetry(logger::telemeterize); - intakeButton.onTrue(multiSubsystemCommands.loadGamepiece());//.raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving())); + intakeButton.onTrue(multiSubsystemCommands.loadCoral());//.raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving())); spitButton.onTrue(armCommands.spit()); - StowButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.Stow)); - L1Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L1)); - L2Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L2)); - L3Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L3)); - L4Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4)); - L4ScoreButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4_Score)); + StowButton.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.Stow)); + L1Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L1)); + L2Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L2)); + L3Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L3)); + L4Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L4)); + L4ScoreButton.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L4_Score)); gamepieceModeToggle.whileTrue(multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE)); gamepieceModeToggle.whileFalse(multiSubsystemCommands.setGamepieceMode(GamepieceMode.CORAL)); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 02a03be..7b28d65 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -28,7 +28,8 @@ public enum ArmPosition { Stow(100, 100), Loading(128, 55), L4_Score(65, 45), - Algae_Score(60, 60); + Algae_Score(60, 60), + Transient(90, 90); double coralAngle, algaeAngle; diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 9552201..49c8061 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -17,7 +17,8 @@ public class MultiSubsystemCommands { public enum OverallPosition { Stow(ElevatorPosition.Stow, ArmPosition.Stow), Coral_Loading(ElevatorPosition.Stow, ArmPosition.Loading), - Algae_Loading(ElevatorPosition.L2, ArmPosition.Loading), + Algae_Loading_L2(ElevatorPosition.L2, ArmPosition.Loading), + Algae_Loading_L3(ElevatorPosition.L3, ArmPosition.Loading), L1(ElevatorPosition.L1, ArmPosition.Stow), L2(ElevatorPosition.L2, ArmPosition.Stow), L3(ElevatorPosition.L3, ArmPosition.Stow), @@ -60,7 +61,9 @@ public MultiSubsystemCommands(Elevator elevator, Arm arm, ElevatorCommands eleva } public Command moveToPosition(OverallPosition setpoint) { - return _armCommands.moveArm(setpoint.) + return _armCommands.moveArm(ArmPosition.Transient) + .andThen(_elevatorCommands.moveElevator(setpoint.getElevatorPosition())) + .andThen(_armCommands.moveArm(setpoint.getArmPosition())); } public Command setGamepieceMode(GamepieceMode mode) { @@ -71,159 +74,27 @@ public Command setGamepieceMode(GamepieceMode mode) { }, _elevator, _arm); } - public Command waitForOverallMechanism() { - return _elevatorCommands.waitUntilAtSetpoint() - .alongWith(_armCommands.waitUntilAtSetpoint()); - } - - private Command score(OverallPosition setpoint) { - if (setpoint == OverallPosition.L4) { - return setOverallSetpoint(OverallPosition.L4_Score) - .andThen(waitForOverallMechanism()) - .andThen(_armCommands.spit()) - .andThen(setOverallSetpoint(OverallPosition.L4)) - .andThen(waitForOverallMechanism()); - } else { - return _armCommands.spit(); - } - } - - public Command scoreGamepieceAtPosition(Supplier setpoint) { - return scoreGamepieceAtPosition(setpoint.get()); - } - public Command scoreGamepieceAtPosition(OverallPosition setpoint) { - if (setpoint == OverallPosition.Stow || setpoint == OverallPosition.Coral_Loading - || setpoint == OverallPosition.L4_Score) { - throw new RuntimeException("scoreGamepieceAtPosition cannot run to stow,loading,or L4_score"); - } - return setOverallSetpoint(setpoint) - .andThen(waitForOverallMechanism()) - .andThen(score(setpoint)) - .andThen(setOverallSetpoint(OverallPosition.Stow)); - } - - public Command loadGamepiece() { - return Commands.either(loadAlgae(), loadCoral(), () -> _arm.getCurrentMode() == GamepieceMode.ALGAE); + return moveToPosition(setpoint) + .andThen(_armCommands.spit()); } public Command loadCoral() { - return setOverallSetpoint(OverallPosition.Coral_Loading) - .andThen(waitForOverallMechanism()) - .andThen(_armCommands.intake()) + return moveToPosition(OverallPosition.Coral_Loading) + .alongWith(_armCommands.intake()) .andThen(new WaitCommand(0.25)) - .andThen(setOverallSetpoint(OverallPosition.Stow)) .andThen(_armCommands.moveGamepieceToLightSensor()) .andThen(new WaitCommand(0.25)) .andThen(_armCommands.moveGamepieceToLightSensor().unless(() -> _arm.lightSensorSeesGamepiece())) - .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), ElevatorPosition.Stow, - _arm.getCurrentPos(), ArmPosition.Loading)); - - } - - public Command loadAlgae() { - return _armCommands.setArmPosition(ArmPosition.Loading) - .andThen(waitForOverallMechanism()) - .andThen(_armCommands.intake()) - .andThen(_armCommands.setArmPosition(ArmPosition.Stow)) - .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), ElevatorPosition.L2, - _arm.getCurrentPos(), ArmPosition.Loading)); - } - - public Command goToL4() { - return setOverallSetpoint(OverallPosition.L4) - .andThen(waitForOverallMechanism()) - .andThen(setOverallSetpoint(OverallPosition.L4_Score)); + .andThen(moveToPosition(OverallPosition.Stow)); } - public boolean canMoveToPos(ElevatorPosition currentElevator, ElevatorPosition desiredElevator, - ArmPosition currentArm, ArmPosition desiredArm) { - boolean canMoveArm = false; - boolean canMoveElevator = false; - - if (_arm.getCurrentMode() == GamepieceMode.CORAL) { - switch (currentElevator) { - case L1: - case L2: - case L3: - canMoveArm = (desiredArm != ArmPosition.L4_Score) && (desiredArm != ArmPosition.Loading); - break; - case L4: - canMoveArm = (desiredArm != ArmPosition.Loading); - break; - case Stow: - canMoveArm = (desiredArm != ArmPosition.L4_Score); - break; - default: - canMoveArm = false; - break; - } - - switch (desiredElevator) { - case L1: - case L2: - case L3: - canMoveElevator = (currentArm != ArmPosition.L4_Score) && (currentArm != ArmPosition.Loading); - break; - case L4: - canMoveElevator = (currentArm != ArmPosition.Loading); - break; - case Stow: - canMoveElevator = (currentArm != ArmPosition.L4_Score); - break; - case Manual: - canMoveElevator = currentArm == ArmPosition.Stow; - break; - default: - canMoveElevator = false; - break; - } - } else { - switch (currentElevator) { - case L1: - case L4: - canMoveArm = false; - break; - case L2: - case L3: - canMoveArm = desiredArm != ArmPosition.Algae_Score; - break; - case Stow: - canMoveArm = desiredArm != ArmPosition.Loading; - break; - default: - canMoveArm = false; - break; - } - - switch (desiredElevator) { - case L1: - case L4: - canMoveElevator = false; - break; - case L2: - case L3: - canMoveElevator = currentArm != ArmPosition.Algae_Score; - break; - case Stow: - canMoveElevator = currentArm != ArmPosition.Loading; - break; - case Manual: - canMoveElevator = currentArm == ArmPosition.Stow; - break; - default: - canMoveElevator = false; - break; - } - - if (currentArm == ArmPosition.Loading) { - canMoveElevator = desiredElevator == _elevator.getPrevPos(); - canMoveArm = canMoveElevator; - } + public Command loadAlgae(OverallPosition position) { + if (position != OverallPosition.Algae_Loading_L2 || position != OverallPosition.Algae_Loading_L3) { + throw new IllegalArgumentException("Can Only Load Algae @ L2 or L3"); } - System.out.println("Arm: " + canMoveArm + " Elevator: " + canMoveElevator); - - return canMoveArm && canMoveElevator && _arm.isAtSetpoint() && _elevator.isAtSetpoint(); + return moveToPosition(position) + .alongWith(_armCommands.intake()); } } \ No newline at end of file From ab01d7ca343d904215127041014575d6f91630df Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Tue, 18 Mar 2025 19:14:14 -0400 Subject: [PATCH 031/106] Added PID resets --- src/main/java/frc/robot/Robot.java | 120 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 9 +- .../java/frc/robot/subsystems/arm/Arm.java | 5 + .../frc/robot/subsystems/arm/ArmCommands.java | 4 + .../robot/subsystems/elevator/Elevator.java | 6 + .../subsystems/elevator/ElevatorCommands.java | 4 + .../MultiSubsystemCommands.java | 2 +- 7 files changed, 84 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ac52791..082e75c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -38,7 +38,6 @@ import java.io.IOException; import org.json.simple.parser.ParseException; - public class Robot extends LoggedRobot { private Command m_autonomousCommand; private Command m_lastAutonomousCommand; @@ -46,8 +45,7 @@ public class Robot extends LoggedRobot { private Field2d m_autoTraj = new Field2d(); public static final double fieldLength = Units.inchesToMeters(690.876); public static final double fieldWidth = Units.inchesToMeters(317); - public static final Translation2d fieldCenter = - new Translation2d(fieldLength / 2, fieldWidth / 2); + public static final Translation2d fieldCenter = new Translation2d(fieldLength / 2, fieldWidth / 2); private final RobotContainer m_robotContainer = new RobotContainer(); @@ -69,7 +67,8 @@ public Robot() { setUseTiming(false); // Run as fast as possible String logPath = LogFileUtil.findReplayLog(); Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log - Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new + // log break; } @@ -80,15 +79,14 @@ public Robot() { // Log active commands Map commandCounts = new HashMap<>(); - BiConsumer logCommandFunction = - (Command command, Boolean active) -> { - String name = command.getName(); - int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); - commandCounts.put(name, count); - Logger.recordOutput( - "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); - Logger.recordOutput("CommandsAll/" + name, count > 0); - }; + BiConsumer logCommandFunction = (Command command, Boolean active) -> { + String name = command.getName(); + int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); + commandCounts.put(name, count); + Logger.recordOutput( + "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); + Logger.recordOutput("CommandsAll/" + name, count > 0); + }; CommandScheduler.getInstance() .onCommandInitialize( (Command command) -> { @@ -119,53 +117,52 @@ public void disabledInit() { @Override public void disabledPeriodic() { - var m_alliance = DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - // Get currently selected command - - // m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - // // Check if is the same as the last one - // if (m_autonomousCommand != m_lastAutonomousCommand && m_autonomousCommand != null) { - // // Check if its contained in the list of our autos - // if (AutoBuilder.getAllAutoNames().contains(m_autonomousCommand.getName())) { - // // Clear the current path - // m_pathsToShow.clear(); - // // Grabs all paths from the auto - // try { - // for (PathPlannerPath path : PathPlannerAuto - // .getPathGroupFromAutoFile(m_autonomousCommand.getName())) { - // // Adds all poses to master list - // m_pathsToShow.addAll(path.getPathPoses()); - // } - // } catch (IOException | ParseException e) { - // // TODO Auto-generated catch block - // e.printStackTrace(); - // } - // // Check to see which alliance we are on Red Alliance - // if (m_alliance) { - // for (int i = 0; i < m_pathsToShow.size(); i++) { - // m_pathsToShow.set(i, - // m_pathsToShow.get(i).rotateAround(fieldCenter, Rotation2d.k180deg)); - // } - // } - // // Displays all poses on Field2d widget - // m_autoTraj.getObject("traj").setPoses(m_pathsToShow); - // } - // } - // m_lastAutonomousCommand = m_autonomousCommand; - - // if (!m_pathsToShow.isEmpty()) { - // var firstPose = m_pathsToShow.get(0); - // Logger.recordOutput("Alignment/StartPose", firstPose); - // SmartDashboard.putBoolean("Alignment/Translation", - // firstPose.getTranslation().getDistance( - // m_robotContainer.getDrive().getPose().getTranslation()) <= Units - // .inchesToMeters(1.5)); - // SmartDashboard.putBoolean("Alignment/Rotation", - // firstPose.getRotation().minus(m_robotContainer.getDrive().getPose().getRotation()) - // .getDegrees() < 1); - // } - // } + var m_alliance = DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + // Get currently selected command + + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + // Check if is the same as the last one + if (m_autonomousCommand != m_lastAutonomousCommand && m_autonomousCommand != null) { + // Check if its contained in the list of our autos + if (AutoBuilder.getAllAutoNames().contains(m_autonomousCommand.getName())) { + // Clear the current path + m_pathsToShow.clear(); + // Grabs all paths from the auto + try { + for (PathPlannerPath path : PathPlannerAuto + .getPathGroupFromAutoFile(m_autonomousCommand.getName())) { + // Adds all poses to master list + m_pathsToShow.addAll(path.getPathPoses()); + } + } catch (IOException | ParseException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + // Check to see which alliance we are on Red Alliance + if (m_alliance) { + for (int i = 0; i < m_pathsToShow.size(); i++) { + m_pathsToShow.set(i, + m_pathsToShow.get(i).rotateAround(fieldCenter, Rotation2d.k180deg)); + } + } + // Displays all poses on Field2d widget + m_autoTraj.getObject("traj").setPoses(m_pathsToShow); + } + } + m_lastAutonomousCommand = m_autonomousCommand; + + if (!m_pathsToShow.isEmpty()) { + var firstPose = m_pathsToShow.get(0); + Logger.recordOutput("Alignment/StartPose", firstPose); + SmartDashboard.putBoolean("Alignment/Translation", + firstPose.getTranslation().getDistance( + m_robotContainer.getDrive().getPose().getTranslation()) <= Units + .inchesToMeters(1.5)); + SmartDashboard.putBoolean("Alignment/Rotation", + firstPose.getRotation().minus(m_robotContainer.getDrive().getPose().getRotation()) + .getDegrees() < 1); + } } @Override @@ -186,7 +183,8 @@ public void autonomousInit() { } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override public void autonomousExit() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b75f9d..527b7b4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -221,11 +221,11 @@ public RobotContainer() { // AutoAlign + Algae Removal NamedCommands.registerCommand("AlgaeL2", - multiSubsystemCommands.loadAlgae(OverallPosition.L2)); + multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L2)); // AutoAlign + Algae Removal NamedCommands.registerCommand("AlgaeL3", - multiSubsystemCommands.loadAlgae(OverallPosition.L3)); + multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L3)); NamedCommands.registerCommand("Stow", multiSubsystemCommands.moveToPosition(OverallPosition.Stow)); @@ -360,8 +360,9 @@ public void calibrateAndStartPIDs() { CommandScheduler.getInstance().schedule(armPIDCommand); } - CommandScheduler.getInstance().schedule(elevatorCommands.setElevatorSetpoint(ElevatorPosition.Stow)); - CommandScheduler.getInstance().schedule(armCommands.setArmPosition(ArmPosition.Stow)); + // Reset PIDs + CommandScheduler.getInstance().schedule(elevatorCommands.resetElevatorPID()); + CommandScheduler.getInstance().schedule(armCommands.resetArmPID()); } public void startIdleAnimations() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 7b28d65..36e8970 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -78,6 +78,7 @@ public Arm(ArmIO io) { _armPidController = new ProfiledPIDController(KP, KI, KD, new Constraints(PROFILE_VEL, PROFILE_ACC)); _armPidController.setTolerance(5); + setArmSetpoint(ArmPosition.Stow); } public void setArmSetpoint(ArmPosition setpoint) { @@ -160,6 +161,10 @@ public ArmPosition getCurrentPos() { return _currentPos; } + public void resetPID(){ + _armPidController.reset(_inputs._armEncoderPositionDegrees); + } + public void runArmPID() { double out = _armPidController.calculate(_inputs._armEncoderPositionDegrees) + (ARM_FEEDFORWARD_COEFF * Math.cos(Units.degreesToRadians(_inputs._armEncoderPositionDegrees))); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 27ff1f8..dae8a4a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -149,4 +149,8 @@ public Command waitUntilAtSetpoint() { public Command moveArm(ArmPosition pos) { return setArmPosition(pos).andThen(waitUntilAtSetpoint()); } + + public Command resetArmPID(){ + return Commands.runOnce(() -> _arm.resetPID()); + } } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index bb1bf27..ddfd71b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -99,6 +99,8 @@ public Elevator(ElevatorIO io) { _currentMode = GamepieceMode.CORAL; _manualAdjustments = new HashMap<>(); + + setSetpoint(ElevatorPosition.Stow); } // Runs the motors down at the calibration speed @@ -140,6 +142,10 @@ public boolean isAtSetpoint() { return atSetpoint; } + public void resetPID() { + _elevatorMotorPID.reset(getCurrentPosInches()); + } + private String getManualAdjustKey() { return _desiredPos.toString() + _currentMode.toString(); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java index e079160..0a7c24c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java @@ -49,4 +49,8 @@ public Command waitUntilAtSetpoint() { public Command moveElevator(ElevatorPosition pos) { return setElevatorSetpoint(pos).andThen(waitUntilAtSetpoint()); } + + public Command resetElevatorPID() { + return Commands.runOnce(() -> _elevator.resetPID()); + } } diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 49c8061..eb06516 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -90,7 +90,7 @@ public Command loadCoral() { } public Command loadAlgae(OverallPosition position) { - if (position != OverallPosition.Algae_Loading_L2 || position != OverallPosition.Algae_Loading_L3) { + if (position != OverallPosition.Algae_Loading_L2 && position != OverallPosition.Algae_Loading_L3) { throw new IllegalArgumentException("Can Only Load Algae @ L2 or L3"); } return moveToPosition(position) From 200c96068ae4b5cd9b62342b39f12f46d255c1cf Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Tue, 18 Mar 2025 19:51:11 -0400 Subject: [PATCH 032/106] Added new light sensors --- .../java/frc/robot/HardwareConstants.java | 4 +- .../java/frc/robot/subsystems/arm/Arm.java | 16 ++++-- .../frc/robot/subsystems/arm/ArmCommands.java | 57 +++++++++++-------- .../frc/robot/subsystems/arm/io/ArmIO.java | 4 +- .../robot/subsystems/arm/io/RealArmIO.java | 32 ++++++++--- .../MultiSubsystemCommands.java | 2 +- 6 files changed, 74 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/HardwareConstants.java b/src/main/java/frc/robot/HardwareConstants.java index 4b86c7c..648d9bc 100644 --- a/src/main/java/frc/robot/HardwareConstants.java +++ b/src/main/java/frc/robot/HardwareConstants.java @@ -31,7 +31,9 @@ public class CAN { public static final int CANDLE_ID = 50; - public static final int CORAL_LASERCAN_ID = 20; + public static final int LOWER_CORAL_LASERCAN_ID = 20; + public static final int UPPER_CORAL_LASERCAN_ID = 21; + public static final int ALGAE_LASERCAN_ID = 22; } public class DIO { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 36e8970..c2bc090 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -133,19 +133,25 @@ public boolean intakeAtDesiredRotations() { public boolean hasPiece() { boolean hasPiece = false; if (_currentMode == GamepieceMode.CORAL) { - boolean currentState = _inputs._lightSensorState; - // hasPiece = _prevLightSensorVal && !currentState; + boolean currentState = _inputs._upperLightSensorState; + hasPiece = _prevLightSensorVal && !currentState; hasPiece = currentState; _prevLightSensorVal = currentState; - } + } else { + hasPiece = _inputs._algaeLightSensorState; + } if (hasPiece) _hasGamepiece = true; return _hasGamepiece; } - public boolean lightSensorSeesGamepiece() { - return _inputs._lightSensorState; + public boolean upperLightSensorSeesGamepiece() { + return _inputs._upperLightSensorState; + } + + public boolean lowerLightSensorSeesGamepiece() { + return _inputs._lowerLightSensorState; } public boolean isAtSetpoint() { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index dae8a4a..78a9513 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -52,48 +52,55 @@ public Command intake() { } private Command intakeCoral() { - return new StartEndCommand( - () -> { + Command c = new Command() { + + @Override + public void execute() { + if (_arm.lowerLightSensorSeesGamepiece()) { + _arm.setIntakeSpeed(0.08); + } else { _arm.setIntakeSpeed(0.35); - }, - () -> { - _arm.setIntakeSpeed(0); - }, _arm).until(() -> _arm.hasPiece()); + } + } + + @Override + public void end(boolean interrupted) { + _arm.setIntakeSpeed(0); + } + + @Override + public boolean isFinished() { + return _arm.hasPiece(); + } + }; + c.addRequirements(_arm); + return c; } private Command intakeAlgae() { Command c = new Command() { - double intakeSpikeCounter = 0; - int counter = 0; @Override public void initialize() { - intakeSpikeCounter = 0; - counter = 0; - _arm.setIntakeSpeed(0.5); - } - - @Override - public void execute() { - counter++; - if (_arm.getIntakeCurrent() >= Arm.HAS_ALGAE_CURRENT && counter > 12) { - intakeSpikeCounter++; - } - Logger.recordOutput("Arm/intakeSpikeCount", intakeSpikeCounter); + this.addRequirements(_arm); + _arm.setIntakeSpeed(0.6); } @Override public void end(boolean interrupted) { - _arm.setIntakeSpeed(0.08); + if (interrupted) { + _arm.setIntakeSpeed(0); + } else { + _arm.setIntakeSpeed(0.05); + } } @Override public boolean isFinished() { - return intakeSpikeCounter > 3; + return _arm.hasPiece(); } }; c.addRequirements(_arm); - return c; } @@ -105,7 +112,7 @@ public Command moveGamepieceToLightSensor() { public void initialize() { // If we see the gamepiece, we want to move further down in the intake // If we don't, it's too far down and needs to go back up - movingDown = _arm.lightSensorSeesGamepiece(); + movingDown = _arm.upperLightSensorSeesGamepiece(); } @Override @@ -121,7 +128,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return (movingDown) ? !_arm.lightSensorSeesGamepiece() : _arm.lightSensorSeesGamepiece(); + return (movingDown) ? !_arm.upperLightSensorSeesGamepiece() : _arm.upperLightSensorSeesGamepiece(); } }; } diff --git a/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java index f4c50f7..f21d2d4 100644 --- a/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java @@ -19,7 +19,9 @@ public static class ArmIOInputs { public double _intakeMotorCurrent = 0.0; public double _intakeMotorVoltage = 0.0; - public boolean _lightSensorState = false; + public boolean _upperLightSensorState = false; + public boolean _lowerLightSensorState = false; + public boolean _algaeLightSensorState = false; } public default void updateInputs(ArmIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java index 5484a2f..197b884 100644 --- a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java @@ -23,12 +23,15 @@ public class RealArmIO implements ArmIO { private static final double POS_AT_90 = 0.711; private static final double POS_AT_0 = 0.458; private static final double ENCODER_CONVERSION = (POS_AT_90 - POS_AT_0) / 90.0; - private static final double LASERCAN_DISTANCE_MM = 50; + private static final double CORAL_LASERCAN_DISTANCE_MM = 50; + private static final double ALGAE_LASERCAN_DISTANCE_MM = 50; private double INTAKE_ROTATION_CONVERSION = 1; private SparkFlex _armMotor; - private LaserCan _laserCan; + private LaserCan _upperLaserCan; + private LaserCan _lowerLaserCan; + private LaserCan _algaeLaserCan; private SparkFlex _intakeMotor; private SparkAbsoluteEncoder _armEncoder; @@ -47,12 +50,18 @@ public RealArmIO() { intakeConfig.idleMode(IdleMode.kBrake); _intakeMotor.configure(intakeConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - _laserCan = new LaserCan(CAN.CORAL_LASERCAN_ID); + _upperLaserCan = new LaserCan(CAN.UPPER_CORAL_LASERCAN_ID); + _lowerLaserCan = new LaserCan(CAN.LOWER_CORAL_LASERCAN_ID); + _algaeLaserCan = new LaserCan(CAN.ALGAE_LASERCAN_ID); // Optionally initialise the settings of the LaserCAN, if you haven't already // done so in GrappleHook try { - _laserCan.setRangingMode(LaserCan.RangingMode.SHORT); - _laserCan.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + _upperLaserCan.setRangingMode(LaserCan.RangingMode.SHORT); + _upperLaserCan.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + _lowerLaserCan.setRangingMode(LaserCan.RangingMode.SHORT); + _lowerLaserCan.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + _algaeLaserCan.setRangingMode(LaserCan.RangingMode.SHORT); + _algaeLaserCan.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); } catch (ConfigurationFailedException e) { System.out.println("Configuration failed! " + e); } @@ -63,9 +72,16 @@ public void updateInputs(ArmIOInputs inputs) { inputs._armMotorCurrent = _armMotor.getOutputCurrent(); inputs._armMotorVoltage = _armMotor.getAppliedOutput() * _armMotor.getBusVoltage(); - LaserCan.Measurement measurement = _laserCan.getMeasurement(); - if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) - inputs._lightSensorState = measurement.distance_mm <= LASERCAN_DISTANCE_MM; + LaserCan.Measurement upperMeasurement = _upperLaserCan.getMeasurement(); + if (upperMeasurement != null && upperMeasurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) + inputs._upperLightSensorState = upperMeasurement.distance_mm <= CORAL_LASERCAN_DISTANCE_MM; + LaserCan.Measurement lowerMeasurement = _upperLaserCan.getMeasurement(); + if (lowerMeasurement != null && lowerMeasurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) + inputs._lowerLightSensorState = lowerMeasurement.distance_mm <= CORAL_LASERCAN_DISTANCE_MM; + LaserCan.Measurement algaeMeasurement = _upperLaserCan.getMeasurement(); + if (algaeMeasurement != null && algaeMeasurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) + inputs._algaeLightSensorState = algaeMeasurement.distance_mm <= ALGAE_LASERCAN_DISTANCE_MM; + inputs._intakeMotorVelocityRotationsPerMin = _intakeMotor.get(); inputs._intakeMotorCurrent = _intakeMotor.getOutputCurrent(); inputs._intakeMotorVoltage = _intakeMotor.getAppliedOutput() * _armMotor.getBusVoltage(); diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index eb06516..16d5657 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -85,7 +85,7 @@ public Command loadCoral() { .andThen(new WaitCommand(0.25)) .andThen(_armCommands.moveGamepieceToLightSensor()) .andThen(new WaitCommand(0.25)) - .andThen(_armCommands.moveGamepieceToLightSensor().unless(() -> _arm.lightSensorSeesGamepiece())) + .andThen(_armCommands.moveGamepieceToLightSensor().unless(() -> _arm.upperLightSensorSeesGamepiece())) .andThen(moveToPosition(OverallPosition.Stow)); } From e98e6a0b0a3752568dc1d9f0f76bdf626957a9ef Mon Sep 17 00:00:00 2001 From: area5188 Date: Wed, 19 Mar 2025 16:44:32 -0400 Subject: [PATCH 033/106] Arm pid tuned better --- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 8 +-- .../java/frc/robot/subsystems/arm/Arm.java | 14 ++--- .../robot/subsystems/arm/io/RealArmIO.java | 6 +- .../MultiSubsystemCommands.java | 4 +- .../frc/robot/subsystems/presets/Preset.java | 61 ------------------- 6 files changed, 16 insertions(+), 79 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/presets/Preset.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 082e75c..fe8ee01 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -22,6 +22,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.PathPlannerPath; +import au.grapplerobotics.CanBridge; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -50,6 +51,7 @@ public class Robot extends LoggedRobot { private final RobotContainer m_robotContainer = new RobotContainer(); public Robot() { + CanBridge.runTCP(); switch (HardwareConstants.currentMode) { case REAL: diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 527b7b4..2a76d8b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -57,7 +57,6 @@ import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.OverallPosition; -import frc.robot.subsystems.presets.Preset; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhotonVision; @@ -85,8 +84,6 @@ public class RobotContainer { private final Vision vision; private final CommandXboxController joystick = new CommandXboxController(0); - private final Preset preset = new Preset(); - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private double MaxAngularRate = RotationsPerSecond.of(0.5).in(RadiansPerSecond); // 1/2 of a rotation per second max // angular velocity @@ -270,10 +267,9 @@ private void configureBindings() { StowButton.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.Stow)); L1Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L1)); - L2Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L2)); - L3Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L3)); + L2Button.onTrue(Commands.either(multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L2), multiSubsystemCommands.moveToPosition(OverallPosition.L2), () -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); + L3Button.onTrue(Commands.either(multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L3), multiSubsystemCommands.moveToPosition(OverallPosition.L3), () -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); L4Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L4)); - L4ScoreButton.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L4_Score)); gamepieceModeToggle.whileTrue(multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE)); gamepieceModeToggle.whileFalse(multiSubsystemCommands.setGamepieceMode(GamepieceMode.CORAL)); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index c2bc090..996240e 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -25,11 +25,11 @@ public class Arm extends SubsystemBase { public enum ArmPosition { - Stow(100, 100), - Loading(128, 55), - L4_Score(65, 45), - Algae_Score(60, 60), - Transient(90, 90); + Stow(114, 114), + Loading(140, 78), + L4_Score(86, 86), + Algae_Score(100, 100), + Transient(100, 100); double coralAngle, algaeAngle; @@ -57,8 +57,8 @@ public enum ArmPosition { private ProfiledPIDController _armPidController; - private static final double KP = 0.06;//0.09; - private static final double KI = 0; //0.01; + private static final double KP = 0.08;//0.09; + private static final double KI = 0.002; //0.01; private static final double KD = 0.01; private static final double PROFILE_VEL = 160; private static final double PROFILE_ACC = 145; diff --git a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java index 197b884..c24c743 100644 --- a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java @@ -20,8 +20,8 @@ public class RealArmIO implements ArmIO { - private static final double POS_AT_90 = 0.711; - private static final double POS_AT_0 = 0.458; + private static final double POS_AT_90 = 0.422; + private static final double POS_AT_0 = 0.168; private static final double ENCODER_CONVERSION = (POS_AT_90 - POS_AT_0) / 90.0; private static final double CORAL_LASERCAN_DISTANCE_MM = 50; private static final double ALGAE_LASERCAN_DISTANCE_MM = 50; @@ -96,7 +96,7 @@ public void setArmMotorSpeed(double speed) { } public void setIntakeMotorSpeed(double speed) { - _intakeMotor.set(speed); + // _intakeMotor.set(speed); } public void resetIntakeEncoders() { diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 16d5657..fef9fd3 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -22,8 +22,7 @@ public enum OverallPosition { L1(ElevatorPosition.L1, ArmPosition.Stow), L2(ElevatorPosition.L2, ArmPosition.Stow), L3(ElevatorPosition.L3, ArmPosition.Stow), - L4(ElevatorPosition.L4, ArmPosition.Stow), - L4_Score(ElevatorPosition.L4, ArmPosition.L4_Score); + L4(ElevatorPosition.L4, ArmPosition.L4_Score); ElevatorPosition _elevatorSetpoint; ArmPosition _armSetpoint; @@ -63,6 +62,7 @@ public MultiSubsystemCommands(Elevator elevator, Arm arm, ElevatorCommands eleva public Command moveToPosition(OverallPosition setpoint) { return _armCommands.moveArm(ArmPosition.Transient) .andThen(_elevatorCommands.moveElevator(setpoint.getElevatorPosition())) + .unless(() -> _elevator.getCurrentPos() == setpoint.getElevatorPosition()) .andThen(_armCommands.moveArm(setpoint.getArmPosition())); } diff --git a/src/main/java/frc/robot/subsystems/presets/Preset.java b/src/main/java/frc/robot/subsystems/presets/Preset.java deleted file mode 100644 index 65c8fda..0000000 --- a/src/main/java/frc/robot/subsystems/presets/Preset.java +++ /dev/null @@ -1,61 +0,0 @@ -package frc.robot.subsystems.presets; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.OverallPosition; - -public class Preset { - public enum ReefSide { - Left, - Right; - } - - private OverallPosition _level = OverallPosition.L1; - private ReefSide _side = ReefSide.Right; - private boolean _isLevelValid = false; - private boolean _isSideValid = false; - - private void setPresetLevel(OverallPosition level) { - if (level == OverallPosition.Stow || level == OverallPosition.Coral_Loading || level == OverallPosition.L4_Score) { - throw new RuntimeException("Invalid Preset Level/Position"); - } - - _level = level; - _isLevelValid = true; - } - - private void setPresetSide(ReefSide side) { - _side = side; - _isSideValid = true; - } - - public OverallPosition getLevel() { - return _level; - } - - public ReefSide getSide() { - return _side; - } - - public boolean isPresetValid() { - return _isLevelValid && _isSideValid; - } - - public Command setPresetLevelCommand(OverallPosition level) { - return new InstantCommand( - () -> setPresetLevel(level)); - } - - public Command setPresetSideCommand(ReefSide side) { - return new InstantCommand( - () -> setPresetSide(side)); - } - - public Command resetPreset() { - return new InstantCommand( - () -> { - _isLevelValid = false; - _isSideValid = false; - }); - } -} \ No newline at end of file From 5a8d428d33cedbfbf3311b451e272657a00baadf Mon Sep 17 00:00:00 2001 From: area5188 Date: Wed, 19 Mar 2025 21:20:05 -0400 Subject: [PATCH 034/106] debugged/slightly tuned coral intake --- src/main/java/frc/robot/subsystems/arm/Arm.java | 7 ++++--- src/main/java/frc/robot/subsystems/arm/ArmCommands.java | 6 +++--- src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java | 6 +++--- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 996240e..e1efe43 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -132,10 +132,11 @@ public boolean intakeAtDesiredRotations() { public boolean hasPiece() { boolean hasPiece = false; + // boolean _hasGamepiece = false; if (_currentMode == GamepieceMode.CORAL) { - boolean currentState = _inputs._upperLightSensorState; - hasPiece = _prevLightSensorVal && !currentState; - hasPiece = currentState; + boolean currentState = _inputs._upperLightSensorState; + hasPiece = (_prevLightSensorVal && !currentState) && _inputs._lowerLightSensorState; + //hasPiece = currentState; _prevLightSensorVal = currentState; } else { hasPiece = _inputs._algaeLightSensorState; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 78a9513..6474ba6 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -57,9 +57,9 @@ private Command intakeCoral() { @Override public void execute() { if (_arm.lowerLightSensorSeesGamepiece()) { - _arm.setIntakeSpeed(0.08); + _arm.setIntakeSpeed(0.08); } else { - _arm.setIntakeSpeed(0.35); + _arm.setIntakeSpeed(0.25); // 0.35 } } @@ -117,7 +117,7 @@ public void initialize() { @Override public void execute() { - double speed = (movingDown) ? 0.07 : -0.07; + double speed = (movingDown) ? 0.1 : -0.1; _arm.setIntakeSpeed(speed); } diff --git a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java index c24c743..da8bb8f 100644 --- a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java @@ -75,10 +75,10 @@ public void updateInputs(ArmIOInputs inputs) { LaserCan.Measurement upperMeasurement = _upperLaserCan.getMeasurement(); if (upperMeasurement != null && upperMeasurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) inputs._upperLightSensorState = upperMeasurement.distance_mm <= CORAL_LASERCAN_DISTANCE_MM; - LaserCan.Measurement lowerMeasurement = _upperLaserCan.getMeasurement(); + LaserCan.Measurement lowerMeasurement = _lowerLaserCan.getMeasurement(); if (lowerMeasurement != null && lowerMeasurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) inputs._lowerLightSensorState = lowerMeasurement.distance_mm <= CORAL_LASERCAN_DISTANCE_MM; - LaserCan.Measurement algaeMeasurement = _upperLaserCan.getMeasurement(); + LaserCan.Measurement algaeMeasurement = _algaeLaserCan.getMeasurement(); if (algaeMeasurement != null && algaeMeasurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) inputs._algaeLightSensorState = algaeMeasurement.distance_mm <= ALGAE_LASERCAN_DISTANCE_MM; @@ -96,7 +96,7 @@ public void setArmMotorSpeed(double speed) { } public void setIntakeMotorSpeed(double speed) { - // _intakeMotor.set(speed); + _intakeMotor.set(speed); } public void resetIntakeEncoders() { From ff4fcba378cd33e5da4b3979865cbd2197ff5768 Mon Sep 17 00:00:00 2001 From: area5188 Date: Thu, 20 Mar 2025 12:38:37 -0400 Subject: [PATCH 035/106] Tuned algae intaking --- src/main/java/frc/robot/subsystems/arm/Arm.java | 1 + src/main/java/frc/robot/subsystems/arm/ArmCommands.java | 4 ++-- src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java | 2 +- src/main/java/frc/robot/subsystems/elevator/Elevator.java | 1 + .../multisubsystemcommands/MultiSubsystemCommands.java | 4 ++-- 5 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index e1efe43..bf38df5 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -169,6 +169,7 @@ public ArmPosition getCurrentPos() { } public void resetPID(){ + _armPidController.setGoal(ArmPosition.Stow.getAngle(_currentMode)); _armPidController.reset(_inputs._armEncoderPositionDegrees); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 6474ba6..662f17d 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -83,7 +83,7 @@ private Command intakeAlgae() { @Override public void initialize() { this.addRequirements(_arm); - _arm.setIntakeSpeed(0.6); + _arm.setIntakeSpeed(0.5); } @Override @@ -117,7 +117,7 @@ public void initialize() { @Override public void execute() { - double speed = (movingDown) ? 0.1 : -0.1; + double speed = (movingDown) ? 0.08 : -0.08; _arm.setIntakeSpeed(speed); } diff --git a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java index da8bb8f..3269d4a 100644 --- a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java @@ -24,7 +24,7 @@ public class RealArmIO implements ArmIO { private static final double POS_AT_0 = 0.168; private static final double ENCODER_CONVERSION = (POS_AT_90 - POS_AT_0) / 90.0; private static final double CORAL_LASERCAN_DISTANCE_MM = 50; - private static final double ALGAE_LASERCAN_DISTANCE_MM = 50; + private static final double ALGAE_LASERCAN_DISTANCE_MM = 30; private double INTAKE_ROTATION_CONVERSION = 1; diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index ddfd71b..94eee96 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -143,6 +143,7 @@ public boolean isAtSetpoint() { } public void resetPID() { + _elevatorMotorPID.setGoal(ElevatorPosition.Stow.getHeight(_currentMode)); _elevatorMotorPID.reset(getCurrentPosInches()); } diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index fef9fd3..8371681 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -82,9 +82,9 @@ public Command scoreGamepieceAtPosition(OverallPosition setpoint) { public Command loadCoral() { return moveToPosition(OverallPosition.Coral_Loading) .alongWith(_armCommands.intake()) - .andThen(new WaitCommand(0.25)) + .andThen(new WaitCommand(0.1)) .andThen(_armCommands.moveGamepieceToLightSensor()) - .andThen(new WaitCommand(0.25)) + .andThen(new WaitCommand(0.1)) .andThen(_armCommands.moveGamepieceToLightSensor().unless(() -> _arm.upperLightSensorSeesGamepiece())) .andThen(moveToPosition(OverallPosition.Stow)); } From d25edc31aae27c96d15a63b13e0bb22221045a98 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Thu, 20 Mar 2025 12:50:50 -0400 Subject: [PATCH 036/106] Maybe made intaking run again if we lost the coral --- .../frc/robot/subsystems/arm/ArmCommands.java | 18 +++++++++++++++++- .../MultiSubsystemCommands.java | 13 +++++-------- 2 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 662f17d..47a1283 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -3,12 +3,14 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.arm.Arm.ArmPosition; import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class ArmCommands { @@ -48,7 +50,15 @@ public Command setArmPosition(ArmPosition setpoint) { } public Command intake() { - return Commands.either(intakeAlgae(), intakeCoral(), () -> _arm.getCurrentMode() == GamepieceMode.ALGAE); + return Commands.either(intakeAlgae(), intakeCoralWithAdjust(), () -> _arm.getCurrentMode() == GamepieceMode.ALGAE); + } + + private Command intakeCoralWithAdjust() { + return intakeCoral() + .andThen(new WaitCommand(0.1)) + .andThen(moveGamepieceToLightSensor()) + .andThen(new WaitCommand(0.1)) + .andThen(moveGamepieceToLightSensor().unless(() -> _arm.upperLightSensorSeesGamepiece())); } private Command intakeCoral() { @@ -110,6 +120,12 @@ public Command moveGamepieceToLightSensor() { @Override public void initialize() { + // if we don't see the gamepiece in the middle, we assume that we don't have it + // Cancel this command (and everything that follows it) by rescheduling intaking + if (!_arm.lowerLightSensorSeesGamepiece()) { + CommandScheduler.getInstance().schedule(intakeCoralWithAdjust()); + } + // If we see the gamepiece, we want to move further down in the intake // If we don't, it's too far down and needs to go back up movingDown = _arm.upperLightSensorSeesGamepiece(); diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 8371681..71fbf07 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -61,9 +61,9 @@ public MultiSubsystemCommands(Elevator elevator, Arm arm, ElevatorCommands eleva public Command moveToPosition(OverallPosition setpoint) { return _armCommands.moveArm(ArmPosition.Transient) - .andThen(_elevatorCommands.moveElevator(setpoint.getElevatorPosition())) - .unless(() -> _elevator.getCurrentPos() == setpoint.getElevatorPosition()) - .andThen(_armCommands.moveArm(setpoint.getArmPosition())); + .andThen(_elevatorCommands.moveElevator(setpoint.getElevatorPosition())) + .unless(() -> _elevator.getCurrentPos() == setpoint.getElevatorPosition()) + .andThen(_armCommands.moveArm(setpoint.getArmPosition())); } public Command setGamepieceMode(GamepieceMode mode) { @@ -81,12 +81,9 @@ public Command scoreGamepieceAtPosition(OverallPosition setpoint) { public Command loadCoral() { return moveToPosition(OverallPosition.Coral_Loading) - .alongWith(_armCommands.intake()) - .andThen(new WaitCommand(0.1)) - .andThen(_armCommands.moveGamepieceToLightSensor()) - .andThen(new WaitCommand(0.1)) - .andThen(_armCommands.moveGamepieceToLightSensor().unless(() -> _arm.upperLightSensorSeesGamepiece())) + .andThen(_armCommands.intake()) .andThen(moveToPosition(OverallPosition.Stow)); + } public Command loadAlgae(OverallPosition position) { From a579fdcb245bb2f97fee01295558297b810848f7 Mon Sep 17 00:00:00 2001 From: area5188 Date: Thu, 20 Mar 2025 18:50:23 -0400 Subject: [PATCH 037/106] Update camera constants for two cameras --- src/main/java/frc/robot/RobotContainer.java | 1 + .../subsystems/vision/VisionConstants.java | 31 +++++++++---------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2a76d8b..b4950d0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -140,6 +140,7 @@ public RobotContainer() { vision = new Vision( drive::addVisionMeasurement, + new VisionIOPhotonVision(camera5Name, robotToCamera5), new VisionIOPhotonVision(camera6Name, robotToCamera6) ); break; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index a04a9b4..b6e48c7 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -42,27 +42,26 @@ public class VisionConstants { // Note: 0.0254 multiplier converts inches to meters // Camera 5 + // Facing forward, not tilted public static Transform3d robotToCamera5 = - new Transform3d(Units.inchesToMeters(-13.75), Units.inchesToMeters(-11.25), Units.inchesToMeters(9.25), - new Rotation3d(0.0, -Math.PI/4, Math.PI)); - - // Camera 6 - public static Transform3d robotToCamera6 = new Transform3d(Units.inchesToMeters(13.75), Units.inchesToMeters(-11.5), Units.inchesToMeters(9.25), new Rotation3d(0.0, 0, 0)); + + // Camera 6 (Back left swerve module) + // Tilted up 10 degrees, facing backwards + public static Transform3d robotToCamera6 = + new Transform3d(Units.inchesToMeters(-13.75), Units.inchesToMeters(11.5), Units.inchesToMeters(9.25), + new Rotation3d(0.0, -Units.degreesToRadians(10), Math.PI)); +// // Camera 7 +// public static Transform3d robotToCamera7 = +// new Transform3d(Units.inchesToMeters(14.125), Units.inchesToMeters(-8.5), Units.inchesToMeters(7.75), +// new Rotation3d(0.0, 0, 0)); - // Camera 7 - // Front right - public static Transform3d robotToCamera7 = - new Transform3d(Units.inchesToMeters(14.125), Units.inchesToMeters(-8.5), Units.inchesToMeters(7.75), - new Rotation3d(0.0, 0, 0)); - - // Front Left - // Camera 8 - public static Transform3d robotToCamera8 = - new Transform3d(Units.inchesToMeters(14.125), Units.inchesToMeters(11.5), Units.inchesToMeters(8.25), - new Rotation3d(0, Units.degreesToRadians(-10), 0)); +// // Camera 8 +// public static Transform3d robotToCamera8 = +// new Transform3d(Units.inchesToMeters(14.125), Units.inchesToMeters(11.5), Units.inchesToMeters(8.25), +// new Rotation3d(0, Units.degreesToRadians(-10), 0)); // Basic filtering thresholds public static double maxAmbiguity = 0.3; From 60d43543ce537db8c2d7d23f7b867ba8b392ca9e Mon Sep 17 00:00:00 2001 From: area5188 Date: Fri, 21 Mar 2025 19:05:49 -0400 Subject: [PATCH 038/106] driver practice 3/21/2025 (cameras, transient angle, algae light sensor) --- src/main/java/frc/robot/subsystems/arm/Arm.java | 6 +++--- src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java | 5 ++++- .../java/frc/robot/subsystems/vision/VisionConstants.java | 6 +++--- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index bf38df5..8c2b673 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -29,7 +29,7 @@ public enum ArmPosition { Loading(140, 78), L4_Score(86, 86), Algae_Score(100, 100), - Transient(100, 100); + Transient(108, 108); double coralAngle, algaeAngle; @@ -57,8 +57,8 @@ public enum ArmPosition { private ProfiledPIDController _armPidController; - private static final double KP = 0.08;//0.09; - private static final double KI = 0.002; //0.01; + private static final double KP = 0.06;//0.09; + private static final double KI = 0.00; //0.01; private static final double KD = 0.01; private static final double PROFILE_VEL = 160; private static final double PROFILE_ACC = 145; diff --git a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java index 3269d4a..1004eed 100644 --- a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java @@ -79,8 +79,11 @@ public void updateInputs(ArmIOInputs inputs) { if (lowerMeasurement != null && lowerMeasurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) inputs._lowerLightSensorState = lowerMeasurement.distance_mm <= CORAL_LASERCAN_DISTANCE_MM; LaserCan.Measurement algaeMeasurement = _algaeLaserCan.getMeasurement(); - if (algaeMeasurement != null && algaeMeasurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) + if (algaeMeasurement != null && algaeMeasurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { inputs._algaeLightSensorState = algaeMeasurement.distance_mm <= ALGAE_LASERCAN_DISTANCE_MM; + } else { + inputs._algaeLightSensorState = false; + } inputs._intakeMotorVelocityRotationsPerMin = _intakeMotor.get(); inputs._intakeMotorCurrent = _intakeMotor.getOutputCurrent(); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index b6e48c7..d413222 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -44,14 +44,14 @@ public class VisionConstants { // Camera 5 // Facing forward, not tilted public static Transform3d robotToCamera5 = - new Transform3d(Units.inchesToMeters(13.75), Units.inchesToMeters(-11.5), Units.inchesToMeters(9.25), + new Transform3d(Units.inchesToMeters(12.75), Units.inchesToMeters(-11.5), Units.inchesToMeters(8.5), new Rotation3d(0.0, 0, 0)); // Camera 6 (Back left swerve module) // Tilted up 10 degrees, facing backwards public static Transform3d robotToCamera6 = - new Transform3d(Units.inchesToMeters(-13.75), Units.inchesToMeters(11.5), Units.inchesToMeters(9.25), - new Rotation3d(0.0, -Units.degreesToRadians(10), Math.PI)); + new Transform3d(Units.inchesToMeters(-13), Units.inchesToMeters(12.5), Units.inchesToMeters(8.75), + new Rotation3d(Units.degreesToRadians(0.0), 0.0, Math.PI)); // // Camera 7 // public static Transform3d robotToCamera7 = From ec9fc7a88303c0b87b8041f294a7fc9e9f88ec5a Mon Sep 17 00:00:00 2001 From: area5188 Date: Sat, 22 Mar 2025 13:43:07 -0400 Subject: [PATCH 039/106] cybertooth practice 3/22/2025 --- src/main/java/frc/robot/RobotContainer.java | 14 +++---- .../java/frc/robot/subsystems/arm/Arm.java | 2 +- .../frc/robot/subsystems/arm/ArmCommands.java | 40 ++++++++++--------- .../robot/subsystems/arm/io/RealArmIO.java | 6 ++- .../robot/subsystems/drive/DriveCommands.java | 2 +- .../robot/subsystems/elevator/Elevator.java | 9 ++--- .../MultiSubsystemCommands.java | 3 +- .../subsystems/vision/VisionConstants.java | 8 ++-- 8 files changed, 45 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b4950d0..1782a66 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.ArmCommands; @@ -103,13 +104,14 @@ public class RobotContainer { private final JoystickButton L2Button = new JoystickButton(buttonbox1, 8); private final JoystickButton L3Button = new JoystickButton(buttonbox1,5); private final JoystickButton L4Button = new JoystickButton(buttonbox1, 2); - private final JoystickButton L4ScoreButton = new JoystickButton(buttonbox1, 3); private final JoystickButton intakeButton = new JoystickButton(buttonbox1, 7); private final JoystickButton spitButton = new JoystickButton(buttonbox1, 9); private final JoystickButton gamepieceModeToggle = new JoystickButton(buttonbox1, 10); + private final JoystickButton manualIntakeButton = new JoystickButton(buttonbox2, 1); + private final JoystickButton incrementElevatorButton = new JoystickButton(buttonbox2, 4); private final JoystickButton decrementElevatorButton = new JoystickButton(buttonbox2, 7); @@ -263,7 +265,7 @@ private void configureBindings() { // drive.registerTelemetry(logger::telemeterize); - intakeButton.onTrue(multiSubsystemCommands.loadCoral());//.raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving())); + intakeButton.onTrue(multiSubsystemCommands.loadCoral().unless(() -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE));//.raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving())); spitButton.onTrue(armCommands.spit()); StowButton.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.Stow)); @@ -301,6 +303,8 @@ private void configureBindings() { joystickApproach( () -> FieldConstants.getNearestReefFace(drive.getPose()))); + manualIntakeButton.whileTrue(armCommands.manualIntake()); + incrementElevatorButton.onTrue(elevatorCommands.incrementElevatorPosition()); decrementElevatorButton.onTrue(elevatorCommands.decrementElevatorPosition()); /* @@ -323,15 +327,11 @@ private void configureBindings() { // // reset the field-centric heading on left bumper press // joystick.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); - + joystick.leftTrigger(0.75).onTrue(armCommands.spit()); } - public Command getAutonomousCommand() { - // try { - return autoChooser.get(); - // return null; } public Drive getDrive() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 8c2b673..dc7d77a 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -27,7 +27,7 @@ public class Arm extends SubsystemBase { public enum ArmPosition { Stow(114, 114), Loading(140, 78), - L4_Score(86, 86), + L4_Score(102, 102), // 86 Algae_Score(100, 100), Transient(108, 108); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 47a1283..889085a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -23,14 +23,14 @@ public ArmCommands(Arm arm) { public Command spit() { return new StartEndCommand( () -> { - if (_arm.getCurrentPos() == ArmPosition.L4_Score) - _arm.setArmSetpoint(ArmPosition.Stow); _arm.spit(); }, () -> { _arm.setIntakeSpeed(0); _arm.clearHasGamepiece(); - }, _arm).withTimeout(1); + }, _arm).withTimeout(1) + .andThen(Commands.runOnce(() -> _arm.setArmSetpoint(ArmPosition.Stow), _arm).unless(() -> _arm.getCurrentPos() != ArmPosition.L4_Score)); + } @@ -39,26 +39,23 @@ public Command setArmPosition(ArmPosition setpoint) { return new InstantCommand( () -> { _arm.setArmSetpoint(setpoint); - } - ).andThen(intakeForNumberOfRotations()); + }).andThen(intakeForNumberOfRotations()); } return new InstantCommand( () -> { _arm.setArmSetpoint(setpoint); - } - ); + }); } public Command intake() { - return Commands.either(intakeAlgae(), intakeCoralWithAdjust(), () -> _arm.getCurrentMode() == GamepieceMode.ALGAE); + return Commands.either(intakeAlgae(), intakeCoralWithAdjust(), + () -> _arm.getCurrentMode() == GamepieceMode.ALGAE); } private Command intakeCoralWithAdjust() { return intakeCoral() - .andThen(new WaitCommand(0.1)) - .andThen(moveGamepieceToLightSensor()) - .andThen(new WaitCommand(0.1)) - .andThen(moveGamepieceToLightSensor().unless(() -> _arm.upperLightSensorSeesGamepiece())); + .andThen(new WaitCommand(0.25)) + .andThen(moveGamepieceToLightSensor()); } private Command intakeCoral() { @@ -67,7 +64,7 @@ private Command intakeCoral() { @Override public void execute() { if (_arm.lowerLightSensorSeesGamepiece()) { - _arm.setIntakeSpeed(0.08); + _arm.setIntakeSpeed(0.08); } else { _arm.setIntakeSpeed(0.25); // 0.35 } @@ -116,7 +113,6 @@ public boolean isFinished() { public Command moveGamepieceToLightSensor() { return new Command() { - boolean movingDown = true; @Override public void initialize() { @@ -128,12 +124,11 @@ public void initialize() { // If we see the gamepiece, we want to move further down in the intake // If we don't, it's too far down and needs to go back up - movingDown = _arm.upperLightSensorSeesGamepiece(); } @Override public void execute() { - double speed = (movingDown) ? 0.08 : -0.08; + double speed = -0.08; _arm.setIntakeSpeed(speed); } @@ -144,7 +139,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return (movingDown) ? !_arm.upperLightSensorSeesGamepiece() : _arm.upperLightSensorSeesGamepiece(); + return _arm.upperLightSensorSeesGamepiece(); } }; } @@ -172,8 +167,15 @@ public Command waitUntilAtSetpoint() { public Command moveArm(ArmPosition pos) { return setArmPosition(pos).andThen(waitUntilAtSetpoint()); } - - public Command resetArmPID(){ + + public Command resetArmPID() { return Commands.runOnce(() -> _arm.resetPID()); } + + public Command manualIntake() { + return Commands.either(Commands.runOnce( + () -> _arm.setIntakeSpeed(0.1), _arm), + Commands.runOnce(() -> _arm.setIntakeSpeed(0.5), _arm), + () -> _arm.getCurrentMode() == GamepieceMode.CORAL); + } } diff --git a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java index 1004eed..ea77a7d 100644 --- a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java @@ -9,6 +9,7 @@ import au.grapplerobotics.ConfigurationFailedException; import au.grapplerobotics.LaserCan; +import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; @@ -24,7 +25,7 @@ public class RealArmIO implements ArmIO { private static final double POS_AT_0 = 0.168; private static final double ENCODER_CONVERSION = (POS_AT_90 - POS_AT_0) / 90.0; private static final double CORAL_LASERCAN_DISTANCE_MM = 50; - private static final double ALGAE_LASERCAN_DISTANCE_MM = 30; + private static final double ALGAE_LASERCAN_DISTANCE_MM = 20; private double INTAKE_ROTATION_CONVERSION = 1; @@ -58,10 +59,13 @@ public RealArmIO() { try { _upperLaserCan.setRangingMode(LaserCan.RangingMode.SHORT); _upperLaserCan.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + _upperLaserCan.setRegionOfInterest(new RegionOfInterest(8, 8, 8, 8)); _lowerLaserCan.setRangingMode(LaserCan.RangingMode.SHORT); _lowerLaserCan.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + _lowerLaserCan.setRegionOfInterest(new RegionOfInterest(8, 8, 8, 8)); _algaeLaserCan.setRangingMode(LaserCan.RangingMode.SHORT); _algaeLaserCan.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + _algaeLaserCan.setRegionOfInterest(new RegionOfInterest(8, 8, 8, 8)); } catch (ConfigurationFailedException e) { System.out.println("Configuration failed! " + e); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java index 098cd7a..e672165 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java @@ -42,7 +42,7 @@ public class DriveCommands { private static final double DEADBAND = 0.1; private static final double ANGLE_KP = 5.0; - private static final double ANGLE_KD = 0.4; + private static final double ANGLE_KD = 0.0; // 0.4 private static final double ANGLE_MAX_VELOCITY = 8.0; private static final double ANGLE_MAX_ACCELERATION = 20.0; private static final double FF_START_DELAY = 2.0; // Secs diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 94eee96..1567a0c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -35,11 +35,10 @@ public class Elevator extends SubsystemBase { public enum ElevatorPosition { L1(5, 5), - L2(11, 20), - L3(27, 38), - L4(52, 52), - Stow(0.5, 0.5), - Manual(0, 0); + L2(11, 18), + L3(27, 36), + L4(50.75, 50.75), + Stow(0.5, 0.5); double coralHeight, algaeHeight; diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 71fbf07..5365e4d 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -91,7 +91,8 @@ public Command loadAlgae(OverallPosition position) { throw new IllegalArgumentException("Can Only Load Algae @ L2 or L3"); } return moveToPosition(position) - .alongWith(_armCommands.intake()); + .alongWith(_armCommands.intake()) + .andThen(_armCommands.setArmPosition(ArmPosition.Stow)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index d413222..b662b17 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -44,14 +44,14 @@ public class VisionConstants { // Camera 5 // Facing forward, not tilted public static Transform3d robotToCamera5 = - new Transform3d(Units.inchesToMeters(12.75), Units.inchesToMeters(-11.5), Units.inchesToMeters(8.5), + new Transform3d(Units.inchesToMeters(13.25), Units.inchesToMeters(-11.5), Units.inchesToMeters(8.5), new Rotation3d(0.0, 0, 0)); // Camera 6 (Back left swerve module) // Tilted up 10 degrees, facing backwards public static Transform3d robotToCamera6 = - new Transform3d(Units.inchesToMeters(-13), Units.inchesToMeters(12.5), Units.inchesToMeters(8.75), - new Rotation3d(Units.degreesToRadians(0.0), 0.0, Math.PI)); + new Transform3d(Units.inchesToMeters(-12.75), Units.inchesToMeters(12.5), Units.inchesToMeters(8.75), + new Rotation3d(0.0, Units.degreesToRadians(+20.0), Units.degreesToRadians(135))); // // Camera 7 // public static Transform3d robotToCamera7 = @@ -77,7 +77,7 @@ public class VisionConstants { public static double[] cameraStdDevFactors = new double[] { 1.0, // Camera 5 - 1.0, // Camera 6 + 2.0, // Camera 6 1.0, // Camera 7 1.0 // Camera 8 }; From 9aa21065ce361e10cff0b3ee6efb9987e03119a5 Mon Sep 17 00:00:00 2001 From: area5188 Date: Sat, 22 Mar 2025 15:23:22 -0400 Subject: [PATCH 040/106] adjusted intake speeds for new arm pulley ratio --- src/main/java/frc/robot/subsystems/arm/Arm.java | 2 +- .../frc/robot/subsystems/arm/ArmCommands.java | 16 ++-------------- 2 files changed, 3 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index dc7d77a..75a5f57 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -127,7 +127,7 @@ public void resetIntakeEncoders() { } public boolean intakeAtDesiredRotations() { - return _inputs._intakeMotorPositionRotations <= -1.5; + return _inputs._intakeMotorPositionRotations <= -1; } public boolean hasPiece() { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 889085a..f1dcd35 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -114,21 +114,9 @@ public boolean isFinished() { public Command moveGamepieceToLightSensor() { return new Command() { - @Override - public void initialize() { - // if we don't see the gamepiece in the middle, we assume that we don't have it - // Cancel this command (and everything that follows it) by rescheduling intaking - if (!_arm.lowerLightSensorSeesGamepiece()) { - CommandScheduler.getInstance().schedule(intakeCoralWithAdjust()); - } - - // If we see the gamepiece, we want to move further down in the intake - // If we don't, it's too far down and needs to go back up - } - @Override public void execute() { - double speed = -0.08; + double speed = -0.1; // -0.08 _arm.setIntakeSpeed(speed); } @@ -153,7 +141,7 @@ public Command runArmPID() { public Command intakeForNumberOfRotations() { return new StartEndCommand(() -> { _arm.resetIntakeEncoders(); - _arm.setIntakeSpeed(-0.1); + _arm.setIntakeSpeed(-0.13); // -0.1 }, () -> { _arm.setIntakeSpeed(0); From 5fa64351c7df4a5d73752f81caf7b2b77fb38777 Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 22 Mar 2025 15:50:29 -0400 Subject: [PATCH 041/106] cybertooth practice --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/subsystems/arm/ArmCommands.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fe8ee01..744bc74 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -114,7 +114,7 @@ public void robotPeriodic() { @Override public void disabledInit() { m_robotContainer.startIdleAnimations(); - // SmartDashboard.putData("Auto Trajectory", m_autoTraj); + SmartDashboard.putData("Auto Trajectory", m_autoTraj); } @Override diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index f1dcd35..70494b6 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -141,7 +141,7 @@ public Command runArmPID() { public Command intakeForNumberOfRotations() { return new StartEndCommand(() -> { _arm.resetIntakeEncoders(); - _arm.setIntakeSpeed(-0.13); // -0.1 + _arm.setIntakeSpeed(-0.15); // -0.1 }, () -> { _arm.setIntakeSpeed(0); From 3cc79e4efd93f6da7cc4d37ed945bfe2c54f3b67 Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 22 Mar 2025 15:58:27 -0400 Subject: [PATCH 042/106] cpark's autos and paths from Friday night --- .../pathplanner/autos/B-ScoreL4ScoreL3.auto | 49 +++++++++++++++++++ src/main/deploy/pathplanner/paths/B_R5.path | 16 +++--- src/main/deploy/pathplanner/paths/C_R5.path | 20 ++++---- src/main/deploy/pathplanner/paths/HP1_R5.path | 8 +-- src/main/deploy/pathplanner/paths/HP2_R5.path | 8 +-- src/main/deploy/pathplanner/paths/R5_HP1.path | 8 +-- src/main/deploy/pathplanner/paths/R5_HP2.path | 8 +-- 7 files changed, 83 insertions(+), 34 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto diff --git a/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto b/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto new file mode 100644 index 0000000..9e1d959 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "C_R5" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "R5_HP2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "HP2_R6" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B_R5.path b/src/main/deploy/pathplanner/paths/B_R5.path index e134d07..32c687f 100644 --- a/src/main/deploy/pathplanner/paths/B_R5.path +++ b/src/main/deploy/pathplanner/paths/B_R5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.1566598360655735, - "y": 6.209 + "x": 7.561692813425667, + "y": 4.2 }, "prevControl": null, "nextControl": { - "x": 6.078596597131393, - "y": 4.827990479111984 + "x": 6.483629574491487, + "y": 2.818990479111984 }, "isLocked": false, "linkedName": "B" }, { "anchor": { - "x": 5.301, - "y": 2.622 + "x": 5.343, + "y": 2.80625 }, "prevControl": { - "x": 6.406601941747573, - "y": 3.0367475728155338 + "x": 6.448601941747572, + "y": 3.220997572815534 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index e44f415..6b1e954 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.779, - "y": 5.058 + "x": 7.588217213114754, + "y": 1.8732069672131149 }, "prevControl": null, "nextControl": { - "x": 8.03547272260748, - "y": 5.149606717229121 + "x": 6.49703510162139, + "y": 1.918841878905079 }, "isLocked": false, "linkedName": "C" }, { "anchor": { - "x": 5.301, - "y": 2.622 + "x": 5.343, + "y": 2.80625 }, "prevControl": { - "x": 6.338446601941748, - "y": 2.8152427184466013 + "x": 5.527699177124219, + "y": 2.5391380847228104 }, "nextControl": null, "isLocked": false, @@ -30,8 +30,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.5923371647509561, - "rotationDegrees": 119.99999999999999 + "waypointRelativePos": 0.3, + "rotationDegrees": 162.15573333673396 } ], "constraintZones": [], diff --git a/src/main/deploy/pathplanner/paths/HP1_R5.path b/src/main/deploy/pathplanner/paths/HP1_R5.path index 8c0f456..b0abbe8 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R5.path +++ b/src/main/deploy/pathplanner/paths/HP1_R5.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 5.301, - "y": 2.622 + "x": 5.343, + "y": 2.80625 }, "prevControl": { - "x": 6.051253603043067, - "y": 3.1674757763543537 + "x": 6.093253603043067, + "y": 3.3517257763543538 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R5.path b/src/main/deploy/pathplanner/paths/HP2_R5.path index 09ca8d4..ab8e548 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R5.path +++ b/src/main/deploy/pathplanner/paths/HP2_R5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.301, - "y": 2.622 + "x": 5.343, + "y": 2.80625 }, "prevControl": { - "x": 5.064463112359014, - "y": 2.541066071483491 + "x": 5.106463112359013, + "y": 2.725316071483491 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R5_HP1.path b/src/main/deploy/pathplanner/paths/R5_HP1.path index 8fd7314..bfd1034 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP1.path +++ b/src/main/deploy/pathplanner/paths/R5_HP1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.301, - "y": 2.622 + "x": 5.343, + "y": 2.80625 }, "prevControl": null, "nextControl": { - "x": 5.200490973792311, - "y": 3.0280121030042224 + "x": 5.242490973792311, + "y": 3.2122621030042224 }, "isLocked": false, "linkedName": "R5" diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index c3d055a..2b09642 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.301, - "y": 2.622 + "x": 5.343, + "y": 2.80625 }, "prevControl": null, "nextControl": { - "x": 2.0720937660850107, - "y": 1.4211897690928004 + "x": 2.1140937660850105, + "y": 1.6054397690928004 }, "isLocked": false, "linkedName": "R5" From 1a6ea1d19ddc1882573d121b8934c23593d70909 Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 22 Mar 2025 16:28:02 -0400 Subject: [PATCH 043/106] trying some autos --- src/main/deploy/pathplanner/paths/B_R5.path | 18 +++++++++--------- src/main/deploy/pathplanner/paths/C_R3.path | 8 ++++---- src/main/deploy/pathplanner/paths/C_R4.path | 8 ++++---- src/main/deploy/pathplanner/paths/C_R5.path | 10 +++++----- src/main/deploy/pathplanner/paths/C_R6.path | 16 ++++++++-------- src/main/deploy/pathplanner/paths/HP1_R5.path | 10 +++++----- src/main/deploy/pathplanner/paths/HP1_R6.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP2_R1.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP2_R2.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP2_R3.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP2_R4.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP2_R5.path | 18 +++++++++--------- src/main/deploy/pathplanner/paths/HP2_R6.path | 16 ++++++++-------- src/main/deploy/pathplanner/paths/LeaveC.path | 8 ++++---- src/main/deploy/pathplanner/paths/R1_HP2.path | 8 ++++---- src/main/deploy/pathplanner/paths/R2_HP2.path | 8 ++++---- src/main/deploy/pathplanner/paths/R3_HP2.path | 8 ++++---- src/main/deploy/pathplanner/paths/R4_HP2.path | 8 ++++---- src/main/deploy/pathplanner/paths/R5_HP1.path | 10 +++++----- src/main/deploy/pathplanner/paths/R5_HP2.path | 18 +++++++++--------- src/main/deploy/pathplanner/paths/R6_HP1.path | 8 ++++---- src/main/deploy/pathplanner/paths/R6_HP2.path | 8 ++++---- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../MultiSubsystemCommands.java | 2 +- 24 files changed, 117 insertions(+), 117 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/B_R5.path b/src/main/deploy/pathplanner/paths/B_R5.path index 32c687f..c905bde 100644 --- a/src/main/deploy/pathplanner/paths/B_R5.path +++ b/src/main/deploy/pathplanner/paths/B_R5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.561692813425667, - "y": 4.2 + "x": 7.1566598360655735, + "y": 6.209 }, "prevControl": null, "nextControl": { - "x": 6.483629574491487, - "y": 2.818990479111984 + "x": 6.078596597131393, + "y": 4.827990479111984 }, "isLocked": false, "linkedName": "B" }, { "anchor": { - "x": 5.343, - "y": 2.80625 + "x": 5.3116683262536375, + "y": 2.983365314185704 }, "prevControl": { - "x": 6.448601941747572, - "y": 3.220997572815534 + "x": 6.41727026800121, + "y": 3.398112887001238 }, "nextControl": null, "isLocked": false, @@ -47,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": 120.89912407537605 }, "reversed": false, "folder": "Start to Reef", diff --git a/src/main/deploy/pathplanner/paths/C_R3.path b/src/main/deploy/pathplanner/paths/C_R3.path index 9d3a50c..3d5b08e 100644 --- a/src/main/deploy/pathplanner/paths/C_R3.path +++ b/src/main/deploy/pathplanner/paths/C_R3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.779, - "y": 5.058 + "x": 7.588217213114754, + "y": 1.8732069672131149 }, "prevControl": null, "nextControl": { - "x": 6.739052505681702, - "y": 5.235142414101164 + "x": 6.548269718796456, + "y": 2.0503493813142786 }, "isLocked": false, "linkedName": "C" diff --git a/src/main/deploy/pathplanner/paths/C_R4.path b/src/main/deploy/pathplanner/paths/C_R4.path index 387507c..cbab404 100644 --- a/src/main/deploy/pathplanner/paths/C_R4.path +++ b/src/main/deploy/pathplanner/paths/C_R4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.779, - "y": 5.058 + "x": 7.588217213114754, + "y": 1.8732069672131149 }, "prevControl": null, "nextControl": { - "x": 6.839422880265144, - "y": 4.594606377694494 + "x": 6.648640093379898, + "y": 1.409813344907609 }, "isLocked": false, "linkedName": "C" diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index 6b1e954..8194db3 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.343, - "y": 2.80625 + "x": 5.3116683262536375, + "y": 2.983365314185704 }, "prevControl": { - "x": 5.527699177124219, - "y": 2.5391380847228104 + "x": 5.474043325340175, + "y": 2.702123565888871 }, "nextControl": null, "isLocked": false, @@ -47,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": 120.89912407537605 }, "reversed": false, "folder": "Start to Reef", diff --git a/src/main/deploy/pathplanner/paths/C_R6.path b/src/main/deploy/pathplanner/paths/C_R6.path index c3a42d3..1d253f1 100644 --- a/src/main/deploy/pathplanner/paths/C_R6.path +++ b/src/main/deploy/pathplanner/paths/C_R6.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.779, - "y": 5.058 + "x": 7.588217213114754, + "y": 1.8732069672131149 }, "prevControl": null, "nextControl": { - "x": 7.0334575808458615, - "y": 4.270077123187671 + "x": 6.842674793960615, + "y": 1.085284090400786 }, "isLocked": false, "linkedName": "C" @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.696, - "y": 2.622 + "x": 3.9882208127212193, + "y": 2.7914590793626552 }, "prevControl": { - "x": 4.830467795017655, - "y": 2.2073128019979675 + "x": 5.122688607738874, + "y": 2.376771881360623 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP1_R5.path b/src/main/deploy/pathplanner/paths/HP1_R5.path index b0abbe8..f553712 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R5.path +++ b/src/main/deploy/pathplanner/paths/HP1_R5.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 5.343, - "y": 2.80625 + "x": 5.3116683262536375, + "y": 2.983365314185704 }, "prevControl": { - "x": 6.093253603043067, - "y": 3.3517257763543538 + "x": 6.061921929296704, + "y": 3.528841090540058 }, "nextControl": null, "isLocked": false, @@ -79,7 +79,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": 120.89912407537605 }, "reversed": false, "folder": "HP to Reef", diff --git a/src/main/deploy/pathplanner/paths/HP1_R6.path b/src/main/deploy/pathplanner/paths/HP1_R6.path index 6dc5c85..b96557f 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R6.path +++ b/src/main/deploy/pathplanner/paths/HP1_R6.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.696, - "y": 2.622 + "x": 3.9882208127212193, + "y": 2.7914590793626552 }, "prevControl": { - "x": 3.1282858658333375, - "y": 3.0999055801131097 + "x": 3.4205066785545566, + "y": 3.269364659475765 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R1.path b/src/main/deploy/pathplanner/paths/HP2_R1.path index b6c7542..69296b3 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R1.path +++ b/src/main/deploy/pathplanner/paths/HP2_R1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.418, - "y": 1.245 + "x": 1.1931710970554246, + "y": 1.000314900025919 }, "prevControl": null, "nextControl": { - "x": 2.606941747572815, - "y": 1.213592233009707 + "x": 2.3821128446282396, + "y": 0.968907133035626 }, "isLocked": false, "linkedName": "HP2" diff --git a/src/main/deploy/pathplanner/paths/HP2_R2.path b/src/main/deploy/pathplanner/paths/HP2_R2.path index f06c78a..9119372 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R2.path +++ b/src/main/deploy/pathplanner/paths/HP2_R2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.418, - "y": 1.245 + "x": 1.1931710970554246, + "y": 1.000314900025919 }, "prevControl": null, "nextControl": { - "x": 3.390728155339805, - "y": 2.4233495145631077 + "x": 3.1658992523952296, + "y": 2.1786644145890266 }, "isLocked": false, "linkedName": "HP2" diff --git a/src/main/deploy/pathplanner/paths/HP2_R3.path b/src/main/deploy/pathplanner/paths/HP2_R3.path index 0dfd16a..83fc567 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R3.path +++ b/src/main/deploy/pathplanner/paths/HP2_R3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.418, - "y": 1.245 + "x": 1.1931710970554246, + "y": 1.000314900025919 }, "prevControl": null, "nextControl": { - "x": 1.579853589087624, - "y": 1.4355345525080807 + "x": 1.3550246861430486, + "y": 1.1908494525339997 }, "isLocked": false, "linkedName": "HP2" diff --git a/src/main/deploy/pathplanner/paths/HP2_R4.path b/src/main/deploy/pathplanner/paths/HP2_R4.path index 5fa6fb1..c4dc8f7 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R4.path +++ b/src/main/deploy/pathplanner/paths/HP2_R4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.418, - "y": 1.245 + "x": 1.1931710970554246, + "y": 1.000314900025919 }, "prevControl": null, "nextControl": { - "x": 3.2040620895951974, - "y": 0.2784113500195273 + "x": 2.979233186650622, + "y": 0.03372625004544627 }, "isLocked": false, "linkedName": "HP2" diff --git a/src/main/deploy/pathplanner/paths/HP2_R5.path b/src/main/deploy/pathplanner/paths/HP2_R5.path index ab8e548..d99d71b 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R5.path +++ b/src/main/deploy/pathplanner/paths/HP2_R5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.418, - "y": 1.245 + "x": 1.1931710970554246, + "y": 1.000314900025919 }, "prevControl": null, "nextControl": { - "x": 5.639755868499901, - "y": 2.721579715385717 + "x": 5.414926965555326, + "y": 2.476894615411636 }, "isLocked": false, "linkedName": "HP2" }, { "anchor": { - "x": 5.343, - "y": 2.80625 + "x": 5.3116683262536375, + "y": 2.983365314185704 }, "prevControl": { - "x": 5.106463112359013, - "y": 2.725316071483491 + "x": 5.075131438612651, + "y": 2.9024313856691952 }, "nextControl": null, "isLocked": false, @@ -47,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": 120.89912407537605 }, "reversed": false, "folder": "HP to Reef", diff --git a/src/main/deploy/pathplanner/paths/HP2_R6.path b/src/main/deploy/pathplanner/paths/HP2_R6.path index 9c5d000..593e99f 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R6.path +++ b/src/main/deploy/pathplanner/paths/HP2_R6.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.418, - "y": 1.245 + "x": 1.1931710970554246, + "y": 1.000314900025919 }, "prevControl": null, "nextControl": { - "x": 2.69930238937698, - "y": 2.0388538665069196 + "x": 2.4744734864324043, + "y": 1.7941687665328385 }, "isLocked": false, "linkedName": "HP2" }, { "anchor": { - "x": 3.696, - "y": 2.622 + "x": 3.9882208127212193, + "y": 2.7914590793626552 }, "prevControl": { - "x": 3.4801362669835636, - "y": 2.49531771374406 + "x": 3.7723569342502774, + "y": 2.6647770409582083 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LeaveC.path b/src/main/deploy/pathplanner/paths/LeaveC.path index e89d596..df0d7cb 100644 --- a/src/main/deploy/pathplanner/paths/LeaveC.path +++ b/src/main/deploy/pathplanner/paths/LeaveC.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.779, - "y": 5.058 + "x": 7.588217213114754, + "y": 1.8732069672131149 }, "prevControl": null, "nextControl": { - "x": 7.530706640433861, - "y": 5.0288382511608125 + "x": 7.339923853548615, + "y": 1.8440452183739273 }, "isLocked": false, "linkedName": "C" diff --git a/src/main/deploy/pathplanner/paths/R1_HP2.path b/src/main/deploy/pathplanner/paths/R1_HP2.path index 69151dd..8ff7c63 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP2.path +++ b/src/main/deploy/pathplanner/paths/R1_HP2.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.418, - "y": 1.245 + "x": 1.1931710970554246, + "y": 1.000314900025919 }, "prevControl": { - "x": 2.1468932038834954, - "y": 1.469174757281552 + "x": 1.92206430093892, + "y": 1.2244896573074708 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R2_HP2.path b/src/main/deploy/pathplanner/paths/R2_HP2.path index 9226a7d..879c62a 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP2.path +++ b/src/main/deploy/pathplanner/paths/R2_HP2.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.418, - "y": 1.245 + "x": 1.1931710970554246, + "y": 1.000314900025919 }, "prevControl": { - "x": 2.7943689320388345, - "y": 1.5714077669902917 + "x": 2.5695400290942594, + "y": 1.3267226670162107 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R3_HP2.path b/src/main/deploy/pathplanner/paths/R3_HP2.path index 5e0e0f8..d9d6fa5 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP2.path +++ b/src/main/deploy/pathplanner/paths/R3_HP2.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 1.418, - "y": 1.245 + "x": 1.1931710970554246, + "y": 1.000314900025919 }, "prevControl": { - "x": 1.6447906799598557, - "y": 1.3501949974254779 + "x": 1.4199617770152804, + "y": 1.1055098974513968 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R4_HP2.path b/src/main/deploy/pathplanner/paths/R4_HP2.path index d6df8ec..205a7a6 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP2.path +++ b/src/main/deploy/pathplanner/paths/R4_HP2.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.418, - "y": 1.245 + "x": 1.1931710970554246, + "y": 1.000314900025919 }, "prevControl": { - "x": 2.210700158361901, - "y": 1.3338537501867498 + "x": 1.9858712554173257, + "y": 1.0891686502126687 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R5_HP1.path b/src/main/deploy/pathplanner/paths/R5_HP1.path index bfd1034..4fbc23b 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP1.path +++ b/src/main/deploy/pathplanner/paths/R5_HP1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.343, - "y": 2.80625 + "x": 5.3116683262536375, + "y": 2.983365314185704 }, "prevControl": null, "nextControl": { - "x": 5.242490973792311, - "y": 3.2122621030042224 + "x": 5.211159300045948, + "y": 3.3893774171899267 }, "isLocked": false, "linkedName": "R5" @@ -85,7 +85,7 @@ "folder": "Reef to HP", "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": 120.89912407537605 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index 2b09642..8b2985c 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.343, - "y": 2.80625 + "x": 5.3116683262536375, + "y": 2.983365314185704 }, "prevControl": null, "nextControl": { - "x": 2.1140937660850105, - "y": 1.6054397690928004 + "x": 2.082762092338648, + "y": 1.7825550832785046 }, "isLocked": false, "linkedName": "R5" }, { "anchor": { - "x": 1.418, - "y": 1.245 + "x": 1.1931710970554246, + "y": 1.000314900025919 }, "prevControl": { - "x": 1.6409967580939544, - "y": 1.3580152462262784 + "x": 1.416167855149379, + "y": 1.1133301462521974 }, "nextControl": null, "isLocked": false, @@ -53,7 +53,7 @@ "folder": "Reef to HP", "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": 120.89912407537605 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R6_HP1.path b/src/main/deploy/pathplanner/paths/R6_HP1.path index ecb0299..8a60639 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP1.path +++ b/src/main/deploy/pathplanner/paths/R6_HP1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.696, - "y": 2.622 + "x": 3.9882208127212193, + "y": 2.7914590793626552 }, "prevControl": null, "nextControl": { - "x": 3.084029126213592, - "y": 2.764126213592233 + "x": 3.3762499389348113, + "y": 2.9335852929548882 }, "isLocked": false, "linkedName": "R6" diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index e1140cc..70fd8ce 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.696, - "y": 2.622 + "x": 3.9882208127212193, + "y": 2.7914590793626552 }, "prevControl": null, "nextControl": { - "x": 3.484284606523374, - "y": 2.993397199875651 + "x": 3.776505419244593, + "y": 3.1628562792383064 }, "isLocked": false, "linkedName": "R6" diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1782a66..d3da11a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -212,8 +212,8 @@ public RobotContainer() { // AutoAlignToReef + Move to L4 + Score NamedCommands.registerCommand("L4", - //multiSubsystemCommands.scoreGamepieceAtPosition(OverallPosition.L4)); - elevatorCommands.setElevatorSetpoint(ElevatorPosition.L3)); + multiSubsystemCommands.scoreGamepieceAtPosition(OverallPosition.L4)); + //elevatorCommands.setElevatorSetpoint(ElevatorPosition.L3)); // AutoAlign to Intake + Intake NamedCommands.registerCommand("Intake", diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 5365e4d..32c1fc3 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -77,7 +77,7 @@ public Command setGamepieceMode(GamepieceMode mode) { public Command scoreGamepieceAtPosition(OverallPosition setpoint) { return moveToPosition(setpoint) .andThen(_armCommands.spit()); - } + } public Command loadCoral() { return moveToPosition(OverallPosition.Coral_Loading) From 120391902c86513ed6d337e0ee43d2a6db10757c Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 22 Mar 2025 17:05:11 -0400 Subject: [PATCH 044/106] 2pieceAuto stuff --- .../pathplanner/autos/B-ScoreL4ScoreL3.auto | 6 +++ src/main/deploy/pathplanner/paths/R5_HP2.path | 9 +++- src/main/deploy/pathplanner/paths/R6_HP2.path | 46 +++++++------------ src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/subsystems/arm/ArmCommands.java | 2 +- 5 files changed, 31 insertions(+), 33 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto b/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto index 9e1d959..99cd2b1 100644 --- a/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto +++ b/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto @@ -39,6 +39,12 @@ "data": { "name": "L4" } + }, + { + "type": "path", + "data": { + "pathName": "R6_HP2" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index 8b2985c..354b43e 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -36,7 +36,14 @@ ], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 0.15781537224264716, + "endWaypointRelativePos": null, + "command": null + } + ], "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index 70fd8ce..4fbb281 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -8,51 +8,37 @@ }, "prevControl": null, "nextControl": { - "x": 3.776505419244593, - "y": 3.1628562792383064 + "x": 3.5195763539459746, + "y": 2.293122682733051 }, "isLocked": false, "linkedName": "R6" }, { "anchor": { - "x": 2.521747572815534, - "y": 3.496796116504854 + "x": 1.1931710970554246, + "y": 1.000314900025919 }, "prevControl": { - "x": 2.571937204010619, - "y": 2.6310249783896342 - }, - "nextControl": { - "x": 2.385436893203884, - "y": 5.848155339805825 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.247, - "y": 6.779 - }, - "prevControl": { - "x": 2.163932038834951, - "y": 6.870485436893204 + "x": 1.3544462534336672, + "y": 1.1913393066737938 }, "nextControl": null, "isLocked": false, - "linkedName": "HP1" + "linkedName": "HP2" } ], - "rotationTargets": [ + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ { - "waypointRelativePos": 1.0681992337164712, - "rotationDegrees": -53.8 + "name": "Stow", + "waypointRelativePos": 0.20085243866822436, + "endWaypointRelativePos": null, + "command": null } ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, @@ -63,7 +49,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -53.8 + "rotation": 53.8 }, "reversed": false, "folder": "Reef to HP", diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d3da11a..5594882 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -213,7 +213,6 @@ public RobotContainer() { // AutoAlignToReef + Move to L4 + Score NamedCommands.registerCommand("L4", multiSubsystemCommands.scoreGamepieceAtPosition(OverallPosition.L4)); - //elevatorCommands.setElevatorSetpoint(ElevatorPosition.L3)); // AutoAlign to Intake + Intake NamedCommands.registerCommand("Intake", diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 70494b6..01004fd 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -64,7 +64,7 @@ private Command intakeCoral() { @Override public void execute() { if (_arm.lowerLightSensorSeesGamepiece()) { - _arm.setIntakeSpeed(0.08); + _arm.setIntakeSpeed(0.07); } else { _arm.setIntakeSpeed(0.25); // 0.35 } From 3b7152adba94911f1233e543a930a27109b211b4 Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 22 Mar 2025 17:56:27 -0400 Subject: [PATCH 045/106] end of day push from andymark session --- .../pathplanner/autos/B-ScoreL4ScoreL3.auto | 78 +++++++++++++++---- src/main/deploy/pathplanner/paths/R5_HP2.path | 35 +++++---- src/main/deploy/pathplanner/paths/R6_HP2.path | 9 +-- src/main/java/frc/robot/RobotContainer.java | 3 + .../frc/robot/subsystems/arm/ArmCommands.java | 2 +- 5 files changed, 89 insertions(+), 38 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto b/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto index 99cd2b1..11d5bfc 100644 --- a/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto +++ b/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto @@ -5,45 +5,91 @@ "data": { "commands": [ { - "type": "path", + "type": "parallel", "data": { - "pathName": "C_R5" + "commands": [ + { + "type": "path", + "data": { + "pathName": "C_R5" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] } }, { "type": "named", "data": { - "name": "L4" + "name": "Score" } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "R5_HP2" + "commands": [ + { + "type": "path", + "data": { + "pathName": "R5_HP2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] } }, { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "path", + "type": "parallel", "data": { - "pathName": "HP2_R6" + "commands": [ + { + "type": "path", + "data": { + "pathName": "HP2_R6" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] } }, { "type": "named", "data": { - "name": "L4" + "name": "Score" } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "R6_HP2" + "commands": [ + { + "type": "path", + "data": { + "pathName": "R6_HP2" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index 354b43e..98c2303 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -8,20 +8,36 @@ }, "prevControl": null, "nextControl": { - "x": 2.082762092338648, - "y": 1.7825550832785046 + "x": 5.708732638888888, + "y": 2.177879050925925 }, "isLocked": false, "linkedName": "R5" }, + { + "anchor": { + "x": 4.244138093171296, + "y": 1.7139442274305554 + }, + "prevControl": { + "x": 4.794722222222221, + "y": 1.697697482638889 + }, + "nextControl": { + "x": 3.8469329960827787, + "y": 1.7256650335741508 + }, + "isLocked": false, + "linkedName": null + }, { "anchor": { "x": 1.1931710970554246, "y": 1.000314900025919 }, "prevControl": { - "x": 1.416167855149379, - "y": 1.1133301462521974 + "x": 1.670863353587962, + "y": 1.3563993778935173 }, "nextControl": null, "isLocked": false, @@ -31,19 +47,12 @@ "rotationTargets": [ { "waypointRelativePos": 0.55, - "rotationDegrees": 53.8 + "rotationDegrees": 85.85745820949654 } ], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Stow", - "waypointRelativePos": 0.15781537224264716, - "endWaypointRelativePos": null, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index 4fbb281..23a235e 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -31,14 +31,7 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Stow", - "waypointRelativePos": 0.20085243866822436, - "endWaypointRelativePos": null, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5594882..ceefda9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -229,6 +229,9 @@ public RobotContainer() { NamedCommands.registerCommand("Stow", multiSubsystemCommands.moveToPosition(OverallPosition.Stow)); + NamedCommands.registerCommand("Score", + armCommands.spit()); + autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); // hide the joystick missing warnings diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 01004fd..c49c9b4 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -141,7 +141,7 @@ public Command runArmPID() { public Command intakeForNumberOfRotations() { return new StartEndCommand(() -> { _arm.resetIntakeEncoders(); - _arm.setIntakeSpeed(-0.15); // -0.1 + _arm.setIntakeSpeed(-0.175); // -0.1 }, () -> { _arm.setIntakeSpeed(0); From 1261170672487bdde0f8223bb113d54b8ed3deb6 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Mon, 24 Mar 2025 14:44:19 -0400 Subject: [PATCH 046/106] Maybe new intake code works *shrugs* --- .../frc/robot/subsystems/arm/ArmCommands.java | 72 +++++++++++++++++-- 1 file changed, 65 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index c49c9b4..40674e3 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -53,21 +53,79 @@ public Command intake() { } private Command intakeCoralWithAdjust() { - return intakeCoral() - .andThen(new WaitCommand(0.25)) - .andThen(moveGamepieceToLightSensor()); + return intakeCoral().andThen(moveArm(ArmPosition.Stow)); + } private Command intakeCoral() { Command c = new Command() { + /* + * This command will run the intake at a fast speed until the lower light sensor detects the coral + * Then, the speed will decrease and run until both sensors detect the coral + * Then, the motor will turn off for a few cycles (~100 ms) + * Then, the motor will run at a slow speed in reverse until the upper light sensor detects the coral + * Then, the arm will go back to stow + * + * In the case that the lower and upper light sensors don't detect a piece when running reverse, + * we assume there is no piece and start running at a fast speed again + */ + boolean hasPieceDetected = false; + boolean prevHasPieceDetected = false; + boolean waiting = false; + boolean done = false; + + int counter = 0; + + @Override + public void initialize() { + // Init flags + hasPieceDetected = false; + prevHasPieceDetected = false; + waiting = false; + done = false; + + counter = 0; + } @Override public void execute() { - if (_arm.lowerLightSensorSeesGamepiece()) { - _arm.setIntakeSpeed(0.07); + // We don't think we have a piece, so try to intake it + if (!hasPieceDetected) { + if (_arm.lowerLightSensorSeesGamepiece()) { + _arm.setIntakeSpeed(0.07); + } else { + _arm.setIntakeSpeed(0.25); // 0.35 + } } else { - _arm.setIntakeSpeed(0.25); // 0.35 + // We think we have a piece, so if this just triggered, wait for a little bit + if (!prevHasPieceDetected && hasPieceDetected) { + // Set counter flag so we start waiting + waiting = true; + _arm.setIntakeSpeed(0); + } + + if (waiting) { + counter++; + if (counter >= 5) { + // We've waited long enough, let the motors run backwards + waiting = false; + } + } else { + // Check if we actually see the gamepiece on our lower sensor after waiting + if (!_arm.lowerLightSensorSeesGamepiece()) { + // Clear out hasPiece so normal intaking will start again + _arm.clearHasGamepiece(); + } else { + // Run the motors backwards until we see the piece in the upper light sensor + _arm.setIntakeSpeed(-0.1); + done = _arm.upperLightSensorSeesGamepiece(); + } + } } + + // Update hasPiece + prevHasPieceDetected = hasPieceDetected; + hasPieceDetected = _arm.hasPiece(); } @Override @@ -77,7 +135,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return _arm.hasPiece(); + return done; } }; c.addRequirements(_arm); From cc0899d84a951799ef911bc9f44fd1d6a0de72b5 Mon Sep 17 00:00:00 2001 From: area5188 Date: Mon, 24 Mar 2025 19:23:32 -0400 Subject: [PATCH 047/106] Tuned intaking --- .../frc/robot/subsystems/arm/ArmCommands.java | 58 +++++++++---------- 1 file changed, 27 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 40674e3..45d91c4 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -80,7 +80,6 @@ private Command intakeCoral() { public void initialize() { // Init flags hasPieceDetected = false; - prevHasPieceDetected = false; waiting = false; done = false; @@ -91,41 +90,32 @@ public void initialize() { public void execute() { // We don't think we have a piece, so try to intake it if (!hasPieceDetected) { - if (_arm.lowerLightSensorSeesGamepiece()) { - _arm.setIntakeSpeed(0.07); + if (_arm.upperLightSensorSeesGamepiece()) { + _arm.setIntakeSpeed(0.09); } else { - _arm.setIntakeSpeed(0.25); // 0.35 + _arm.setIntakeSpeed(0.22); // 0.35 } + + // Update hasPiece + hasPieceDetected = _arm.hasPiece(); } else { - // We think we have a piece, so if this just triggered, wait for a little bit - if (!prevHasPieceDetected && hasPieceDetected) { - // Set counter flag so we start waiting - waiting = true; - _arm.setIntakeSpeed(0); + // Check if we actually see the gamepiece on our lower sensor after waiting + if (!_arm.lowerLightSensorSeesGamepiece()) { + counter++; } - if (waiting) { - counter++; - if (counter >= 5) { - // We've waited long enough, let the motors run backwards - waiting = false; - } + if (counter > 15) { + // Assume that at this point we don't have a piece + // Clear out hasPiece so normal intaking will start again + _arm.clearHasGamepiece(); + hasPieceDetected = false; + counter = 0; } else { - // Check if we actually see the gamepiece on our lower sensor after waiting - if (!_arm.lowerLightSensorSeesGamepiece()) { - // Clear out hasPiece so normal intaking will start again - _arm.clearHasGamepiece(); - } else { - // Run the motors backwards until we see the piece in the upper light sensor - _arm.setIntakeSpeed(-0.1); - done = _arm.upperLightSensorSeesGamepiece(); - } + // Run the motors backwards until we see the piece in the upper light sensor + _arm.setIntakeSpeed(-0.09); + done = _arm.upperLightSensorSeesGamepiece(); } } - - // Update hasPiece - prevHasPieceDetected = hasPieceDetected; - hasPieceDetected = _arm.hasPiece(); } @Override @@ -219,9 +209,15 @@ public Command resetArmPID() { } public Command manualIntake() { - return Commands.either(Commands.runOnce( - () -> _arm.setIntakeSpeed(0.1), _arm), - Commands.runOnce(() -> _arm.setIntakeSpeed(0.5), _arm), + return Commands.either( + new StartEndCommand( + () -> _arm.setIntakeSpeed(-0.1), + () -> _arm.setIntakeSpeed(0), + _arm), + new StartEndCommand( + () -> _arm.setIntakeSpeed(0.5), + () -> _arm.setIntakeSpeed(0), + _arm), () -> _arm.getCurrentMode() == GamepieceMode.CORAL); } } From 6e5b818f92136aefe664ae42b7293c993f88435b Mon Sep 17 00:00:00 2001 From: shlap Date: Mon, 24 Mar 2025 19:25:03 -0400 Subject: [PATCH 048/106] added a lot of autos plus removed moved to stow for load coral --- ...{B-ScoreL4ScoreL3.auto => A-2.5Piece.auto} | 16 ++-- .../deploy/pathplanner/autos/B-2Piece.auto | 88 +++++++++++++++++++ src/main/deploy/pathplanner/autos/LeaveD.auto | 2 +- .../deploy/pathplanner/autos/Score+Algae.auto | 37 -------- .../deploy/pathplanner/autos/TestAuto.auto | 2 +- src/main/deploy/pathplanner/paths/A_R1.path | 30 +++---- src/main/deploy/pathplanner/paths/A_R2.path | 16 ++-- src/main/deploy/pathplanner/paths/A_R3.path | 18 ++-- src/main/deploy/pathplanner/paths/A_R4.path | 18 ++-- src/main/deploy/pathplanner/paths/B_R2.path | 16 ++-- src/main/deploy/pathplanner/paths/B_R3.path | 18 ++-- src/main/deploy/pathplanner/paths/B_R4.path | 18 ++-- src/main/deploy/pathplanner/paths/B_R5.path | 20 ++--- src/main/deploy/pathplanner/paths/BackR1.path | 54 ++++++++++++ src/main/deploy/pathplanner/paths/BackR2.path | 54 ++++++++++++ src/main/deploy/pathplanner/paths/BackR3.path | 54 ++++++++++++ .../paths/{LeaveD.path => BackR4.path} | 24 ++--- src/main/deploy/pathplanner/paths/BackR5.path | 54 ++++++++++++ src/main/deploy/pathplanner/paths/BackR6.path | 54 ++++++++++++ src/main/deploy/pathplanner/paths/C_R3.path | 18 ++-- src/main/deploy/pathplanner/paths/C_R4.path | 18 ++-- src/main/deploy/pathplanner/paths/C_R5.path | 20 ++--- src/main/deploy/pathplanner/paths/C_R6.path | 30 +++---- src/main/deploy/pathplanner/paths/HP1_R1.path | 10 +-- src/main/deploy/pathplanner/paths/HP1_R2.path | 6 +- src/main/deploy/pathplanner/paths/HP1_R3.path | 10 +-- src/main/deploy/pathplanner/paths/HP1_R4.path | 10 +-- src/main/deploy/pathplanner/paths/HP1_R5.path | 12 +-- src/main/deploy/pathplanner/paths/HP1_R6.path | 22 ++--- src/main/deploy/pathplanner/paths/HP2_R1.path | 10 +-- src/main/deploy/pathplanner/paths/HP2_R2.path | 20 ++--- src/main/deploy/pathplanner/paths/HP2_R3.path | 10 +-- src/main/deploy/pathplanner/paths/HP2_R4.path | 10 +-- src/main/deploy/pathplanner/paths/HP2_R5.path | 12 +-- src/main/deploy/pathplanner/paths/HP2_R6.path | 10 +-- src/main/deploy/pathplanner/paths/LeaveA.path | 16 ++-- src/main/deploy/pathplanner/paths/LeaveB.path | 16 ++-- src/main/deploy/pathplanner/paths/LeaveC.path | 16 ++-- .../deploy/pathplanner/paths/PushScore.path | 70 +++++++++++++++ src/main/deploy/pathplanner/paths/R1_HP1.path | 22 ++--- src/main/deploy/pathplanner/paths/R1_HP2.path | 10 +-- src/main/deploy/pathplanner/paths/R2_HP1.path | 6 +- src/main/deploy/pathplanner/paths/R2_HP2.path | 8 +- src/main/deploy/pathplanner/paths/R3_HP1.path | 10 +-- src/main/deploy/pathplanner/paths/R3_HP2.path | 30 +++---- src/main/deploy/pathplanner/paths/R4_HP1.path | 40 +++------ src/main/deploy/pathplanner/paths/R4_HP2.path | 12 +-- src/main/deploy/pathplanner/paths/R5_HP1.path | 12 +-- src/main/deploy/pathplanner/paths/R5_HP2.path | 12 +-- src/main/deploy/pathplanner/paths/R6_HP1.path | 10 +-- src/main/deploy/pathplanner/paths/R6_HP2.path | 14 +-- .../paths/{TestPath.path => ScoreR1.path} | 22 ++--- .../deploy/pathplanner/paths/ScoreR2.path | 54 ++++++++++++ .../{CoralAlgae_R3.path => ScoreR3.path} | 22 ++--- .../deploy/pathplanner/paths/ScoreR4.path | 54 ++++++++++++ .../deploy/pathplanner/paths/ScoreR5.path | 54 ++++++++++++ .../deploy/pathplanner/paths/ScoreR6.path | 54 ++++++++++++ src/main/deploy/pathplanner/settings.json | 3 +- .../MultiSubsystemCommands.java | 4 +- 59 files changed, 988 insertions(+), 404 deletions(-) rename src/main/deploy/pathplanner/autos/{B-ScoreL4ScoreL3.auto => A-2.5Piece.auto} (85%) create mode 100644 src/main/deploy/pathplanner/autos/B-2Piece.auto delete mode 100644 src/main/deploy/pathplanner/autos/Score+Algae.auto create mode 100644 src/main/deploy/pathplanner/paths/BackR1.path create mode 100644 src/main/deploy/pathplanner/paths/BackR2.path create mode 100644 src/main/deploy/pathplanner/paths/BackR3.path rename src/main/deploy/pathplanner/paths/{LeaveD.path => BackR4.path} (67%) create mode 100644 src/main/deploy/pathplanner/paths/BackR5.path create mode 100644 src/main/deploy/pathplanner/paths/BackR6.path create mode 100644 src/main/deploy/pathplanner/paths/PushScore.path rename src/main/deploy/pathplanner/paths/{TestPath.path => ScoreR1.path} (71%) create mode 100644 src/main/deploy/pathplanner/paths/ScoreR2.path rename src/main/deploy/pathplanner/paths/{CoralAlgae_R3.path => ScoreR3.path} (70%) create mode 100644 src/main/deploy/pathplanner/paths/ScoreR4.path create mode 100644 src/main/deploy/pathplanner/paths/ScoreR5.path create mode 100644 src/main/deploy/pathplanner/paths/ScoreR6.path diff --git a/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto b/src/main/deploy/pathplanner/autos/A-2.5Piece.auto similarity index 85% rename from src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto rename to src/main/deploy/pathplanner/autos/A-2.5Piece.auto index 11d5bfc..7604d6a 100644 --- a/src/main/deploy/pathplanner/autos/B-ScoreL4ScoreL3.auto +++ b/src/main/deploy/pathplanner/autos/A-2.5Piece.auto @@ -11,7 +11,7 @@ { "type": "path", "data": { - "pathName": "C_R5" + "pathName": "A_R3" } }, { @@ -23,12 +23,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "parallel", "data": { @@ -36,7 +30,7 @@ { "type": "path", "data": { - "pathName": "R5_HP2" + "pathName": "R3_HP1" } }, { @@ -55,7 +49,7 @@ { "type": "path", "data": { - "pathName": "HP2_R6" + "pathName": "HP1_R2" } }, { @@ -80,13 +74,13 @@ { "type": "path", "data": { - "pathName": "R6_HP2" + "pathName": "R2_HP1" } }, { "type": "named", "data": { - "name": "Stow" + "name": "Intake" } } ] diff --git a/src/main/deploy/pathplanner/autos/B-2Piece.auto b/src/main/deploy/pathplanner/autos/B-2Piece.auto new file mode 100644 index 0000000..a7bff0c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/B-2Piece.auto @@ -0,0 +1,88 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "C_R5" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "R5_HP2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HP2_R6" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeaveD.auto b/src/main/deploy/pathplanner/autos/LeaveD.auto index a34b3a7..90b6ccc 100644 --- a/src/main/deploy/pathplanner/autos/LeaveD.auto +++ b/src/main/deploy/pathplanner/autos/LeaveD.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "LeaveD" + "pathName": null } } ] diff --git a/src/main/deploy/pathplanner/autos/Score+Algae.auto b/src/main/deploy/pathplanner/autos/Score+Algae.auto deleted file mode 100644 index a83d321..0000000 --- a/src/main/deploy/pathplanner/autos/Score+Algae.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "A_R3" - } - }, - { - "type": "named", - "data": { - "name": "L2" - } - }, - { - "type": "path", - "data": { - "pathName": "CoralAlgae_R3" - } - }, - { - "type": "named", - "data": { - "name": "AlgaeL3" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TestAuto.auto b/src/main/deploy/pathplanner/autos/TestAuto.auto index b266b0f..35b4eaf 100644 --- a/src/main/deploy/pathplanner/autos/TestAuto.auto +++ b/src/main/deploy/pathplanner/autos/TestAuto.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "TestPath" + "pathName": "ScoreR2" } } ] diff --git a/src/main/deploy/pathplanner/paths/A_R1.path b/src/main/deploy/pathplanner/paths/A_R1.path index 5794171..316b0f2 100644 --- a/src/main/deploy/pathplanner/paths/A_R1.path +++ b/src/main/deploy/pathplanner/paths/A_R1.path @@ -3,45 +3,45 @@ "waypoints": [ { "anchor": { - "x": 7.779246464530422, - "y": 7.262 + "x": 7.5282786885245905, + "y": 6.188780737704917 }, "prevControl": null, "nextControl": { - "x": 7.224741685769536, - "y": 6.582720823328432 + "x": 6.041803278688524, + "y": 6.212756147540984 }, "isLocked": false, "linkedName": "A" }, { "anchor": { - "x": 3.0669902912561082, - "y": 5.371067961167508 + "x": 3.368545081967213, + "y": 5.889088114754099 }, "prevControl": { - "x": 3.368986851901508, - "y": 6.050287320862891 + "x": 3.8495277425772816, + "y": 6.521960036609454 }, "nextControl": { - "x": 2.9654216119293113, - "y": 5.142630216081872 + "x": 3.1407786885245907, + "y": 5.589395491803279 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.85, - "y": 4.05 + "x": 3.2841721390676866, + "y": 3.837271284603343 }, "prevControl": { - "x": 2.784190034029836, - "y": 3.8088173961932457 + "x": 3.2992336144775223, + "y": 4.801256940341048 }, "nextControl": null, "isLocked": false, - "linkedName": "R1" + "linkedName": "ScoreR1" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/A_R2.path b/src/main/deploy/pathplanner/paths/A_R2.path index 08ac6d1..ecbe813 100644 --- a/src/main/deploy/pathplanner/paths/A_R2.path +++ b/src/main/deploy/pathplanner/paths/A_R2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.779246464530422, - "y": 7.262 + "x": 7.5282786885245905, + "y": 6.188780737704917 }, "prevControl": null, "nextControl": { - "x": 8.286997072703329, - "y": 7.408445022150453 + "x": 6.58125, + "y": 6.536424180327868 }, "isLocked": false, "linkedName": "A" @@ -17,15 +17,15 @@ { "anchor": { "x": 3.7, - "y": 5.45 + "y": 5.067417254011322 }, "prevControl": { - "x": 3.416014327133664, - "y": 5.34575096413502 + "x": 3.9799180327868857, + "y": 6.117878319585092 }, "nextControl": null, "isLocked": false, - "linkedName": "R2" + "linkedName": "ScoreR2" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index 5d0b6df..aa36053 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.779246464530422, - "y": 7.262 + "x": 7.5282786885245905, + "y": 6.188780737704917 }, "prevControl": null, "nextControl": { - "x": 7.442242366169765, - "y": 7.027920081967213 + "x": 6.383867159681032, + "y": 6.218696771513908 }, "isLocked": false, "linkedName": "A" }, { "anchor": { - "x": 5.166700819672132, - "y": 5.5414446721311466 + "x": 5.0115, + "y": 5.204749999999999 }, "prevControl": { - "x": 5.502356557377048, - "y": 5.68529713114754 + "x": 6.112541230111333, + "y": 5.549023212606629 }, "nextControl": null, "isLocked": false, - "linkedName": "R3" + "linkedName": "ScoreR3" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/A_R4.path b/src/main/deploy/pathplanner/paths/A_R4.path index 6f846c3..949aa72 100644 --- a/src/main/deploy/pathplanner/paths/A_R4.path +++ b/src/main/deploy/pathplanner/paths/A_R4.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.779246464530422, - "y": 7.262 + "x": 7.5282786885245905, + "y": 6.188780737704917 }, "prevControl": null, "nextControl": { - "x": 7.17447998867732, - "y": 6.3175303768448945 + "x": 6.923512212671489, + "y": 5.244311114549812 }, "isLocked": false, "linkedName": "A" }, { "anchor": { - "x": 6.1, - "y": 4.05 + "x": 5.694000000000001, + "y": 4.17125 }, "prevControl": { - "x": 6.709660199424142, - "y": 4.994337423812124 + "x": 6.303660199424143, + "y": 5.115587423812124 }, "nextControl": null, "isLocked": false, - "linkedName": "R4" + "linkedName": "ScoreR4" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/B_R2.path b/src/main/deploy/pathplanner/paths/B_R2.path index b99e3f1..55d9894 100644 --- a/src/main/deploy/pathplanner/paths/B_R2.path +++ b/src/main/deploy/pathplanner/paths/B_R2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.1566598360655735, - "y": 6.209 + "x": 7.564241803278687, + "y": 4.030993852459016 }, "prevControl": null, "nextControl": { - "x": 3.8376501273277093, - "y": 6.1207766990291255 + "x": 6.28155737704918, + "y": 6.080891393442624 }, "isLocked": false, "linkedName": "B" @@ -17,15 +17,15 @@ { "anchor": { "x": 3.7, - "y": 5.45 + "y": 5.067417254011322 }, "prevControl": { - "x": 4.083398058252427, - "y": 6.0185436893203885 + "x": 3.8360655737704916, + "y": 6.2617307786014855 }, "nextControl": null, "isLocked": false, - "linkedName": "R2" + "linkedName": "ScoreR2" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/B_R3.path b/src/main/deploy/pathplanner/paths/B_R3.path index ab8241d..250c6f2 100644 --- a/src/main/deploy/pathplanner/paths/B_R3.path +++ b/src/main/deploy/pathplanner/paths/B_R3.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.1566598360655735, - "y": 6.209 + "x": 7.564241803278687, + "y": 4.030993852459016 }, "prevControl": null, "nextControl": { - "x": 5.521960126711664, - "y": 5.905521108644577 + "x": 7.318666600733812, + "y": 4.998201937758377 }, "isLocked": false, "linkedName": "B" }, { "anchor": { - "x": 5.166700819672132, - "y": 5.5414446721311466 + "x": 5.0115, + "y": 5.204749999999999 }, "prevControl": { - "x": 6.206223156251808, - "y": 5.861689776911927 + "x": 7.091722704080768, + "y": 4.222624963698702 }, "nextControl": null, "isLocked": false, - "linkedName": "R3" + "linkedName": "ScoreR3" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/B_R4.path b/src/main/deploy/pathplanner/paths/B_R4.path index c4a508d..5ffbc21 100644 --- a/src/main/deploy/pathplanner/paths/B_R4.path +++ b/src/main/deploy/pathplanner/paths/B_R4.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.1566598360655735, - "y": 6.209 + "x": 7.564241803278687, + "y": 4.030993852459016 }, "prevControl": null, "nextControl": { - "x": 6.213223218209901, - "y": 5.212845335404368 + "x": 7.132684426229508, + "y": 4.09093237704918 }, "isLocked": false, "linkedName": "B" }, { "anchor": { - "x": 6.1, - "y": 4.05 + "x": 5.694000000000001, + "y": 4.17125 }, "prevControl": { - "x": 6.907920582003962, - "y": 4.909535891476532 + "x": 6.438979508196723, + "y": 4.21218237704918 }, "nextControl": null, "isLocked": false, - "linkedName": "R4" + "linkedName": "ScoreR4" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/B_R5.path b/src/main/deploy/pathplanner/paths/B_R5.path index c905bde..6191c95 100644 --- a/src/main/deploy/pathplanner/paths/B_R5.path +++ b/src/main/deploy/pathplanner/paths/B_R5.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.1566598360655735, - "y": 6.209 + "x": 7.564241803278687, + "y": 4.030993852459016 }, "prevControl": null, "nextControl": { - "x": 6.078596597131393, - "y": 4.827990479111984 + "x": 4.941511198192275, + "y": 2.716734633143889 }, "isLocked": false, "linkedName": "B" }, { "anchor": { - "x": 5.3116683262536375, - "y": 2.983365314185704 + "x": 5.33452868852459, + "y": 2.9817499999999995 }, "prevControl": { - "x": 6.41727026800121, - "y": 3.398112887001238 + "x": 5.583513140708558, + "y": 2.959239056646833 }, "nextControl": null, "isLocked": false, - "linkedName": "R5" + "linkedName": "ScoreR5" } ], "rotationTargets": [ @@ -47,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 120.89912407537605 + "rotation": 120.9 }, "reversed": false, "folder": "Start to Reef", diff --git a/src/main/deploy/pathplanner/paths/BackR1.path b/src/main/deploy/pathplanner/paths/BackR1.path new file mode 100644 index 0000000..4100421 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BackR1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.2841721390676866, + "y": 3.837271284603343 + }, + "prevControl": null, + "nextControl": { + "x": 3.0624733393919525, + "y": 3.952811936420523 + }, + "isLocked": false, + "linkedName": "ScoreR1" + }, + { + "anchor": { + "x": 2.85, + "y": 4.05 + }, + "prevControl": { + "x": 3.083375538410217, + "y": 3.9603570523033667 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "R1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Move back from Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BackR2.path b/src/main/deploy/pathplanner/paths/BackR2.path new file mode 100644 index 0000000..6f8b69e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BackR2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.7, + "y": 5.067417254011322 + }, + "prevControl": null, + "nextControl": { + "x": 3.8766157187288273, + "y": 4.890479728602211 + }, + "isLocked": false, + "linkedName": "ScoreR2" + }, + { + "anchor": { + "x": 3.7, + "y": 5.45 + }, + "prevControl": { + "x": 3.5007042638146406, + "y": 5.299065545555909 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "R2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.5 + }, + "reversed": false, + "folder": "Move back from Score", + "idealStartingState": { + "velocity": 0, + "rotation": -59.5 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BackR3.path b/src/main/deploy/pathplanner/paths/BackR3.path new file mode 100644 index 0000000..a80a130 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BackR3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.0115, + "y": 5.204749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 5.639996772870152, + "y": 5.677770651864003 + }, + "isLocked": false, + "linkedName": "ScoreR3" + }, + { + "anchor": { + "x": 5.2985655737704915, + "y": 5.42156762295082 + }, + "prevControl": { + "x": 4.861673689437825, + "y": 5.09856310846954 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "R3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -121.5 + }, + "reversed": false, + "folder": "Move back from Score", + "idealStartingState": { + "velocity": 0, + "rotation": -121.5 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeaveD.path b/src/main/deploy/pathplanner/paths/BackR4.path similarity index 67% rename from src/main/deploy/pathplanner/paths/LeaveD.path rename to src/main/deploy/pathplanner/paths/BackR4.path index ceb46ab..3929d8c 100644 --- a/src/main/deploy/pathplanner/paths/LeaveD.path +++ b/src/main/deploy/pathplanner/paths/BackR4.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.2165983606557385, - "y": 4.019006147540983 + "x": 5.694000000000001, + "y": 4.17125 }, "prevControl": null, "nextControl": { - "x": 6.856967213114754, - "y": 4.030993852459016 + "x": 6.3997159041487, + "y": 3.9722441316677544 }, "isLocked": false, - "linkedName": "Mid" + "linkedName": "ScoreR4" }, { "anchor": { - "x": 6.809016393442622, - "y": 4.019006147540983 + "x": 6.1, + "y": 4.05 }, "prevControl": { - "x": 7.057487327111027, - "y": 4.046614029059696 + "x": 5.289896564230451, + "y": 4.251800313049038 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "R4" } ], "rotationTargets": [], @@ -45,10 +45,10 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Leave Start", + "folder": "Move back from Score", "idealStartingState": { "velocity": 0, - "rotation": 178.51213247117218 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BackR5.path b/src/main/deploy/pathplanner/paths/BackR5.path new file mode 100644 index 0000000..3b95bee --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BackR5.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.33452868852459, + "y": 2.9817499999999995 + }, + "prevControl": null, + "nextControl": { + "x": 5.469767584510981, + "y": 2.7375562528045037 + }, + "isLocked": false, + "linkedName": "ScoreR5" + }, + { + "anchor": { + "x": 5.33452868852459, + "y": 2.604456967213115 + }, + "prevControl": { + "x": 5.177502002026731, + "y": 2.798988763399505 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "R5" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.89912407537605 + }, + "reversed": false, + "folder": "Move back from Score", + "idealStartingState": { + "velocity": 0, + "rotation": 120.9 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BackR6.path b/src/main/deploy/pathplanner/paths/BackR6.path new file mode 100644 index 0000000..38bcf22 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BackR6.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9585000000000004, + "y": 2.7965 + }, + "prevControl": null, + "nextControl": { + "x": 4.214913439492637, + "y": 2.8376815500697425 + }, + "isLocked": false, + "linkedName": "ScoreR6" + }, + { + "anchor": { + "x": 3.6802254098360656, + "y": 2.6404200819672123 + }, + "prevControl": { + "x": 3.4458839304965028, + "y": 2.553333509747695 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "R6" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": "Move back from Score", + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C_R3.path b/src/main/deploy/pathplanner/paths/C_R3.path index 3d5b08e..883802b 100644 --- a/src/main/deploy/pathplanner/paths/C_R3.path +++ b/src/main/deploy/pathplanner/paths/C_R3.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.588217213114754, - "y": 1.8732069672131149 + "x": 7.564241803278687, + "y": 1.9810963114754099 }, "prevControl": null, "nextControl": { - "x": 6.548269718796456, - "y": 2.0503493813142786 + "x": 7.137153970256865, + "y": 2.807760810345652 }, "isLocked": false, "linkedName": "C" }, { "anchor": { - "x": 5.166700819672132, - "y": 5.5414446721311466 + "x": 5.0115, + "y": 5.204749999999999 }, "prevControl": { - "x": 6.203474054637512, - "y": 5.404294915727 + "x": 5.994491803278688, + "y": 5.348602459016391 }, "nextControl": null, "isLocked": false, - "linkedName": "R3" + "linkedName": "ScoreR3" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/C_R4.path b/src/main/deploy/pathplanner/paths/C_R4.path index cbab404..70418a8 100644 --- a/src/main/deploy/pathplanner/paths/C_R4.path +++ b/src/main/deploy/pathplanner/paths/C_R4.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.588217213114754, - "y": 1.8732069672131149 + "x": 7.564241803278687, + "y": 1.9810963114754099 }, "prevControl": null, "nextControl": { - "x": 6.648640093379898, - "y": 1.409813344907609 + "x": 7.3218440432465695, + "y": 2.3290798941036686 }, "isLocked": false, "linkedName": "C" }, { "anchor": { - "x": 6.1, - "y": 4.05 + "x": 5.694000000000001, + "y": 4.17125 }, "prevControl": { - "x": 7.115829452407849, - "y": 4.6012069401364855 + "x": 5.836577979832988, + "y": 3.9650437461282673 }, "nextControl": null, "isLocked": false, - "linkedName": "R4" + "linkedName": "ScoreR4" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index 8194db3..197f51d 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.588217213114754, - "y": 1.8732069672131149 + "x": 7.564241803278687, + "y": 1.9810963114754099 }, "prevControl": null, "nextControl": { - "x": 6.49703510162139, - "y": 1.918841878905079 + "x": 6.33834728384462, + "y": 1.8987244973168789 }, "isLocked": false, "linkedName": "C" }, { "anchor": { - "x": 5.3116683262536375, - "y": 2.983365314185704 + "x": 5.33452868852459, + "y": 2.9817499999999995 }, "prevControl": { - "x": 5.474043325340175, - "y": 2.702123565888871 + "x": 5.468015810725064, + "y": 2.7703707479277213 }, "nextControl": null, "isLocked": false, - "linkedName": "R5" + "linkedName": "ScoreR5" } ], "rotationTargets": [ @@ -47,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 120.89912407537605 + "rotation": 120.9 }, "reversed": false, "folder": "Start to Reef", diff --git a/src/main/deploy/pathplanner/paths/C_R6.path b/src/main/deploy/pathplanner/paths/C_R6.path index 1d253f1..e5add5f 100644 --- a/src/main/deploy/pathplanner/paths/C_R6.path +++ b/src/main/deploy/pathplanner/paths/C_R6.path @@ -3,45 +3,45 @@ "waypoints": [ { "anchor": { - "x": 7.588217213114754, - "y": 1.8732069672131149 + "x": 7.564241803278687, + "y": 1.9810963114754099 }, "prevControl": null, "nextControl": { - "x": 6.842674793960615, - "y": 1.085284090400786 + "x": 6.503921064781505, + "y": 2.0306848647021543 }, "isLocked": false, "linkedName": "C" }, { "anchor": { - "x": 5.179805825242718, - "y": 2.014417475728154 + "x": 5.130737704918032, + "y": 1.9810963114754099 }, "prevControl": { - "x": 6.26974843361184, - "y": 1.7804533425914806 + "x": 5.730733894125063, + "y": 1.867583518922728 }, "nextControl": { - "x": 3.3519048801310447, - "y": 2.406789750010256 + "x": 4.687192622950819, + "y": 2.0650102459016395 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.9882208127212193, - "y": 2.7914590793626552 + "x": 3.9585000000000004, + "y": 2.7965 }, "prevControl": { - "x": 5.122688607738874, - "y": 2.376771881360623 + "x": 4.420243751108583, + "y": 2.592761318137971 }, "nextControl": null, "isLocked": false, - "linkedName": "R6" + "linkedName": "ScoreR6" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/HP1_R1.path b/src/main/deploy/pathplanner/paths/HP1_R1.path index d9f60e1..abcb6b2 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R1.path +++ b/src/main/deploy/pathplanner/paths/HP1_R1.path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 2.85, - "y": 4.05 + "x": 3.2841721390676866, + "y": 3.837271284603343 }, "prevControl": { - "x": 2.778195881418401, - "y": 3.7155294420433176 + "x": 3.2123680204860876, + "y": 3.5028007266466608 }, "nextControl": null, "isLocked": false, - "linkedName": "R1" + "linkedName": "ScoreR1" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/HP1_R2.path b/src/main/deploy/pathplanner/paths/HP1_R2.path index 813b803..8c73518 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R2.path +++ b/src/main/deploy/pathplanner/paths/HP1_R2.path @@ -33,15 +33,15 @@ { "anchor": { "x": 3.7, - "y": 5.45 + "y": 5.067417254011322 }, "prevControl": { "x": 3.864246218182973, - "y": 5.261524060388093 + "y": 4.878941314399415 }, "nextControl": null, "isLocked": false, - "linkedName": "R2" + "linkedName": "ScoreR2" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/HP1_R3.path b/src/main/deploy/pathplanner/paths/HP1_R3.path index 1f5a8d7..bf9efda 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R3.path +++ b/src/main/deploy/pathplanner/paths/HP1_R3.path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 5.166700819672132, - "y": 5.5414446721311466 + "x": 5.0115, + "y": 5.204749999999999 }, "prevControl": { - "x": 4.928770878437863, - "y": 5.618186727513171 + "x": 4.773570058765731, + "y": 5.2814920553820235 }, "nextControl": null, "isLocked": false, - "linkedName": "R3" + "linkedName": "ScoreR3" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/HP1_R4.path b/src/main/deploy/pathplanner/paths/HP1_R4.path index 2ecfc54..2ec648b 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R4.path +++ b/src/main/deploy/pathplanner/paths/HP1_R4.path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 6.1, - "y": 4.05 + "x": 5.694000000000001, + "y": 4.17125 }, "prevControl": { - "x": 6.299382431398496, - "y": 4.806897065998337 + "x": 5.893382431398497, + "y": 4.928147065998337 }, "nextControl": null, "isLocked": false, - "linkedName": "R4" + "linkedName": "ScoreR4" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/HP1_R5.path b/src/main/deploy/pathplanner/paths/HP1_R5.path index f553712..c6e2ecf 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R5.path +++ b/src/main/deploy/pathplanner/paths/HP1_R5.path @@ -48,16 +48,16 @@ }, { "anchor": { - "x": 5.3116683262536375, - "y": 2.983365314185704 + "x": 5.33452868852459, + "y": 2.9817499999999995 }, "prevControl": { - "x": 6.061921929296704, - "y": 3.528841090540058 + "x": 6.084782291567657, + "y": 3.5272257763543533 }, "nextControl": null, "isLocked": false, - "linkedName": "R5" + "linkedName": "ScoreR5" } ], "rotationTargets": [ @@ -79,7 +79,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 120.89912407537605 + "rotation": 120.9 }, "reversed": false, "folder": "HP to Reef", diff --git a/src/main/deploy/pathplanner/paths/HP1_R6.path b/src/main/deploy/pathplanner/paths/HP1_R6.path index b96557f..e586210 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R6.path +++ b/src/main/deploy/pathplanner/paths/HP1_R6.path @@ -16,32 +16,32 @@ }, { "anchor": { - "x": 2.7177259886248595, - "y": 4.018533305367078 + "x": 2.4765, + "y": 3.966499999999999 }, "prevControl": { - "x": 2.5539339198179394, - "y": 4.762454946512012 + "x": 2.31270793119308, + "y": 4.710421641144933 }, "nextControl": { - "x": 2.824202270680717, - "y": 3.534932291795547 + "x": 2.5829762820558577, + "y": 3.482898986428468 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.9882208127212193, - "y": 2.7914590793626552 + "x": 3.9585000000000004, + "y": 2.7965 }, "prevControl": { - "x": 3.4205066785545566, - "y": 3.269364659475765 + "x": 2.8665, + "y": 2.7965 }, "nextControl": null, "isLocked": false, - "linkedName": "R6" + "linkedName": "ScoreR6" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/HP2_R1.path b/src/main/deploy/pathplanner/paths/HP2_R1.path index 69296b3..5df0be5 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R1.path +++ b/src/main/deploy/pathplanner/paths/HP2_R1.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 2.85, - "y": 4.05 + "x": 3.2841721390676866, + "y": 3.837271284603343 }, "prevControl": { - "x": 2.7728704117477947, - "y": 4.3210022711044775 + "x": 3.207042550815481, + "y": 4.108273555707821 }, "nextControl": null, "isLocked": false, - "linkedName": "R1" + "linkedName": "ScoreR1" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/HP2_R2.path b/src/main/deploy/pathplanner/paths/HP2_R2.path index 9119372..8c9cebb 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R2.path +++ b/src/main/deploy/pathplanner/paths/HP2_R2.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 2.709174757281553, - "y": 3.8375728155339806 + "x": 2.535, + "y": 3.771499999999999 }, "prevControl": { - "x": 2.874165506224661, - "y": 3.177609819761547 + "x": 2.699990748943108, + "y": 3.1115370042275656 }, "nextControl": { - "x": 2.470631067961165, - "y": 4.7917475728155345 + "x": 2.2964563106796123, + "y": 4.725674757281553 }, "isLocked": false, "linkedName": null @@ -33,15 +33,15 @@ { "anchor": { "x": 3.7, - "y": 5.45 + "y": 5.067417254011322 }, "prevControl": { - "x": 3.2114563106796115, - "y": 4.928058252427184 + "x": 2.925, + "y": 4.951249999999999 }, "nextControl": null, "isLocked": false, - "linkedName": "R2" + "linkedName": "ScoreR2" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/HP2_R3.path b/src/main/deploy/pathplanner/paths/HP2_R3.path index 83fc567..e65052f 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R3.path +++ b/src/main/deploy/pathplanner/paths/HP2_R3.path @@ -48,16 +48,16 @@ }, { "anchor": { - "x": 5.166700819672132, - "y": 5.5414446721311466 + "x": 5.0115, + "y": 5.204749999999999 }, "prevControl": { - "x": 5.229172446940028, - "y": 6.571188920022359 + "x": 5.0739716272678965, + "y": 6.234494247891211 }, "nextControl": null, "isLocked": false, - "linkedName": "R3" + "linkedName": "ScoreR3" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/HP2_R4.path b/src/main/deploy/pathplanner/paths/HP2_R4.path index c4dc8f7..b09363a 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R4.path +++ b/src/main/deploy/pathplanner/paths/HP2_R4.path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 6.1, - "y": 4.05 + "x": 5.694000000000001, + "y": 4.17125 }, "prevControl": { - "x": 6.396, - "y": 3.1572499999999994 + "x": 5.990000000000001, + "y": 3.2784999999999993 }, "nextControl": null, "isLocked": false, - "linkedName": "R4" + "linkedName": "ScoreR4" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/HP2_R5.path b/src/main/deploy/pathplanner/paths/HP2_R5.path index d99d71b..52ba79c 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R5.path +++ b/src/main/deploy/pathplanner/paths/HP2_R5.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 5.3116683262536375, - "y": 2.983365314185704 + "x": 5.33452868852459, + "y": 2.9817499999999995 }, "prevControl": { - "x": 5.075131438612651, - "y": 2.9024313856691952 + "x": 5.097991800883603, + "y": 2.9008160714834905 }, "nextControl": null, "isLocked": false, - "linkedName": "R5" + "linkedName": "ScoreR5" } ], "rotationTargets": [ @@ -47,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 120.89912407537605 + "rotation": 120.9 }, "reversed": false, "folder": "HP to Reef", diff --git a/src/main/deploy/pathplanner/paths/HP2_R6.path b/src/main/deploy/pathplanner/paths/HP2_R6.path index 593e99f..bdd2325 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R6.path +++ b/src/main/deploy/pathplanner/paths/HP2_R6.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 3.9882208127212193, - "y": 2.7914590793626552 + "x": 3.9585000000000004, + "y": 2.7965 }, "prevControl": { - "x": 3.7723569342502774, - "y": 2.6647770409582083 + "x": 3.7426361215290584, + "y": 2.669817961595553 }, "nextControl": null, "isLocked": false, - "linkedName": "R6" + "linkedName": "ScoreR6" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/LeaveA.path b/src/main/deploy/pathplanner/paths/LeaveA.path index 2c15e54..c4f8eef 100644 --- a/src/main/deploy/pathplanner/paths/LeaveA.path +++ b/src/main/deploy/pathplanner/paths/LeaveA.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.779246464530422, - "y": 7.262 + "x": 7.5282786885245905, + "y": 6.188780737704917 }, "prevControl": null, "nextControl": { - "x": 7.520884554371662, - "y": 7.254098699313813 + "x": 7.269916778365831, + "y": 6.1808794370187305 }, "isLocked": false, "linkedName": "A" }, { "anchor": { - "x": 7.5, - "y": 7.262 + "x": 6.809016393442622, + "y": 6.188780737704917 }, "prevControl": { - "x": 7.749604617834939, - "y": 7.276054705812421 + "x": 7.058621011277562, + "y": 6.202835443517339 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LeaveB.path b/src/main/deploy/pathplanner/paths/LeaveB.path index bd95189..93fc446 100644 --- a/src/main/deploy/pathplanner/paths/LeaveB.path +++ b/src/main/deploy/pathplanner/paths/LeaveB.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.1566598360655735, - "y": 6.209 + "x": 7.564241803278687, + "y": 4.030993852459016 }, "prevControl": null, "nextControl": { - "x": 6.906773561742195, - "y": 6.201460112415871 + "x": 7.314355528955309, + "y": 4.0234539648748875 }, "isLocked": false, "linkedName": "B" }, { "anchor": { - "x": 6.58125, - "y": 6.209 + "x": 6.880942622950819, + "y": 4.030993852459016 }, "prevControl": { - "x": 6.831049987149319, - "y": 6.199001679130972 + "x": 7.130742610100138, + "y": 4.020995531589988 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LeaveC.path b/src/main/deploy/pathplanner/paths/LeaveC.path index df0d7cb..d696859 100644 --- a/src/main/deploy/pathplanner/paths/LeaveC.path +++ b/src/main/deploy/pathplanner/paths/LeaveC.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.588217213114754, - "y": 1.8732069672131149 + "x": 7.564241803278687, + "y": 1.9810963114754099 }, "prevControl": null, "nextControl": { - "x": 7.339923853548615, - "y": 1.8440452183739273 + "x": 7.315948443712548, + "y": 1.9519345626362223 }, "isLocked": false, "linkedName": "C" }, { "anchor": { - "x": 7.5, - "y": 5.058 + "x": 6.749077868852458, + "y": 1.9810963114754099 }, "prevControl": { - "x": 7.7497678481494265, - "y": 5.0687713523201605 + "x": 6.998845717001885, + "y": 1.9918676637955703 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/PushScore.path b/src/main/deploy/pathplanner/paths/PushScore.path new file mode 100644 index 0000000..a46b60d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/PushScore.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.5855, + "y": 4.0249999999999995 + }, + "prevControl": null, + "nextControl": { + "x": 7.975499999999999, + "y": 3.9957499999999997 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.94625, + "y": 4.0249999999999995 + }, + "prevControl": { + "x": 8.19625, + "y": 4.0249999999999995 + }, + "nextControl": { + "x": 6.946250000000003, + "y": 4.0249999999999995 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.694000000000001, + "y": 4.17125 + }, + "prevControl": { + "x": 6.2010000000000005, + "y": 4.103 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ScoreR4" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R1_HP1.path b/src/main/deploy/pathplanner/paths/R1_HP1.path index aa38231..274216d 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP1.path +++ b/src/main/deploy/pathplanner/paths/R1_HP1.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.85, - "y": 4.05 + "x": 3.2841721390676866, + "y": 3.837271284603343 }, "prevControl": null, "nextControl": { - "x": 2.849121552272355, - "y": 4.390187198760307 + "x": 2.8275, + "y": 4.2005 }, "isLocked": false, - "linkedName": "R1" + "linkedName": "ScoreR1" }, { "anchor": { - "x": 2.85, - "y": 5.87917236641607 + "x": 2.40825, + "y": 5.516749999999999 }, "prevControl": { - "x": 3.280781993388673, - "y": 5.350200535202626 + "x": 2.8390319933886725, + "y": 4.987778168786555 }, "nextControl": { - "x": 2.506508605458991, - "y": 6.300957108501335 + "x": 2.064758605458991, + "y": 5.938534742085264 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/R1_HP2.path b/src/main/deploy/pathplanner/paths/R1_HP2.path index 8ff7c63..4f2fee7 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP2.path +++ b/src/main/deploy/pathplanner/paths/R1_HP2.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 2.85, - "y": 4.05 + "x": 3.2841721390676866, + "y": 3.837271284603343 }, "prevControl": null, "nextControl": { - "x": 2.933014723974309, - "y": 3.482082157302947 + "x": 2.964, + "y": 3.1767499999999997 }, "isLocked": false, - "linkedName": "R1" + "linkedName": "ScoreR1" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/R2_HP1.path b/src/main/deploy/pathplanner/paths/R2_HP1.path index 411551d..2717e56 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP1.path +++ b/src/main/deploy/pathplanner/paths/R2_HP1.path @@ -4,15 +4,15 @@ { "anchor": { "x": 3.7, - "y": 5.45 + "y": 5.067417254011322 }, "prevControl": null, "nextControl": { "x": 3.3930000000000002, - "y": 5.88725 + "y": 5.504667254011322 }, "isLocked": true, - "linkedName": "R2" + "linkedName": "ScoreR2" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/R2_HP2.path b/src/main/deploy/pathplanner/paths/R2_HP2.path index 879c62a..ca05abd 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP2.path +++ b/src/main/deploy/pathplanner/paths/R2_HP2.path @@ -4,15 +4,15 @@ { "anchor": { "x": 3.7, - "y": 5.45 + "y": 5.067417254011322 }, "prevControl": null, "nextControl": { - "x": 2.8284466019417476, - "y": 5.064368932038835 + "x": 2.8470000000000004, + "y": 5.26325 }, "isLocked": false, - "linkedName": "R2" + "linkedName": "ScoreR2" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/R3_HP1.path b/src/main/deploy/pathplanner/paths/R3_HP1.path index bffcc76..19f9c03 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/R3_HP1.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 5.166700819672132, - "y": 5.5414446721311466 + "x": 5.0115, + "y": 5.204749999999999 }, "prevControl": null, "nextControl": { - "x": 5.165822371944486, - "y": 5.881631870891454 + "x": 5.0106215522723545, + "y": 5.544937198760306 }, "isLocked": false, - "linkedName": "R3" + "linkedName": "ScoreR3" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/R3_HP2.path b/src/main/deploy/pathplanner/paths/R3_HP2.path index d9d6fa5..36b882c 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP2.path +++ b/src/main/deploy/pathplanner/paths/R3_HP2.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 5.166700819672132, - "y": 5.5414446721311466 + "x": 5.0115, + "y": 5.204749999999999 }, "prevControl": null, "nextControl": { - "x": 4.437950179660972, - "y": 6.913151589586172 + "x": 4.28274935998884, + "y": 6.576456917455024 }, "isLocked": false, - "linkedName": "R3" + "linkedName": "ScoreR3" }, { "anchor": { @@ -20,28 +20,28 @@ "y": 5.576493502516883 }, "prevControl": { - "x": 3.161967272343525, - "y": 5.822659919233287 + "x": 3.498200629049366, + "y": 6.380665289450421 }, "nextControl": { - "x": 2.589902912627433, - "y": 2.593737864084271 + "x": 2.33025, + "y": 3.9079999999999995 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.453592233016892, - "y": 1.7758737864117529 + "x": 2.457, + "y": 2.279749999999999 }, "prevControl": { - "x": 2.721136619743959, - "y": 2.199101907871769 + "x": 2.7245443867270667, + "y": 2.7029781214600153 }, "nextControl": { - "x": 2.3200074722954174, - "y": 1.56455622513729 + "x": 2.3234152392785252, + "y": 2.068432438725536 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/R4_HP1.path b/src/main/deploy/pathplanner/paths/R4_HP1.path index 29982f6..56b9ee3 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP1.path +++ b/src/main/deploy/pathplanner/paths/R4_HP1.path @@ -3,45 +3,29 @@ "waypoints": [ { "anchor": { - "x": 6.1, - "y": 4.05 + "x": 5.694000000000001, + "y": 4.17125 }, "prevControl": null, "nextControl": { - "x": 5.947382318427628, - "y": 4.5856687162071434 + "x": 6.425250000000001, + "y": 4.73675 }, "isLocked": false, - "linkedName": "R4" + "linkedName": "ScoreR4" }, { "anchor": { - "x": 5.629198413591059, - "y": 5.458108455275157 + "x": 5.391749999999999, + "y": 5.653250000000001 }, "prevControl": { - "x": 5.947227462297942, - "y": 5.1430120797868195 + "x": 5.77666058421225, + "y": 5.424619197550555 }, "nextControl": { - "x": 4.736796116505998, - "y": 6.34228155340034 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.1181067961176496, - "y": 6.376359223303252 - }, - "prevControl": { - "x": 3.487827573715305, - "y": 6.109813309370754 - }, - "nextControl": { - "x": 2.4581067023021, - "y": 6.852178649370358 + "x": 4.42305158640894, + "y": 6.2286415447248435 }, "isLocked": false, "linkedName": null @@ -62,7 +46,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.464367816091949, + "waypointRelativePos": 1.25, "rotationDegrees": -53.8 } ], diff --git a/src/main/deploy/pathplanner/paths/R4_HP2.path b/src/main/deploy/pathplanner/paths/R4_HP2.path index 205a7a6..dc013a2 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP2.path +++ b/src/main/deploy/pathplanner/paths/R4_HP2.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 6.1, - "y": 4.05 + "x": 5.694000000000001, + "y": 4.17125 }, "prevControl": null, "nextControl": { - "x": 5.970254952512325, - "y": 4.4576470052549215 + "x": 5.564254952512326, + "y": 4.578897005254921 }, "isLocked": false, - "linkedName": "R4" + "linkedName": "ScoreR4" }, { "anchor": { @@ -36,7 +36,7 @@ "y": 1.000314900025919 }, "prevControl": { - "x": 1.9858712554173257, + "x": 1.9858712554173255, "y": 1.0891686502126687 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/R5_HP1.path b/src/main/deploy/pathplanner/paths/R5_HP1.path index 4fbc23b..e1a30f2 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP1.path +++ b/src/main/deploy/pathplanner/paths/R5_HP1.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 5.3116683262536375, - "y": 2.983365314185704 + "x": 5.33452868852459, + "y": 2.9817499999999995 }, "prevControl": null, "nextControl": { - "x": 5.211159300045948, - "y": 3.3893774171899267 + "x": 5.234019662316901, + "y": 3.387762103004222 }, "isLocked": false, - "linkedName": "R5" + "linkedName": "ScoreR5" }, { "anchor": { @@ -85,7 +85,7 @@ "folder": "Reef to HP", "idealStartingState": { "velocity": 0, - "rotation": 120.89912407537605 + "rotation": 120.9 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index 98c2303..aebf642 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 5.3116683262536375, - "y": 2.983365314185704 + "x": 5.33452868852459, + "y": 2.9817499999999995 }, "prevControl": null, "nextControl": { - "x": 5.708732638888888, - "y": 2.177879050925925 + "x": 5.73159300115984, + "y": 2.1762637367402204 }, "isLocked": false, - "linkedName": "R5" + "linkedName": "ScoreR5" }, { "anchor": { @@ -69,7 +69,7 @@ "folder": "Reef to HP", "idealStartingState": { "velocity": 0, - "rotation": 120.89912407537605 + "rotation": 120.9 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R6_HP1.path b/src/main/deploy/pathplanner/paths/R6_HP1.path index 8a60639..fe80fe5 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP1.path +++ b/src/main/deploy/pathplanner/paths/R6_HP1.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.9882208127212193, - "y": 2.7914590793626552 + "x": 3.9585000000000004, + "y": 2.7965 }, "prevControl": null, "nextControl": { - "x": 3.3762499389348113, - "y": 2.9335852929548882 + "x": 3.3465291262135923, + "y": 2.938626213592233 }, "isLocked": false, - "linkedName": "R6" + "linkedName": "ScoreR6" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index 23a235e..fa0b23c 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.9882208127212193, - "y": 2.7914590793626552 + "x": 3.9585000000000004, + "y": 2.7965 }, "prevControl": null, "nextControl": { - "x": 3.5195763539459746, - "y": 2.293122682733051 + "x": 3.4898555412247556, + "y": 2.2981636033703956 }, "isLocked": false, - "linkedName": "R6" + "linkedName": "ScoreR6" }, { "anchor": { @@ -20,8 +20,8 @@ "y": 1.000314900025919 }, "prevControl": { - "x": 1.3544462534336672, - "y": 1.1913393066737938 + "x": 1.354446253433668, + "y": 1.191339306673795 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/TestPath.path b/src/main/deploy/pathplanner/paths/ScoreR1.path similarity index 71% rename from src/main/deploy/pathplanner/paths/TestPath.path rename to src/main/deploy/pathplanner/paths/ScoreR1.path index 641d703..3c2d153 100644 --- a/src/main/deploy/pathplanner/paths/TestPath.path +++ b/src/main/deploy/pathplanner/paths/ScoreR1.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 2.85, + "y": 4.05 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 3.0693210647773, + "y": 3.930007206279079 }, "isLocked": false, - "linkedName": null + "linkedName": "R1" }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 3.2841721390676866, + "y": 3.837271284603343 }, "prevControl": { - "x": 3.752151639344262, - "y": 6.6563012295081965 + "x": 3.0477187212070858, + "y": 3.9184487518097058 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "ScoreR1" } ], "rotationTargets": [], @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Test", + "folder": "Move to Score", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/ScoreR2.path b/src/main/deploy/pathplanner/paths/ScoreR2.path new file mode 100644 index 0000000..810fb09 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ScoreR2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.7, + "y": 5.45 + }, + "prevControl": null, + "nextControl": { + "x": 3.69264400526603, + "y": 5.200108244750905 + }, + "isLocked": false, + "linkedName": "R2" + }, + { + "anchor": { + "x": 3.7, + "y": 5.067417254011322 + }, + "prevControl": { + "x": 3.7006651920836937, + "y": 5.31741636904874 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ScoreR2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.5 + }, + "reversed": false, + "folder": "Move to Score", + "idealStartingState": { + "velocity": 0, + "rotation": -59.5 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CoralAlgae_R3.path b/src/main/deploy/pathplanner/paths/ScoreR3.path similarity index 70% rename from src/main/deploy/pathplanner/paths/CoralAlgae_R3.path rename to src/main/deploy/pathplanner/paths/ScoreR3.path index c78d839..f6c86cd 100644 --- a/src/main/deploy/pathplanner/paths/CoralAlgae_R3.path +++ b/src/main/deploy/pathplanner/paths/ScoreR3.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 5.166700819672132, - "y": 5.5414446721311466 + "x": 5.2985655737704915, + "y": 5.42156762295082 }, "prevControl": null, "nextControl": { - "x": 5.376465786078236, - "y": 5.405434897230054 + "x": 5.098015063530137, + "y": 5.272304456555579 }, "isLocked": false, "linkedName": "R3" }, { "anchor": { - "x": 5.324783571442977, - "y": 5.43542116633939 + "x": 5.0115, + "y": 5.204749999999999 }, "prevControl": { - "x": 5.105176780835683, - "y": 5.554890231453185 + "x": 5.204482558904262, + "y": 5.363676813215275 }, "nextControl": null, "isLocked": false, - "linkedName": "R3Algae" + "linkedName": "ScoreR3" } ], "rotationTargets": [], @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -45,7 +45,7 @@ "rotation": -121.5 }, "reversed": false, - "folder": null, + "folder": "Move to Score", "idealStartingState": { "velocity": 0, "rotation": -121.5 diff --git a/src/main/deploy/pathplanner/paths/ScoreR4.path b/src/main/deploy/pathplanner/paths/ScoreR4.path new file mode 100644 index 0000000..ce1362e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ScoreR4.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.1, + "y": 4.05 + }, + "prevControl": null, + "nextControl": { + "x": 5.865632200817294, + "y": 4.137015715283246 + }, + "isLocked": false, + "linkedName": "R4" + }, + { + "anchor": { + "x": 5.694000000000001, + "y": 4.17125 + }, + "prevControl": { + "x": 5.9397180467335815, + "y": 4.125177866237452 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ScoreR4" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Move to Score", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ScoreR5.path b/src/main/deploy/pathplanner/paths/ScoreR5.path new file mode 100644 index 0000000..92a6843 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ScoreR5.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.33452868852459, + "y": 2.604456967213115 + }, + "prevControl": null, + "nextControl": { + "x": 5.180504780071455, + "y": 2.8473379695519756 + }, + "isLocked": false, + "linkedName": "R5" + }, + { + "anchor": { + "x": 5.33452868852459, + "y": 2.9817499999999995 + }, + "prevControl": { + "x": 5.580881831494815, + "y": 2.8335505578856015 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ScoreR5" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 120.9 + }, + "reversed": false, + "folder": "Move to Score", + "idealStartingState": { + "velocity": 0, + "rotation": 120.89912407537605 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ScoreR6.path b/src/main/deploy/pathplanner/paths/ScoreR6.path new file mode 100644 index 0000000..5988b73 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ScoreR6.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6802254098360656, + "y": 2.6404200819672123 + }, + "prevControl": null, + "nextControl": { + "x": 3.8968743230212, + "y": 2.7651728310980435 + }, + "isLocked": false, + "linkedName": "R6" + }, + { + "anchor": { + "x": 3.9585000000000004, + "y": 2.7965 + }, + "prevControl": { + "x": 3.7316487075937057, + "y": 2.69143577614814 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ScoreR6" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": "Move to Score", + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "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 0fd10e4..6d24c1d 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -5,7 +5,8 @@ "pathFolders": [ "HP to Reef", "Leave Start", - "Test", + "Move to Score", + "Move back from Score", "Reef to HP", "Start to Reef" ], diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 32c1fc3..9cebf6c 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -81,9 +81,7 @@ public Command scoreGamepieceAtPosition(OverallPosition setpoint) { public Command loadCoral() { return moveToPosition(OverallPosition.Coral_Loading) - .andThen(_armCommands.intake()) - .andThen(moveToPosition(OverallPosition.Stow)); - + .andThen(_armCommands.intake()); } public Command loadAlgae(OverallPosition position) { From ba10ea7b79af5f69d555da7e5daafb7437da62d4 Mon Sep 17 00:00:00 2001 From: area5188 Date: Mon, 24 Mar 2025 19:34:47 -0400 Subject: [PATCH 049/106] Added feedforward offset --- src/main/java/frc/robot/subsystems/arm/Arm.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 75a5f57..ea2ae60 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -66,6 +66,7 @@ public enum ArmPosition { public static final double HAS_ALGAE_CURRENT = 40; private static final double ARM_FEEDFORWARD_COEFF = 0.4; + private static final double ARM_FEEDFORWARD_ANGLE_OFFSET = -22.3; SysIdRoutine routine = new SysIdRoutine(new Config(), new SysIdRoutine.Mechanism(this::setArmVoltage, this::populateLog, this)); @@ -175,7 +176,7 @@ public void resetPID(){ public void runArmPID() { double out = _armPidController.calculate(_inputs._armEncoderPositionDegrees) - + (ARM_FEEDFORWARD_COEFF * Math.cos(Units.degreesToRadians(_inputs._armEncoderPositionDegrees))); + + (ARM_FEEDFORWARD_COEFF * Math.cos(Units.degreesToRadians(_inputs._armEncoderPositionDegrees + ARM_FEEDFORWARD_ANGLE_OFFSET))); _io.setArmMotorVoltage(Voltage.ofBaseUnits(out, Volt)); } From f6b37541a285cf782c7795321ae7cb205b6c4793 Mon Sep 17 00:00:00 2001 From: shlap Date: Mon, 24 Mar 2025 19:35:17 -0400 Subject: [PATCH 050/106] added pusscore auto --- .../deploy/pathplanner/autos/PushScore.auto | 51 +++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/PushScore.auto diff --git a/src/main/deploy/pathplanner/autos/PushScore.auto b/src/main/deploy/pathplanner/autos/PushScore.auto new file mode 100644 index 0000000..beff776 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PushScore.auto @@ -0,0 +1,51 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "PushScore" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BackR4" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file From f6348159f8a8e8394e3da210a638229dbe1fdf43 Mon Sep 17 00:00:00 2001 From: shlap Date: Mon, 24 Mar 2025 19:57:16 -0400 Subject: [PATCH 051/106] changed auto --- src/main/deploy/pathplanner/autos/B-2Piece.auto | 17 +++++++++++++++-- src/main/java/frc/robot/subsystems/arm/Arm.java | 4 ++-- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/B-2Piece.auto b/src/main/deploy/pathplanner/autos/B-2Piece.auto index a7bff0c..319335f 100644 --- a/src/main/deploy/pathplanner/autos/B-2Piece.auto +++ b/src/main/deploy/pathplanner/autos/B-2Piece.auto @@ -74,9 +74,22 @@ } }, { - "type": "named", + "type": "parallel", "data": { - "name": "Stow" + "commands": [ + { + "type": "path", + "data": { + "pathName": "R6_HP2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] } } ] diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index ea2ae60..28ccadf 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -175,8 +175,8 @@ public void resetPID(){ } public void runArmPID() { - double out = _armPidController.calculate(_inputs._armEncoderPositionDegrees) - + (ARM_FEEDFORWARD_COEFF * Math.cos(Units.degreesToRadians(_inputs._armEncoderPositionDegrees + ARM_FEEDFORWARD_ANGLE_OFFSET))); + // double out = _armPidController.calculate(_inputs._armEncoderPositionDegrees) + double out = (ARM_FEEDFORWARD_COEFF * Math.cos(Units.degreesToRadians(_inputs._armEncoderPositionDegrees + ARM_FEEDFORWARD_ANGLE_OFFSET))); _io.setArmMotorVoltage(Voltage.ofBaseUnits(out, Volt)); } From de8b4ea298f5ae905b5809fd646cc2dc519e47a8 Mon Sep 17 00:00:00 2001 From: area5188 Date: Mon, 24 Mar 2025 19:58:40 -0400 Subject: [PATCH 052/106] Tuned arm feedforward --- src/main/java/frc/robot/subsystems/arm/Arm.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index ea2ae60..f73a04b 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -65,7 +65,7 @@ public enum ArmPosition { public static final double HAS_ALGAE_CURRENT = 40; - private static final double ARM_FEEDFORWARD_COEFF = 0.4; + private static final double ARM_FEEDFORWARD_COEFF = 0.6; private static final double ARM_FEEDFORWARD_ANGLE_OFFSET = -22.3; SysIdRoutine routine = new SysIdRoutine(new Config(), @@ -176,7 +176,7 @@ public void resetPID(){ public void runArmPID() { double out = _armPidController.calculate(_inputs._armEncoderPositionDegrees) - + (ARM_FEEDFORWARD_COEFF * Math.cos(Units.degreesToRadians(_inputs._armEncoderPositionDegrees + ARM_FEEDFORWARD_ANGLE_OFFSET))); + + (ARM_FEEDFORWARD_COEFF * Math.cos(Units.degreesToRadians(_inputs._armEncoderPositionDegrees + ARM_FEEDFORWARD_ANGLE_OFFSET))); _io.setArmMotorVoltage(Voltage.ofBaseUnits(out, Volt)); } From 39a8db9678d276d289ef5e4ca4a3f8c0677493fa Mon Sep 17 00:00:00 2001 From: shlap Date: Mon, 24 Mar 2025 20:04:50 -0400 Subject: [PATCH 053/106] everything in auto is parallel --- src/main/deploy/pathplanner/autos/B-2Piece.auto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/autos/B-2Piece.auto b/src/main/deploy/pathplanner/autos/B-2Piece.auto index 319335f..ad19c1c 100644 --- a/src/main/deploy/pathplanner/autos/B-2Piece.auto +++ b/src/main/deploy/pathplanner/autos/B-2Piece.auto @@ -5,7 +5,7 @@ "data": { "commands": [ { - "type": "sequential", + "type": "parallel", "data": { "commands": [ { From 4198f15357ce10e97509ffe8274061bacf818415 Mon Sep 17 00:00:00 2001 From: shlap Date: Mon, 24 Mar 2025 20:17:05 -0400 Subject: [PATCH 054/106] Changed positions --- src/main/deploy/pathplanner/paths/HP1_R1.path | 4 ++-- src/main/deploy/pathplanner/paths/HP1_R3.path | 4 ++-- src/main/deploy/pathplanner/paths/HP1_R4.path | 4 ++-- src/main/deploy/pathplanner/paths/HP1_R5.path | 4 ++-- src/main/deploy/pathplanner/paths/HP1_R6.path | 4 ++-- src/main/deploy/pathplanner/paths/HP2_R1.path | 4 ++-- src/main/deploy/pathplanner/paths/HP2_R2.path | 4 ++-- src/main/deploy/pathplanner/paths/HP2_R3.path | 4 ++-- src/main/deploy/pathplanner/paths/HP2_R4.path | 4 ++-- src/main/deploy/pathplanner/paths/HP2_R5.path | 4 ++-- src/main/deploy/pathplanner/paths/R1_HP1.path | 4 ++-- src/main/deploy/pathplanner/paths/R1_HP2.path | 4 ++-- src/main/deploy/pathplanner/paths/R2_HP2.path | 4 ++-- src/main/deploy/pathplanner/paths/R3_HP1.path | 4 ++-- src/main/deploy/pathplanner/paths/R3_HP2.path | 4 ++-- src/main/deploy/pathplanner/paths/R4_HP1.path | 4 ++-- src/main/deploy/pathplanner/paths/R4_HP2.path | 12 ++++++------ src/main/deploy/pathplanner/paths/R5_HP1.path | 12 ++++++------ src/main/deploy/pathplanner/paths/R5_HP2.path | 4 ++-- src/main/deploy/pathplanner/paths/R6_HP1.path | 4 ++-- 20 files changed, 48 insertions(+), 48 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/HP1_R1.path b/src/main/deploy/pathplanner/paths/HP1_R1.path index abcb6b2..b37bd77 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R1.path +++ b/src/main/deploy/pathplanner/paths/HP1_R1.path @@ -36,8 +36,8 @@ "y": 3.837271284603343 }, "prevControl": { - "x": 3.2123680204860876, - "y": 3.5028007266466608 + "x": 2.13525, + "y": 3.89825 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP1_R3.path b/src/main/deploy/pathplanner/paths/HP1_R3.path index bf9efda..d7102ae 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R3.path +++ b/src/main/deploy/pathplanner/paths/HP1_R3.path @@ -36,8 +36,8 @@ "y": 5.204749999999999 }, "prevControl": { - "x": 4.773570058765731, - "y": 5.2814920553820235 + "x": 5.664749999999999, + "y": 5.77025 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP1_R4.path b/src/main/deploy/pathplanner/paths/HP1_R4.path index 2ec648b..8ea5ff4 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R4.path +++ b/src/main/deploy/pathplanner/paths/HP1_R4.path @@ -36,8 +36,8 @@ "y": 4.17125 }, "prevControl": { - "x": 5.893382431398497, - "y": 4.928147065998337 + "x": 7.1175, + "y": 4.063999999999999 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP1_R5.path b/src/main/deploy/pathplanner/paths/HP1_R5.path index c6e2ecf..ce95c4f 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R5.path +++ b/src/main/deploy/pathplanner/paths/HP1_R5.path @@ -52,8 +52,8 @@ "y": 2.9817499999999995 }, "prevControl": { - "x": 6.084782291567657, - "y": 3.5272257763543533 + "x": 5.762249999999999, + "y": 2.4844999999999993 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP1_R6.path b/src/main/deploy/pathplanner/paths/HP1_R6.path index e586210..5978ec5 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R6.path +++ b/src/main/deploy/pathplanner/paths/HP1_R6.path @@ -36,8 +36,8 @@ "y": 2.7965 }, "prevControl": { - "x": 2.8665, - "y": 2.7965 + "x": 3.315, + "y": 1.88 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R1.path b/src/main/deploy/pathplanner/paths/HP2_R1.path index 5df0be5..7623739 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R1.path +++ b/src/main/deploy/pathplanner/paths/HP2_R1.path @@ -20,8 +20,8 @@ "y": 3.837271284603343 }, "prevControl": { - "x": 3.207042550815481, - "y": 4.108273555707821 + "x": 2.11575, + "y": 4.112749999999999 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R2.path b/src/main/deploy/pathplanner/paths/HP2_R2.path index 8c9cebb..9b8ba00 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R2.path +++ b/src/main/deploy/pathplanner/paths/HP2_R2.path @@ -36,8 +36,8 @@ "y": 5.067417254011322 }, "prevControl": { - "x": 2.925, - "y": 4.951249999999999 + "x": 3.16875, + "y": 5.82875 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R3.path b/src/main/deploy/pathplanner/paths/HP2_R3.path index e65052f..130e52d 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R3.path +++ b/src/main/deploy/pathplanner/paths/HP2_R3.path @@ -52,8 +52,8 @@ "y": 5.204749999999999 }, "prevControl": { - "x": 5.0739716272678965, - "y": 6.234494247891211 + "x": 5.421, + "y": 6.3454999999999995 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R4.path b/src/main/deploy/pathplanner/paths/HP2_R4.path index b09363a..66901b5 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R4.path +++ b/src/main/deploy/pathplanner/paths/HP2_R4.path @@ -36,8 +36,8 @@ "y": 4.17125 }, "prevControl": { - "x": 5.990000000000001, - "y": 3.2784999999999993 + "x": 7.244249999999999, + "y": 4.063999999999999 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R5.path b/src/main/deploy/pathplanner/paths/HP2_R5.path index 52ba79c..fd90c49 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R5.path +++ b/src/main/deploy/pathplanner/paths/HP2_R5.path @@ -20,8 +20,8 @@ "y": 2.9817499999999995 }, "prevControl": { - "x": 5.097991800883603, - "y": 2.9008160714834905 + "x": 6.1034999999999995, + "y": 2.094499999999999 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R1_HP1.path b/src/main/deploy/pathplanner/paths/R1_HP1.path index 274216d..55299ca 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP1.path +++ b/src/main/deploy/pathplanner/paths/R1_HP1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.8275, - "y": 4.2005 + "x": 2.2230000000000003, + "y": 3.6642499999999996 }, "isLocked": false, "linkedName": "ScoreR1" diff --git a/src/main/deploy/pathplanner/paths/R1_HP2.path b/src/main/deploy/pathplanner/paths/R1_HP2.path index 4f2fee7..a37bc66 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP2.path +++ b/src/main/deploy/pathplanner/paths/R1_HP2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.964, - "y": 3.1767499999999997 + "x": 2.262, + "y": 3.9372499999999997 }, "isLocked": false, "linkedName": "ScoreR1" diff --git a/src/main/deploy/pathplanner/paths/R2_HP2.path b/src/main/deploy/pathplanner/paths/R2_HP2.path index ca05abd..89b052b 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP2.path +++ b/src/main/deploy/pathplanner/paths/R2_HP2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.8470000000000004, - "y": 5.26325 + "x": 3.1005000000000003, + "y": 5.9945 }, "isLocked": false, "linkedName": "ScoreR2" diff --git a/src/main/deploy/pathplanner/paths/R3_HP1.path b/src/main/deploy/pathplanner/paths/R3_HP1.path index 19f9c03..c017bb8 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/R3_HP1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.0106215522723545, - "y": 5.544937198760306 + "x": 6.005999999999999, + "y": 6.3065 }, "isLocked": false, "linkedName": "ScoreR3" diff --git a/src/main/deploy/pathplanner/paths/R3_HP2.path b/src/main/deploy/pathplanner/paths/R3_HP2.path index 36b882c..1c71048 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP2.path +++ b/src/main/deploy/pathplanner/paths/R3_HP2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 4.28274935998884, - "y": 6.576456917455024 + "x": 5.742750000000001, + "y": 6.2675 }, "isLocked": false, "linkedName": "ScoreR3" diff --git a/src/main/deploy/pathplanner/paths/R4_HP1.path b/src/main/deploy/pathplanner/paths/R4_HP1.path index 56b9ee3..fa41e43 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP1.path +++ b/src/main/deploy/pathplanner/paths/R4_HP1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.425250000000001, - "y": 4.73675 + "x": 7.0005, + "y": 4.2395 }, "isLocked": false, "linkedName": "ScoreR4" diff --git a/src/main/deploy/pathplanner/paths/R4_HP2.path b/src/main/deploy/pathplanner/paths/R4_HP2.path index dc013a2..0722e7e 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP2.path +++ b/src/main/deploy/pathplanner/paths/R4_HP2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.564254952512326, - "y": 4.578897005254921 + "x": 7.06875, + "y": 4.2395 }, "isLocked": false, "linkedName": "ScoreR4" @@ -20,12 +20,12 @@ "y": 1.4862135922330106 }, "prevControl": { - "x": 6.372898260607746, - "y": 1.7692330578997566 + "x": 5.499, + "y": 1.53875 }, "nextControl": { - "x": 2.0993031068751495, - "y": 0.8354826853696199 + "x": 2.052457683064141, + "y": 1.1090934443173022 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/R5_HP1.path b/src/main/deploy/pathplanner/paths/R5_HP1.path index e1a30f2..774e704 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP1.path +++ b/src/main/deploy/pathplanner/paths/R5_HP1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.234019662316901, - "y": 3.387762103004222 + "x": 5.733, + "y": 2.4454999999999996 }, "isLocked": false, "linkedName": "ScoreR5" @@ -20,12 +20,12 @@ "y": 2.167766990298476 }, "prevControl": { - "x": 4.508926689171617, - "y": 1.350515834104014 + "x": 4.8555, + "y": 1.645999999999999 }, "nextControl": { - "x": 2.546686845785932, - "y": 3.1055620379337188 + "x": 2.2954675427007376, + "y": 2.7058542064619293 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index aebf642..e095879 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.73159300115984, - "y": 2.1762637367402204 + "x": 6.24, + "y": 1.938499999999999 }, "isLocked": false, "linkedName": "ScoreR5" diff --git a/src/main/deploy/pathplanner/paths/R6_HP1.path b/src/main/deploy/pathplanner/paths/R6_HP1.path index fe80fe5..d13facf 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP1.path +++ b/src/main/deploy/pathplanner/paths/R6_HP1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 3.3465291262135923, - "y": 2.938626213592233 + "x": 3.6270000000000007, + "y": 1.88 }, "isLocked": false, "linkedName": "ScoreR6" From d1c26a15ce81deb250621b3f022e973fbf71b254 Mon Sep 17 00:00:00 2001 From: area5188 Date: Wed, 26 Mar 2025 18:35:04 -0400 Subject: [PATCH 055/106] driver practice 3/26/2025 --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++-------- src/main/java/frc/robot/subsystems/arm/Arm.java | 6 +++--- .../frc/robot/subsystems/arm/ArmCommands.java | 2 +- .../frc/robot/subsystems/elevator/Elevator.java | 6 +++--- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ceefda9..701f008 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -71,10 +71,10 @@ public class RobotContainer { private final Drive drive; private final Elevator elevatorSubsystem = new Elevator(new RealElevatorIO()); private final Arm armSubsystem = new Arm(new RealArmIO()); - // // private final LEDs LEDSubsystem = new LEDs(); + private final LEDs LEDSubsystem = new LEDs(); private final ElevatorCommands elevatorCommands = new ElevatorCommands(elevatorSubsystem); private final ArmCommands armCommands = new ArmCommands(armSubsystem); - // // private final LEDsCommands LEDCommands = new LEDsCommands(LEDSubsystem); + private final LEDsCommands LEDCommands = new LEDsCommands(LEDSubsystem); private final Climber climber = new Climber(new RealClimberIO()); private final ClimberCommands ClimberCommands = new ClimberCommands(climber); @@ -365,15 +365,15 @@ public void calibrateAndStartPIDs() { } public void startIdleAnimations() { - // Command disabled1 = LEDCommands.disabledAnimation1(); - // if (!CommandScheduler.getInstance().isScheduled(disabled1)) - // CommandScheduler.getInstance().schedule(disabled1); + Command disabled1 = LEDCommands.disabledAnimation1(); + if (!CommandScheduler.getInstance().isScheduled(disabled1)) + CommandScheduler.getInstance().schedule(disabled1); } public void startEnabledLEDs() { - // Command initialLEDs = LEDCommands.pickingUpCoral(); - // if (!CommandScheduler.getInstance().isScheduled(initialLEDs)) - // CommandScheduler.getInstance().schedule(initialLEDs); + Command initialLEDs = LEDCommands.pickingUpCoral(); + if (!CommandScheduler.getInstance().isScheduled(initialLEDs)) + CommandScheduler.getInstance().schedule(initialLEDs); } } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index f73a04b..4527107 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -57,11 +57,11 @@ public enum ArmPosition { private ProfiledPIDController _armPidController; - private static final double KP = 0.06;//0.09; + private static final double KP = 0.1;//0.09; private static final double KI = 0.00; //0.01; private static final double KD = 0.01; - private static final double PROFILE_VEL = 160; - private static final double PROFILE_ACC = 145; + private static final double PROFILE_VEL = 330; + private static final double PROFILE_ACC = 300; public static final double HAS_ALGAE_CURRENT = 40; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 45d91c4..e5447b1 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -28,7 +28,7 @@ public Command spit() { () -> { _arm.setIntakeSpeed(0); _arm.clearHasGamepiece(); - }, _arm).withTimeout(1) + }, _arm).withTimeout(0.3) .andThen(Commands.runOnce(() -> _arm.setArmSetpoint(ArmPosition.Stow), _arm).unless(() -> _arm.getCurrentPos() != ArmPosition.L4_Score)); diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 1567a0c..b9b3f29 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -58,11 +58,11 @@ public enum ElevatorPosition { private static final int INCREMENT_CONSTANT = 1; private static final int DECREMENT_CONSTANT = 1; - private static final double ELEVATOR_MOTOR_KP = 0.8; //0.75; + private static final double ELEVATOR_MOTOR_KP = 1.5; //0.75; private static final double ELEVATOR_MOTOR_KI = 0;//0.15; private static final double ELEVATOR_MOTOR_KD = 0; - private static final double ELEVATOR_PID_VEL = 220; - private static final double ELEVATOR_PID_ACC = 215; + private static final double ELEVATOR_PID_VEL = 400; + private static final double ELEVATOR_PID_ACC = 335; private static final double ELEVATOR_MAX_INCHES = 52; //48; private static final double ELEVATOR_MAX_ROTATIONS = 87; // 36.4; From 90beac943efbb22baaf9c126f5c1978f96394f70 Mon Sep 17 00:00:00 2001 From: area5188 Date: Wed, 26 Mar 2025 19:11:06 -0400 Subject: [PATCH 056/106] cheeseckae --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/arm/Arm.java | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 701f008..16371c7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -260,7 +260,7 @@ private void configureBindings() { () -> -joystick.getRightX())); - //climberstick.start().and(climberstick.y()).onTrue(getAutonomousCommand()) + //joystick.start().and(joystick.y()).onTrue(getAutonomousCommand()); // reset the field-centric heading on left bumper press // joystick.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 4527107..cc9e3f8 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -29,7 +29,8 @@ public enum ArmPosition { Loading(140, 78), L4_Score(102, 102), // 86 Algae_Score(100, 100), - Transient(108, 108); + Transient(108, 108), + Climbing(50, 50); double coralAngle, algaeAngle; From 96f1fc3ef327404126dba3ac1777c5805b04f2ff Mon Sep 17 00:00:00 2001 From: shlap Date: Wed, 26 Mar 2025 19:20:20 -0400 Subject: [PATCH 057/106] Changed some setpoints --- .../autos/{B-2Piece.auto => C-2Piece.auto} | 16 ++++++++-------- src/main/deploy/pathplanner/paths/A_R2.path | 8 ++++---- src/main/deploy/pathplanner/paths/A_R3.path | 8 ++++---- src/main/deploy/pathplanner/paths/A_R4.path | 8 ++++---- src/main/deploy/pathplanner/paths/B_R2.path | 8 ++++---- src/main/deploy/pathplanner/paths/B_R3.path | 8 ++++---- src/main/deploy/pathplanner/paths/B_R4.path | 8 ++++---- src/main/deploy/pathplanner/paths/B_R5.path | 8 ++++---- src/main/deploy/pathplanner/paths/BackR2.path | 8 ++++---- src/main/deploy/pathplanner/paths/BackR3.path | 8 ++++---- src/main/deploy/pathplanner/paths/BackR4.path | 8 ++++---- src/main/deploy/pathplanner/paths/BackR5.path | 8 ++++---- src/main/deploy/pathplanner/paths/BackR6.path | 8 ++++---- src/main/deploy/pathplanner/paths/C_R3.path | 8 ++++---- src/main/deploy/pathplanner/paths/C_R4.path | 8 ++++---- src/main/deploy/pathplanner/paths/C_R5.path | 8 ++++---- src/main/deploy/pathplanner/paths/C_R6.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP1_R2.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP1_R3.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP1_R4.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP1_R5.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP1_R6.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP2_R2.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP2_R3.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP2_R4.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP2_R5.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP2_R6.path | 8 ++++---- src/main/deploy/pathplanner/paths/PushScore.path | 8 ++++---- src/main/deploy/pathplanner/paths/R2_HP1.path | 8 ++++---- src/main/deploy/pathplanner/paths/R2_HP2.path | 8 ++++---- src/main/deploy/pathplanner/paths/R3_HP1.path | 8 ++++---- src/main/deploy/pathplanner/paths/R3_HP2.path | 8 ++++---- src/main/deploy/pathplanner/paths/R4_HP1.path | 8 ++++---- src/main/deploy/pathplanner/paths/R4_HP2.path | 8 ++++---- src/main/deploy/pathplanner/paths/R5_HP1.path | 8 ++++---- src/main/deploy/pathplanner/paths/R5_HP2.path | 8 ++++---- src/main/deploy/pathplanner/paths/R6_HP1.path | 8 ++++---- src/main/deploy/pathplanner/paths/R6_HP2.path | 8 ++++---- src/main/deploy/pathplanner/paths/ScoreR2.path | 8 ++++---- src/main/deploy/pathplanner/paths/ScoreR3.path | 8 ++++---- src/main/deploy/pathplanner/paths/ScoreR4.path | 8 ++++---- src/main/deploy/pathplanner/paths/ScoreR5.path | 8 ++++---- src/main/deploy/pathplanner/paths/ScoreR6.path | 8 ++++---- src/main/java/frc/robot/RobotContainer.java | 7 ++++++- 44 files changed, 182 insertions(+), 177 deletions(-) rename src/main/deploy/pathplanner/autos/{B-2Piece.auto => C-2Piece.auto} (100%) diff --git a/src/main/deploy/pathplanner/autos/B-2Piece.auto b/src/main/deploy/pathplanner/autos/C-2Piece.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/B-2Piece.auto rename to src/main/deploy/pathplanner/autos/C-2Piece.auto index ad19c1c..33670a7 100644 --- a/src/main/deploy/pathplanner/autos/B-2Piece.auto +++ b/src/main/deploy/pathplanner/autos/C-2Piece.auto @@ -9,15 +9,15 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "C_R5" + "name": "L4" } }, { - "type": "named", + "type": "path", "data": { - "name": "L4" + "pathName": "C_R5" } } ] @@ -53,15 +53,15 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "HP2_R6" + "name": "L4" } }, { - "type": "named", + "type": "path", "data": { - "name": "L4" + "pathName": "HP2_R6" } } ] diff --git a/src/main/deploy/pathplanner/paths/A_R2.path b/src/main/deploy/pathplanner/paths/A_R2.path index ecbe813..e252522 100644 --- a/src/main/deploy/pathplanner/paths/A_R2.path +++ b/src/main/deploy/pathplanner/paths/A_R2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.7, - "y": 5.067417254011322 + "x": 3.722528569965569, + "y": 4.991511920463674 }, "prevControl": { - "x": 3.9799180327868857, - "y": 6.117878319585092 + "x": 4.002446602752455, + "y": 6.041972986037444 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index aa36053..d2e77ef 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.0115, - "y": 5.204749999999999 + "x": 4.998872950819672, + "y": 5.181813524590163 }, "prevControl": { - "x": 6.112541230111333, - "y": 5.549023212606629 + "x": 6.099914180931005, + "y": 5.526086737196794 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/A_R4.path b/src/main/deploy/pathplanner/paths/A_R4.path index 949aa72..4f0ed62 100644 --- a/src/main/deploy/pathplanner/paths/A_R4.path +++ b/src/main/deploy/pathplanner/paths/A_R4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.694000000000001, - "y": 4.17125 + "x": 5.658196721311475, + "y": 4.198821721311475 }, "prevControl": { - "x": 6.303660199424143, - "y": 5.115587423812124 + "x": 6.267856920735617, + "y": 5.143159145123599 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/B_R2.path b/src/main/deploy/pathplanner/paths/B_R2.path index 55d9894..2467a94 100644 --- a/src/main/deploy/pathplanner/paths/B_R2.path +++ b/src/main/deploy/pathplanner/paths/B_R2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.7, - "y": 5.067417254011322 + "x": 3.722528569965569, + "y": 4.991511920463674 }, "prevControl": { - "x": 3.8360655737704916, - "y": 6.2617307786014855 + "x": 3.8585941437360605, + "y": 6.185825445053838 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/B_R3.path b/src/main/deploy/pathplanner/paths/B_R3.path index 250c6f2..d371ede 100644 --- a/src/main/deploy/pathplanner/paths/B_R3.path +++ b/src/main/deploy/pathplanner/paths/B_R3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.0115, - "y": 5.204749999999999 + "x": 4.998872950819672, + "y": 5.181813524590163 }, "prevControl": { - "x": 7.091722704080768, - "y": 4.222624963698702 + "x": 7.07909565490044, + "y": 4.199688488288866 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/B_R4.path b/src/main/deploy/pathplanner/paths/B_R4.path index 5ffbc21..97f04b6 100644 --- a/src/main/deploy/pathplanner/paths/B_R4.path +++ b/src/main/deploy/pathplanner/paths/B_R4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.694000000000001, - "y": 4.17125 + "x": 5.658196721311475, + "y": 4.198821721311475 }, "prevControl": { - "x": 6.438979508196723, - "y": 4.21218237704918 + "x": 6.403176229508197, + "y": 4.239754098360655 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/B_R5.path b/src/main/deploy/pathplanner/paths/B_R5.path index 6191c95..3c4b411 100644 --- a/src/main/deploy/pathplanner/paths/B_R5.path +++ b/src/main/deploy/pathplanner/paths/B_R5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.33452868852459, - "y": 2.9817499999999995 + "x": 5.238627049180328, + "y": 3.036014344262295 }, "prevControl": { - "x": 5.583513140708558, - "y": 2.959239056646833 + "x": 5.487611501364296, + "y": 3.0135034009091286 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BackR2.path b/src/main/deploy/pathplanner/paths/BackR2.path index 6f8b69e..ef0f5fe 100644 --- a/src/main/deploy/pathplanner/paths/BackR2.path +++ b/src/main/deploy/pathplanner/paths/BackR2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.7, - "y": 5.067417254011322 + "x": 3.722528569965569, + "y": 4.991511920463674 }, "prevControl": null, "nextControl": { - "x": 3.8766157187288273, - "y": 4.890479728602211 + "x": 3.899144288694396, + "y": 4.814574395054563 }, "isLocked": false, "linkedName": "ScoreR2" diff --git a/src/main/deploy/pathplanner/paths/BackR3.path b/src/main/deploy/pathplanner/paths/BackR3.path index a80a130..c266030 100644 --- a/src/main/deploy/pathplanner/paths/BackR3.path +++ b/src/main/deploy/pathplanner/paths/BackR3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.0115, - "y": 5.204749999999999 + "x": 4.998872950819672, + "y": 5.181813524590163 }, "prevControl": null, "nextControl": { - "x": 5.639996772870152, - "y": 5.677770651864003 + "x": 5.627369723689824, + "y": 5.654834176454168 }, "isLocked": false, "linkedName": "ScoreR3" diff --git a/src/main/deploy/pathplanner/paths/BackR4.path b/src/main/deploy/pathplanner/paths/BackR4.path index 3929d8c..47fe882 100644 --- a/src/main/deploy/pathplanner/paths/BackR4.path +++ b/src/main/deploy/pathplanner/paths/BackR4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.694000000000001, - "y": 4.17125 + "x": 5.658196721311475, + "y": 4.198821721311475 }, "prevControl": null, "nextControl": { - "x": 6.3997159041487, - "y": 3.9722441316677544 + "x": 6.363912625460174, + "y": 3.99981585297923 }, "isLocked": false, "linkedName": "ScoreR4" diff --git a/src/main/deploy/pathplanner/paths/BackR5.path b/src/main/deploy/pathplanner/paths/BackR5.path index 3b95bee..2c4f726 100644 --- a/src/main/deploy/pathplanner/paths/BackR5.path +++ b/src/main/deploy/pathplanner/paths/BackR5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.33452868852459, - "y": 2.9817499999999995 + "x": 5.238627049180328, + "y": 3.036014344262295 }, "prevControl": null, "nextControl": { - "x": 5.469767584510981, - "y": 2.7375562528045037 + "x": 5.3738659451667194, + "y": 2.791820597066799 }, "isLocked": false, "linkedName": "ScoreR5" diff --git a/src/main/deploy/pathplanner/paths/BackR6.path b/src/main/deploy/pathplanner/paths/BackR6.path index 38bcf22..bf89257 100644 --- a/src/main/deploy/pathplanner/paths/BackR6.path +++ b/src/main/deploy/pathplanner/paths/BackR6.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.9585000000000004, - "y": 2.7965 + "x": 4.039856557377049, + "y": 2.916137295081967 }, "prevControl": null, "nextControl": { - "x": 4.214913439492637, - "y": 2.8376815500697425 + "x": 4.296269996869686, + "y": 2.9573188451517094 }, "isLocked": false, "linkedName": "ScoreR6" diff --git a/src/main/deploy/pathplanner/paths/C_R3.path b/src/main/deploy/pathplanner/paths/C_R3.path index 883802b..56c00d0 100644 --- a/src/main/deploy/pathplanner/paths/C_R3.path +++ b/src/main/deploy/pathplanner/paths/C_R3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.0115, - "y": 5.204749999999999 + "x": 4.998872950819672, + "y": 5.181813524590163 }, "prevControl": { - "x": 5.994491803278688, - "y": 5.348602459016391 + "x": 5.9818647540983605, + "y": 5.325665983606555 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C_R4.path b/src/main/deploy/pathplanner/paths/C_R4.path index 70418a8..c68b571 100644 --- a/src/main/deploy/pathplanner/paths/C_R4.path +++ b/src/main/deploy/pathplanner/paths/C_R4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.694000000000001, - "y": 4.17125 + "x": 5.658196721311475, + "y": 4.198821721311475 }, "prevControl": { - "x": 5.836577979832988, - "y": 3.9650437461282673 + "x": 5.800774701144462, + "y": 3.9926154674397427 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index 197f51d..a68f584 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.33452868852459, - "y": 2.9817499999999995 + "x": 5.238627049180328, + "y": 3.036014344262295 }, "prevControl": { - "x": 5.468015810725064, - "y": 2.7703707479277213 + "x": 5.372114171380803, + "y": 2.8246350921900167 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C_R6.path b/src/main/deploy/pathplanner/paths/C_R6.path index e5add5f..7287b60 100644 --- a/src/main/deploy/pathplanner/paths/C_R6.path +++ b/src/main/deploy/pathplanner/paths/C_R6.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.9585000000000004, - "y": 2.7965 + "x": 4.039856557377049, + "y": 2.916137295081967 }, "prevControl": { - "x": 4.420243751108583, - "y": 2.592761318137971 + "x": 4.5016003084856315, + "y": 2.712398613219938 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP1_R2.path b/src/main/deploy/pathplanner/paths/HP1_R2.path index 8c73518..0e8f674 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R2.path +++ b/src/main/deploy/pathplanner/paths/HP1_R2.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.7, - "y": 5.067417254011322 + "x": 3.722528569965569, + "y": 4.991511920463674 }, "prevControl": { - "x": 3.864246218182973, - "y": 4.878941314399415 + "x": 3.886774788148542, + "y": 4.803035980851767 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP1_R3.path b/src/main/deploy/pathplanner/paths/HP1_R3.path index d7102ae..6a81af3 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R3.path +++ b/src/main/deploy/pathplanner/paths/HP1_R3.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 5.0115, - "y": 5.204749999999999 + "x": 4.998872950819672, + "y": 5.181813524590163 }, "prevControl": { - "x": 5.664749999999999, - "y": 5.77025 + "x": 5.652122950819671, + "y": 5.747313524590164 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP1_R4.path b/src/main/deploy/pathplanner/paths/HP1_R4.path index 8ea5ff4..0b1d0f9 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R4.path +++ b/src/main/deploy/pathplanner/paths/HP1_R4.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 5.694000000000001, - "y": 4.17125 + "x": 5.658196721311475, + "y": 4.198821721311475 }, "prevControl": { - "x": 7.1175, - "y": 4.063999999999999 + "x": 7.081696721311474, + "y": 4.091571721311475 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP1_R5.path b/src/main/deploy/pathplanner/paths/HP1_R5.path index ce95c4f..04da20c 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R5.path +++ b/src/main/deploy/pathplanner/paths/HP1_R5.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 5.33452868852459, - "y": 2.9817499999999995 + "x": 5.238627049180328, + "y": 3.036014344262295 }, "prevControl": { - "x": 5.762249999999999, - "y": 2.4844999999999993 + "x": 5.666348360655737, + "y": 2.5387643442622947 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP1_R6.path b/src/main/deploy/pathplanner/paths/HP1_R6.path index 5978ec5..5df3e4b 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R6.path +++ b/src/main/deploy/pathplanner/paths/HP1_R6.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.9585000000000004, - "y": 2.7965 + "x": 4.039856557377049, + "y": 2.916137295081967 }, "prevControl": { - "x": 3.315, - "y": 1.88 + "x": 3.3963565573770484, + "y": 1.9996372950819667 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R2.path b/src/main/deploy/pathplanner/paths/HP2_R2.path index 9b8ba00..9cfb7dd 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R2.path +++ b/src/main/deploy/pathplanner/paths/HP2_R2.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.7, - "y": 5.067417254011322 + "x": 3.722528569965569, + "y": 4.991511920463674 }, "prevControl": { - "x": 3.16875, - "y": 5.82875 + "x": 3.191278569965569, + "y": 5.7528446664523525 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R3.path b/src/main/deploy/pathplanner/paths/HP2_R3.path index 130e52d..beab262 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R3.path +++ b/src/main/deploy/pathplanner/paths/HP2_R3.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 5.0115, - "y": 5.204749999999999 + "x": 4.998872950819672, + "y": 5.181813524590163 }, "prevControl": { - "x": 5.421, - "y": 6.3454999999999995 + "x": 5.408372950819673, + "y": 6.322563524590164 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R4.path b/src/main/deploy/pathplanner/paths/HP2_R4.path index 66901b5..e0d239b 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R4.path +++ b/src/main/deploy/pathplanner/paths/HP2_R4.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 5.694000000000001, - "y": 4.17125 + "x": 5.658196721311475, + "y": 4.198821721311475 }, "prevControl": { - "x": 7.244249999999999, - "y": 4.063999999999999 + "x": 7.208446721311473, + "y": 4.091571721311475 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R5.path b/src/main/deploy/pathplanner/paths/HP2_R5.path index fd90c49..8cec311 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R5.path +++ b/src/main/deploy/pathplanner/paths/HP2_R5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.33452868852459, - "y": 2.9817499999999995 + "x": 5.238627049180328, + "y": 3.036014344262295 }, "prevControl": { - "x": 6.1034999999999995, - "y": 2.094499999999999 + "x": 6.007598360655738, + "y": 2.1487643442622946 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R6.path b/src/main/deploy/pathplanner/paths/HP2_R6.path index bdd2325..85b44ae 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R6.path +++ b/src/main/deploy/pathplanner/paths/HP2_R6.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.9585000000000004, - "y": 2.7965 + "x": 4.039856557377049, + "y": 2.916137295081967 }, "prevControl": { - "x": 3.7426361215290584, - "y": 2.669817961595553 + "x": 3.823992678906107, + "y": 2.78945525667752 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/PushScore.path b/src/main/deploy/pathplanner/paths/PushScore.path index a46b60d..bd78b8e 100644 --- a/src/main/deploy/pathplanner/paths/PushScore.path +++ b/src/main/deploy/pathplanner/paths/PushScore.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 5.694000000000001, - "y": 4.17125 + "x": 5.658196721311475, + "y": 4.198821721311475 }, "prevControl": { - "x": 6.2010000000000005, - "y": 4.103 + "x": 6.1651967213114744, + "y": 4.130571721311475 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R2_HP1.path b/src/main/deploy/pathplanner/paths/R2_HP1.path index 2717e56..82b545e 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP1.path +++ b/src/main/deploy/pathplanner/paths/R2_HP1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.7, - "y": 5.067417254011322 + "x": 3.722528569965569, + "y": 4.991511920463674 }, "prevControl": null, "nextControl": { - "x": 3.3930000000000002, - "y": 5.504667254011322 + "x": 3.415528569965569, + "y": 5.428761920463674 }, "isLocked": true, "linkedName": "ScoreR2" diff --git a/src/main/deploy/pathplanner/paths/R2_HP2.path b/src/main/deploy/pathplanner/paths/R2_HP2.path index 89b052b..c24c178 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP2.path +++ b/src/main/deploy/pathplanner/paths/R2_HP2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.7, - "y": 5.067417254011322 + "x": 3.722528569965569, + "y": 4.991511920463674 }, "prevControl": null, "nextControl": { - "x": 3.1005000000000003, - "y": 5.9945 + "x": 3.123028569965569, + "y": 5.9185946664523525 }, "isLocked": false, "linkedName": "ScoreR2" diff --git a/src/main/deploy/pathplanner/paths/R3_HP1.path b/src/main/deploy/pathplanner/paths/R3_HP1.path index c017bb8..f4b139a 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/R3_HP1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.0115, - "y": 5.204749999999999 + "x": 4.998872950819672, + "y": 5.181813524590163 }, "prevControl": null, "nextControl": { - "x": 6.005999999999999, - "y": 6.3065 + "x": 5.993372950819672, + "y": 6.283563524590164 }, "isLocked": false, "linkedName": "ScoreR3" diff --git a/src/main/deploy/pathplanner/paths/R3_HP2.path b/src/main/deploy/pathplanner/paths/R3_HP2.path index 1c71048..7be6591 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP2.path +++ b/src/main/deploy/pathplanner/paths/R3_HP2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.0115, - "y": 5.204749999999999 + "x": 4.998872950819672, + "y": 5.181813524590163 }, "prevControl": null, "nextControl": { - "x": 5.742750000000001, - "y": 6.2675 + "x": 5.730122950819673, + "y": 6.2445635245901645 }, "isLocked": false, "linkedName": "ScoreR3" diff --git a/src/main/deploy/pathplanner/paths/R4_HP1.path b/src/main/deploy/pathplanner/paths/R4_HP1.path index fa41e43..4e8e664 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP1.path +++ b/src/main/deploy/pathplanner/paths/R4_HP1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.694000000000001, - "y": 4.17125 + "x": 5.658196721311475, + "y": 4.198821721311475 }, "prevControl": null, "nextControl": { - "x": 7.0005, - "y": 4.2395 + "x": 6.964696721311474, + "y": 4.267071721311475 }, "isLocked": false, "linkedName": "ScoreR4" diff --git a/src/main/deploy/pathplanner/paths/R4_HP2.path b/src/main/deploy/pathplanner/paths/R4_HP2.path index 0722e7e..b702f39 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP2.path +++ b/src/main/deploy/pathplanner/paths/R4_HP2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.694000000000001, - "y": 4.17125 + "x": 5.658196721311475, + "y": 4.198821721311475 }, "prevControl": null, "nextControl": { - "x": 7.06875, - "y": 4.2395 + "x": 7.032946721311474, + "y": 4.267071721311475 }, "isLocked": false, "linkedName": "ScoreR4" diff --git a/src/main/deploy/pathplanner/paths/R5_HP1.path b/src/main/deploy/pathplanner/paths/R5_HP1.path index 774e704..ab001d7 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP1.path +++ b/src/main/deploy/pathplanner/paths/R5_HP1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.33452868852459, - "y": 2.9817499999999995 + "x": 5.238627049180328, + "y": 3.036014344262295 }, "prevControl": null, "nextControl": { - "x": 5.733, - "y": 2.4454999999999996 + "x": 5.637098360655738, + "y": 2.499764344262295 }, "isLocked": false, "linkedName": "ScoreR5" diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index e095879..5d819fb 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.33452868852459, - "y": 2.9817499999999995 + "x": 5.238627049180328, + "y": 3.036014344262295 }, "prevControl": null, "nextControl": { - "x": 6.24, - "y": 1.938499999999999 + "x": 6.144098360655739, + "y": 1.9927643442622944 }, "isLocked": false, "linkedName": "ScoreR5" diff --git a/src/main/deploy/pathplanner/paths/R6_HP1.path b/src/main/deploy/pathplanner/paths/R6_HP1.path index d13facf..1c85dfc 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP1.path +++ b/src/main/deploy/pathplanner/paths/R6_HP1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.9585000000000004, - "y": 2.7965 + "x": 4.039856557377049, + "y": 2.916137295081967 }, "prevControl": null, "nextControl": { - "x": 3.6270000000000007, - "y": 1.88 + "x": 3.708356557377049, + "y": 1.9996372950819667 }, "isLocked": false, "linkedName": "ScoreR6" diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index fa0b23c..96bccac 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.9585000000000004, - "y": 2.7965 + "x": 4.039856557377049, + "y": 2.916137295081967 }, "prevControl": null, "nextControl": { - "x": 3.4898555412247556, - "y": 2.2981636033703956 + "x": 3.571212098601804, + "y": 2.4178008984523625 }, "isLocked": false, "linkedName": "ScoreR6" diff --git a/src/main/deploy/pathplanner/paths/ScoreR2.path b/src/main/deploy/pathplanner/paths/ScoreR2.path index 810fb09..73347d0 100644 --- a/src/main/deploy/pathplanner/paths/ScoreR2.path +++ b/src/main/deploy/pathplanner/paths/ScoreR2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.7, - "y": 5.067417254011322 + "x": 3.722528569965569, + "y": 4.991511920463674 }, "prevControl": { - "x": 3.7006651920836937, - "y": 5.31741636904874 + "x": 3.7231937620492626, + "y": 5.241511035501092 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/ScoreR3.path b/src/main/deploy/pathplanner/paths/ScoreR3.path index f6c86cd..dd1ab75 100644 --- a/src/main/deploy/pathplanner/paths/ScoreR3.path +++ b/src/main/deploy/pathplanner/paths/ScoreR3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.0115, - "y": 5.204749999999999 + "x": 4.998872950819672, + "y": 5.181813524590163 }, "prevControl": { - "x": 5.204482558904262, - "y": 5.363676813215275 + "x": 5.1918555097239345, + "y": 5.340740337805439 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/ScoreR4.path b/src/main/deploy/pathplanner/paths/ScoreR4.path index ce1362e..1315acf 100644 --- a/src/main/deploy/pathplanner/paths/ScoreR4.path +++ b/src/main/deploy/pathplanner/paths/ScoreR4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.694000000000001, - "y": 4.17125 + "x": 5.658196721311475, + "y": 4.198821721311475 }, "prevControl": { - "x": 5.9397180467335815, - "y": 4.125177866237452 + "x": 5.903914768045055, + "y": 4.152749587548928 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/ScoreR5.path b/src/main/deploy/pathplanner/paths/ScoreR5.path index 92a6843..e634718 100644 --- a/src/main/deploy/pathplanner/paths/ScoreR5.path +++ b/src/main/deploy/pathplanner/paths/ScoreR5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.33452868852459, - "y": 2.9817499999999995 + "x": 5.238627049180328, + "y": 3.036014344262295 }, "prevControl": { - "x": 5.580881831494815, - "y": 2.8335505578856015 + "x": 5.484980192150553, + "y": 2.887814902147897 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/ScoreR6.path b/src/main/deploy/pathplanner/paths/ScoreR6.path index 5988b73..e0ef623 100644 --- a/src/main/deploy/pathplanner/paths/ScoreR6.path +++ b/src/main/deploy/pathplanner/paths/ScoreR6.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.9585000000000004, - "y": 2.7965 + "x": 4.039856557377049, + "y": 2.916137295081967 }, "prevControl": { - "x": 3.7316487075937057, - "y": 2.69143577614814 + "x": 3.813005264970754, + "y": 2.8110730712301066 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 16371c7..b00468a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -84,6 +84,7 @@ public class RobotContainer { private final Vision vision; private final CommandXboxController joystick = new CommandXboxController(0); + private final CommandXboxController climberstick = new CommandXboxController(3); private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private double MaxAngularRate = RotationsPerSecond.of(0.5).in(RadiansPerSecond); // 1/2 of a rotation per second max @@ -260,7 +261,11 @@ private void configureBindings() { () -> -joystick.getRightX())); - //joystick.start().and(joystick.y()).onTrue(getAutonomousCommand()); + //climberstick.start().and(climberstick.y()).onTrue(getAutonomousCommand()); + + climber.setDefaultCommand( + ClimberCommands.runClimber( + () -> climberstick.getLeftY())); // reset the field-centric heading on left bumper press // joystick.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); From 5a73a2d420937978405f0fb23182fea748f848c6 Mon Sep 17 00:00:00 2001 From: shlap Date: Wed, 26 Mar 2025 19:27:40 -0400 Subject: [PATCH 058/106] i am speed --- .../autos/{C-2Piece.auto => C-2.5Piece.auto} | 0 src/main/deploy/pathplanner/autos/C-3P0.auto | 51 +++++++++++++++++++ src/main/deploy/pathplanner/paths/A_R1.path | 4 +- src/main/deploy/pathplanner/paths/B_R5.path | 2 +- src/main/deploy/pathplanner/paths/C_R5.path | 4 +- src/main/deploy/pathplanner/paths/C_R6.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R5.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R6.path | 4 +- src/main/deploy/pathplanner/paths/R5_HP2.path | 4 +- src/main/deploy/pathplanner/paths/R6_HP2.path | 4 +- 10 files changed, 66 insertions(+), 15 deletions(-) rename src/main/deploy/pathplanner/autos/{C-2Piece.auto => C-2.5Piece.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/C-3P0.auto diff --git a/src/main/deploy/pathplanner/autos/C-2Piece.auto b/src/main/deploy/pathplanner/autos/C-2.5Piece.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/C-2Piece.auto rename to src/main/deploy/pathplanner/autos/C-2.5Piece.auto diff --git a/src/main/deploy/pathplanner/autos/C-3P0.auto b/src/main/deploy/pathplanner/autos/C-3P0.auto new file mode 100644 index 0000000..d6814b2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/C-3P0.auto @@ -0,0 +1,51 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "C_R5" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "R5_HP2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A_R1.path b/src/main/deploy/pathplanner/paths/A_R1.path index 316b0f2..96197e9 100644 --- a/src/main/deploy/pathplanner/paths/A_R1.path +++ b/src/main/deploy/pathplanner/paths/A_R1.path @@ -59,7 +59,7 @@ "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -71,5 +71,5 @@ "velocity": 0.0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B_R5.path b/src/main/deploy/pathplanner/paths/B_R5.path index 3c4b411..e257ca0 100644 --- a/src/main/deploy/pathplanner/paths/B_R5.path +++ b/src/main/deploy/pathplanner/paths/B_R5.path @@ -55,5 +55,5 @@ "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index a68f584..2dbe508 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -43,7 +43,7 @@ "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -55,5 +55,5 @@ "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C_R6.path b/src/main/deploy/pathplanner/paths/C_R6.path index 7287b60..e1442c3 100644 --- a/src/main/deploy/pathplanner/paths/C_R6.path +++ b/src/main/deploy/pathplanner/paths/C_R6.path @@ -59,7 +59,7 @@ "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -71,5 +71,5 @@ "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP2_R5.path b/src/main/deploy/pathplanner/paths/HP2_R5.path index 8cec311..fac2eae 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R5.path +++ b/src/main/deploy/pathplanner/paths/HP2_R5.path @@ -43,7 +43,7 @@ "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -55,5 +55,5 @@ "velocity": 0, "rotation": 53.8 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP2_R6.path b/src/main/deploy/pathplanner/paths/HP2_R6.path index 85b44ae..2c490c7 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R6.path +++ b/src/main/deploy/pathplanner/paths/HP2_R6.path @@ -43,7 +43,7 @@ "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -55,5 +55,5 @@ "velocity": 0, "rotation": 53.8 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index 5d819fb..9c16724 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -59,7 +59,7 @@ "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -71,5 +71,5 @@ "velocity": 0, "rotation": 120.9 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index 96bccac..89b4bed 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -38,7 +38,7 @@ "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 59.99999999999999 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file From adc050624266555ff9512c1c63e5b4a43cd0446d Mon Sep 17 00:00:00 2001 From: shlap Date: Wed, 26 Mar 2025 19:32:34 -0400 Subject: [PATCH 059/106] i am not sped --- src/main/deploy/pathplanner/paths/C_R5.path | 6 +++--- src/main/deploy/pathplanner/paths/HP2_R4.path | 4 ++-- src/main/deploy/pathplanner/paths/HP2_R5.path | 10 +++++----- src/main/deploy/pathplanner/paths/HP2_R6.path | 6 +++--- src/main/deploy/pathplanner/paths/R5_HP2.path | 8 ++++---- src/main/deploy/pathplanner/paths/R6_HP2.path | 10 +++++----- 6 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index 2dbe508..5b6b0bd 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -38,12 +38,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.0, + "maxVelocity": 10.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R4.path b/src/main/deploy/pathplanner/paths/HP2_R4.path index e0d239b..1bd7658 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R4.path +++ b/src/main/deploy/pathplanner/paths/HP2_R4.path @@ -59,7 +59,7 @@ "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -71,5 +71,5 @@ "velocity": 0, "rotation": 53.8 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP2_R5.path b/src/main/deploy/pathplanner/paths/HP2_R5.path index fac2eae..eced67f 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R5.path +++ b/src/main/deploy/pathplanner/paths/HP2_R5.path @@ -38,12 +38,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 800.0, + "maxAngularAcceleration": 800.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R6.path b/src/main/deploy/pathplanner/paths/HP2_R6.path index 2c490c7..6ee0b20 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R6.path +++ b/src/main/deploy/pathplanner/paths/HP2_R6.path @@ -38,12 +38,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.0, + "maxVelocity": 10.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index 9c16724..3ac082b 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -54,12 +54,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.0, + "maxVelocity": 10.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 800.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index 89b4bed..9b03a00 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -33,12 +33,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 800.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, From 7e222eec33e82c5bed8d756f4ec5ac9dedbdf19c Mon Sep 17 00:00:00 2001 From: shlap Date: Wed, 26 Mar 2025 20:08:47 -0400 Subject: [PATCH 060/106] Climber button setup --- src/main/java/frc/robot/RobotContainer.java | 7 ++++++- .../multisubsystemcommands/MultiSubsystemCommands.java | 7 ++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b00468a..a435b63 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -303,13 +303,18 @@ private void configureBindings() { joystickApproach( () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.LEFT))); - // a -button approach reef joystick.a() .whileTrue( joystickApproach( () -> FieldConstants.getNearestReefFace(drive.getPose()))); + // Set up robot for climb + climberstick.x() + .onTrue( + armCommands.moveArm(ArmPosition.Climbing) + .andThen(elevatorCommands.moveElevator(ElevatorPosition.Stow))); + manualIntakeButton.whileTrue(armCommands.manualIntake()); incrementElevatorButton.onTrue(elevatorCommands.incrementElevatorPosition()); diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 9cebf6c..6806edc 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.multisubsystemcommands; +import java.util.function.DoubleSupplier; import java.util.function.Supplier; import edu.wpi.first.wpilibj2.command.Command; @@ -8,6 +9,8 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.Arm.ArmPosition; +import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.climber.ClimberCommands; import frc.robot.subsystems.arm.ArmCommands; import frc.robot.subsystems.elevator.Elevator; import frc.robot.subsystems.elevator.ElevatorCommands; @@ -48,8 +51,10 @@ public enum GamepieceMode { private Elevator _elevator; private Arm _arm; + private Climber _climber; private ElevatorCommands _elevatorCommands; private ArmCommands _armCommands; + private ClimberCommands _climberCommands; public MultiSubsystemCommands(Elevator elevator, Arm arm, ElevatorCommands elevatorCommands, ArmCommands armCommands) { @@ -92,5 +97,5 @@ public Command loadAlgae(OverallPosition position) { .alongWith(_armCommands.intake()) .andThen(_armCommands.setArmPosition(ArmPosition.Stow)); } - + } \ No newline at end of file From dfc5018daa8b2f1776520ad91bb3dffc7bcff39f Mon Sep 17 00:00:00 2001 From: area5188 Date: Thu, 27 Mar 2025 19:15:25 -0400 Subject: [PATCH 061/106] at hatchet house --- src/main/java/frc/robot/subsystems/climber/ClimberCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java index 5474c0f..7b99f2a 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java @@ -17,6 +17,6 @@ public Command runClimber(DoubleSupplier ySupplier) { () -> { _climber.setClimberSpeed(ySupplier.getAsDouble()); }, - _climber); + _climber).unless(() -> (ySupplier.getAsDouble() < 0.05)); } } \ No newline at end of file From 4b1b28a20f861b469c68eae745ddd32d06848de4 Mon Sep 17 00:00:00 2001 From: area5188 Date: Thu, 27 Mar 2025 20:42:29 -0400 Subject: [PATCH 062/106] increased coral spit speed and duration --- src/main/java/frc/robot/subsystems/arm/Arm.java | 2 +- src/main/java/frc/robot/subsystems/arm/ArmCommands.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index cc9e3f8..9b5de6d 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -96,7 +96,7 @@ public void setIntakeSpeed(double speed) { } public void spit() { - double speed = (_currentMode == GamepieceMode.ALGAE) ? -0.5 : 0.5; + double speed = (_currentMode == GamepieceMode.ALGAE) ? -0.5 : 0.8; setIntakeSpeed(speed); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index e5447b1..5d26839 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -28,7 +28,7 @@ public Command spit() { () -> { _arm.setIntakeSpeed(0); _arm.clearHasGamepiece(); - }, _arm).withTimeout(0.3) + }, _arm).withTimeout(0.5) .andThen(Commands.runOnce(() -> _arm.setArmSetpoint(ArmPosition.Stow), _arm).unless(() -> _arm.getCurrentPos() != ArmPosition.L4_Score)); From 3464a220a8912bf95f31f350611483ad8729fe34 Mon Sep 17 00:00:00 2001 From: area5188 Date: Thu, 27 Mar 2025 20:48:09 -0400 Subject: [PATCH 063/106] remove climber =))) --- src/main/java/frc/robot/RobotContainer.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a435b63..065de0b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,8 +36,8 @@ import frc.robot.subsystems.arm.ArmCommands; import frc.robot.subsystems.arm.Arm.ArmPosition; import frc.robot.subsystems.arm.io.RealArmIO; -import frc.robot.subsystems.climber.Climber; -import frc.robot.subsystems.climber.ClimberCommands; +// import frc.robot.subsystems.climber.Climber; +// import frc.robot.subsystems.climber.ClimberCommands; import frc.robot.subsystems.climber.io.RealClimberIO; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveCommands; @@ -76,8 +76,8 @@ public class RobotContainer { private final ArmCommands armCommands = new ArmCommands(armSubsystem); private final LEDsCommands LEDCommands = new LEDsCommands(LEDSubsystem); - private final Climber climber = new Climber(new RealClimberIO()); - private final ClimberCommands ClimberCommands = new ClimberCommands(climber); + // private final Climber climber = new Climber(new RealClimberIO()); + // private final ClimberCommands ClimberCommands = new ClimberCommands(climber); private final MultiSubsystemCommands multiSubsystemCommands = new MultiSubsystemCommands(elevatorSubsystem, armSubsystem, elevatorCommands, armCommands); @@ -263,9 +263,9 @@ private void configureBindings() { //climberstick.start().and(climberstick.y()).onTrue(getAutonomousCommand()); - climber.setDefaultCommand( - ClimberCommands.runClimber( - () -> climberstick.getLeftY())); + // climber.setDefaultCommand( + // ClimberCommands.runClimber( + // () -> climberstick.getLeftY())); // reset the field-centric heading on left bumper press // joystick.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); From 8ef944f3e5e470e25b914ac5cae821e3067a5054 Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Fri, 28 Mar 2025 08:38:34 -0400 Subject: [PATCH 064/106] added ability to reject tags not on reef --- src/main/java/frc/robot/RobotContainer.java | 3 +-- .../frc/robot/subsystems/vision/Vision.java | 19 ++++++++++++++++--- .../subsystems/vision/VisionConstants.java | 6 ++++++ 3 files changed, 23 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 065de0b..55e282b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -143,8 +143,7 @@ public RobotContainer() { vision = new Vision( drive::addVisionMeasurement, - new VisionIOPhotonVision(camera5Name, robotToCamera5), - new VisionIOPhotonVision(camera6Name, robotToCamera6) + new VisionIOPhotonVision(camera5Name, robotToCamera5) ); break; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 07e3d99..b379897 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -32,6 +32,7 @@ import java.util.LinkedList; import java.util.List; import org.littletonrobotics.junction.Logger; +import java.util.List; public class Vision extends SubsystemBase { @@ -39,6 +40,8 @@ public class Vision extends SubsystemBase { private final VisionIO[] io; private final VisionIOInputsAutoLogged[] inputs; private final Alert[] disconnectedAlerts; + public boolean visionHasTarget = false; + private boolean seesThisTarget = false; public Vision(VisionConsumer consumer, VisionIO... io) { this.consumer = consumer; @@ -95,9 +98,19 @@ public void periodic() { // Add tag poses for (int tagId : inputs[cameraIndex].tagIds) { var tagPose = aprilTagLayout.getTagPose(tagId); - if (tagPose.isPresent()) { - tagPoses.add(tagPose.get()); - } + if (tagPose.isPresent() && !rejectedTags.contains(tagId)) { + tagPoses.add(tagPose.get()); + seesThisTarget = true; + } + } + + // Report to visionhas Target whether or not vision sees at least one tag + if (seesThisTarget) { + visionHasTarget = true; + // Now reset seesThisTarget for next periodic loop + seesThisTarget = false; + } else { + visionHasTarget = false; } // Loop over pose observations diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index b662b17..2c116fa 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -21,6 +21,8 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.util.Units; +import java.util.List; +import java.util.Arrays; public class VisionConstants { // AprilTag layout @@ -82,6 +84,10 @@ public class VisionConstants { 1.0 // Camera 8 }; + + // List of tags that can be rejected if we want to align solely on the reef + public static List rejectedTags = Arrays.asList(2, 3, 4, 5, 14, 15, 16); + // Multipliers to apply for MegaTag 2 observations public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve public static double angularStdDevMegatag2Factor = From 884fce0f4a04aae2f5dabdefeddef9998228f6d7 Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 28 Mar 2025 08:44:01 -0400 Subject: [PATCH 065/106] Auto changes --- .../deploy/pathplanner/autos/A-3PieceL4.auto | 139 ++++++++++++++++++ src/main/deploy/pathplanner/autos/A_R3.auto | 25 ---- src/main/deploy/pathplanner/autos/C-3P0.auto | 88 +++++++++++ .../{C-2.5Piece.auto => C-3PieceL4.auto} | 38 +++++ src/main/deploy/pathplanner/paths/A_R3.path | 10 +- src/main/deploy/pathplanner/paths/R3_HP1.path | 18 +-- 6 files changed, 279 insertions(+), 39 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/A-3PieceL4.auto delete mode 100644 src/main/deploy/pathplanner/autos/A_R3.auto rename src/main/deploy/pathplanner/autos/{C-2.5Piece.auto => C-3PieceL4.auto} (71%) diff --git a/src/main/deploy/pathplanner/autos/A-3PieceL4.auto b/src/main/deploy/pathplanner/autos/A-3PieceL4.auto new file mode 100644 index 0000000..f793cf1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-3PieceL4.auto @@ -0,0 +1,139 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A_R3" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "R3_HP1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HP1_R2" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "R2_HP1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HP1_R1" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A_R3.auto b/src/main/deploy/pathplanner/autos/A_R3.auto deleted file mode 100644 index 055c9e7..0000000 --- a/src/main/deploy/pathplanner/autos/A_R3.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "A_R3" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/C-3P0.auto b/src/main/deploy/pathplanner/autos/C-3P0.auto index d6814b2..931842f 100644 --- a/src/main/deploy/pathplanner/autos/C-3P0.auto +++ b/src/main/deploy/pathplanner/autos/C-3P0.auto @@ -23,6 +23,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Score" + } + }, { "type": "parallel", "data": { @@ -41,6 +47,88 @@ } ] } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "HP2_R6" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "R6_HP2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HP2_R6" + } + }, + { + "type": "named", + "data": { + "name": "L1" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } } ] } diff --git a/src/main/deploy/pathplanner/autos/C-2.5Piece.auto b/src/main/deploy/pathplanner/autos/C-3PieceL4.auto similarity index 71% rename from src/main/deploy/pathplanner/autos/C-2.5Piece.auto rename to src/main/deploy/pathplanner/autos/C-3PieceL4.auto index 33670a7..a950447 100644 --- a/src/main/deploy/pathplanner/autos/C-2.5Piece.auto +++ b/src/main/deploy/pathplanner/autos/C-3PieceL4.auto @@ -91,6 +91,44 @@ } ] } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HP2_R1" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } } ] } diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index d2e77ef..28a5e54 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -20,8 +20,8 @@ "y": 5.181813524590163 }, "prevControl": { - "x": 6.099914180931005, - "y": 5.526086737196794 + "x": 5.658196721311475, + "y": 5.529456967213114 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.0, + "maxVelocity": 10.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0.0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R3_HP1.path b/src/main/deploy/pathplanner/paths/R3_HP1.path index f4b139a..c4e0f71 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/R3_HP1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.993372950819672, - "y": 6.283563524590164 + "x": 5.778073770491804, + "y": 6.320645491803279 }, "isLocked": false, "linkedName": "ScoreR3" @@ -20,12 +20,12 @@ "y": 6.779 }, "prevControl": { - "x": 3.37130174471824, - "y": 6.250028168786556 + "x": 3.380532786885246, + "y": 6.728227459016393 }, "nextControl": { - "x": 2.597028356788558, - "y": 7.200784742085265 + "x": 2.400148726044413, + "y": 6.841352720966541 }, "isLocked": false, "linkedName": null @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.0, + "maxVelocity": 10.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -71,5 +71,5 @@ "velocity": 0, "rotation": -121.5 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file From 312ec6e4f9762b062a05e0c43742b7cce6b8668e Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 28 Mar 2025 09:27:46 -0400 Subject: [PATCH 066/106] i slow --- .../deploy/pathplanner/autos/B-Score.auto | 38 +++++++++++++++++++ src/main/deploy/pathplanner/paths/A_R3.path | 2 +- src/main/deploy/pathplanner/paths/C_R5.path | 2 +- src/main/deploy/pathplanner/paths/HP1_R2.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R5.path | 2 +- src/main/deploy/pathplanner/paths/HP2_R6.path | 2 +- src/main/deploy/pathplanner/paths/R2_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R3_HP1.path | 2 +- src/main/deploy/pathplanner/paths/R5_HP2.path | 2 +- src/main/deploy/pathplanner/paths/R6_HP2.path | 2 +- 10 files changed, 49 insertions(+), 11 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/B-Score.auto diff --git a/src/main/deploy/pathplanner/autos/B-Score.auto b/src/main/deploy/pathplanner/autos/B-Score.auto new file mode 100644 index 0000000..334e6ae --- /dev/null +++ b/src/main/deploy/pathplanner/autos/B-Score.auto @@ -0,0 +1,38 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "B_R4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index 28a5e54..ebd27c7 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index 5b6b0bd..19ce3a9 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -39,7 +39,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R2.path b/src/main/deploy/pathplanner/paths/HP1_R2.path index 0e8f674..ef93839 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R2.path +++ b/src/main/deploy/pathplanner/paths/HP1_R2.path @@ -49,7 +49,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 10.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -66,5 +66,5 @@ "velocity": 0, "rotation": -53.8 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP2_R5.path b/src/main/deploy/pathplanner/paths/HP2_R5.path index eced67f..f891c9c 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R5.path +++ b/src/main/deploy/pathplanner/paths/HP2_R5.path @@ -39,7 +39,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 800.0, "maxAngularAcceleration": 800.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R6.path b/src/main/deploy/pathplanner/paths/HP2_R6.path index 6ee0b20..5df6442 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R6.path +++ b/src/main/deploy/pathplanner/paths/HP2_R6.path @@ -39,7 +39,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R2_HP1.path b/src/main/deploy/pathplanner/paths/R2_HP1.path index 82b545e..001fcf2 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP1.path +++ b/src/main/deploy/pathplanner/paths/R2_HP1.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 10.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -59.5 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R3_HP1.path b/src/main/deploy/pathplanner/paths/R3_HP1.path index c4e0f71..3afbede 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/R3_HP1.path @@ -55,7 +55,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index 3ac082b..2f87d94 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -55,7 +55,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 800.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index 9b03a00..bb49b7c 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 800.0, "nominalVoltage": 12.0, From fc8f33ab633c61543c0e0069b7b3a45d14532185 Mon Sep 17 00:00:00 2001 From: area5188 Date: Fri, 28 Mar 2025 10:12:33 -0400 Subject: [PATCH 067/106] adjust setpoints --- src/main/java/frc/robot/subsystems/arm/Arm.java | 4 ++-- src/main/java/frc/robot/subsystems/elevator/Elevator.java | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 9b5de6d..d692c27 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -27,7 +27,7 @@ public class Arm extends SubsystemBase { public enum ArmPosition { Stow(114, 114), Loading(140, 78), - L4_Score(102, 102), // 86 + L4_Score(100, 102), // 86 Algae_Score(100, 100), Transient(108, 108), Climbing(50, 50); @@ -96,7 +96,7 @@ public void setIntakeSpeed(double speed) { } public void spit() { - double speed = (_currentMode == GamepieceMode.ALGAE) ? -0.5 : 0.8; + double speed = (_currentMode == GamepieceMode.ALGAE) ? -0.5 : 0.5; setIntakeSpeed(speed); } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index b9b3f29..a1bdccf 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -34,10 +34,10 @@ public class Elevator extends SubsystemBase { public enum ElevatorPosition { - L1(5, 5), - L2(11, 18), - L3(27, 36), - L4(50.75, 50.75), + L1(7, 5), + L2(13, 18), + L3(29, 36), + L4(52, 50), Stow(0.5, 0.5); double coralHeight, algaeHeight; From 5360689cf06104ee7a140974da16cf4a6732c5ce Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 28 Mar 2025 12:15:59 -0400 Subject: [PATCH 068/106] hp1 --- src/main/deploy/pathplanner/paths/HP1_R1.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP1_R2.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP1_R3.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP1_R4.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP1_R5.path | 8 ++++---- src/main/deploy/pathplanner/paths/HP1_R6.path | 8 ++++---- src/main/deploy/pathplanner/paths/R1_HP1.path | 8 ++++---- src/main/deploy/pathplanner/paths/R2_HP1.path | 12 ++++++------ src/main/deploy/pathplanner/paths/R3_HP1.path | 8 ++++---- src/main/deploy/pathplanner/paths/R4_HP1.path | 8 ++++---- src/main/deploy/pathplanner/paths/R5_HP1.path | 8 ++++---- src/main/deploy/pathplanner/paths/R6_HP1.path | 8 ++++---- 12 files changed, 50 insertions(+), 50 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/HP1_R1.path b/src/main/deploy/pathplanner/paths/HP1_R1.path index b37bd77..b3b53cb 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R1.path +++ b/src/main/deploy/pathplanner/paths/HP1_R1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.247, - "y": 6.779 + "x": 1.1505, + "y": 7.008500000000001 }, "prevControl": null, "nextControl": { - "x": 2.3313267046615067, - "y": 6.710013936271338 + "x": 2.234826704661507, + "y": 6.9395139362713385 }, "isLocked": false, "linkedName": "HP1" diff --git a/src/main/deploy/pathplanner/paths/HP1_R2.path b/src/main/deploy/pathplanner/paths/HP1_R2.path index ef93839..a0ecd5c 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R2.path +++ b/src/main/deploy/pathplanner/paths/HP1_R2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.247, - "y": 6.779 + "x": 1.1505, + "y": 7.008500000000001 }, "prevControl": null, "nextControl": { - "x": 1.4879523599452857, - "y": 6.712351967494925 + "x": 1.3914523599452857, + "y": 6.941851967494926 }, "isLocked": false, "linkedName": "HP1" diff --git a/src/main/deploy/pathplanner/paths/HP1_R3.path b/src/main/deploy/pathplanner/paths/HP1_R3.path index 6a81af3..78d3d4d 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R3.path +++ b/src/main/deploy/pathplanner/paths/HP1_R3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.247, - "y": 6.779 + "x": 1.1505, + "y": 7.008500000000001 }, "prevControl": null, "nextControl": { - "x": 1.5110523892007985, - "y": 6.7071813828893925 + "x": 1.4145523892007985, + "y": 6.936681382889393 }, "isLocked": false, "linkedName": "HP1" diff --git a/src/main/deploy/pathplanner/paths/HP1_R4.path b/src/main/deploy/pathplanner/paths/HP1_R4.path index 0b1d0f9..2adbba9 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R4.path +++ b/src/main/deploy/pathplanner/paths/HP1_R4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.247, - "y": 6.779 + "x": 1.1505, + "y": 7.008500000000001 }, "prevControl": null, "nextControl": { - "x": 1.6122329879569814, - "y": 6.67470666218961 + "x": 1.5157329879569814, + "y": 6.904206662189611 }, "isLocked": false, "linkedName": "HP1" diff --git a/src/main/deploy/pathplanner/paths/HP1_R5.path b/src/main/deploy/pathplanner/paths/HP1_R5.path index 04da20c..3cfb0f3 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R5.path +++ b/src/main/deploy/pathplanner/paths/HP1_R5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.247, - "y": 6.779 + "x": 1.1505, + "y": 7.008500000000001 }, "prevControl": null, "nextControl": { - "x": 1.9956867780812888, - "y": 5.868660610858647 + "x": 1.8991867780812888, + "y": 6.098160610858648 }, "isLocked": false, "linkedName": "HP1" diff --git a/src/main/deploy/pathplanner/paths/HP1_R6.path b/src/main/deploy/pathplanner/paths/HP1_R6.path index 5df3e4b..a58a3c0 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R6.path +++ b/src/main/deploy/pathplanner/paths/HP1_R6.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.247, - "y": 6.779 + "x": 1.1505, + "y": 7.008500000000001 }, "prevControl": null, "nextControl": { - "x": 2.471821944376244, - "y": 6.861442318740352 + "x": 2.3753219443762443, + "y": 7.090942318740352 }, "isLocked": false, "linkedName": "HP1" diff --git a/src/main/deploy/pathplanner/paths/R1_HP1.path b/src/main/deploy/pathplanner/paths/R1_HP1.path index 55299ca..7723b2e 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP1.path +++ b/src/main/deploy/pathplanner/paths/R1_HP1.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.247, - "y": 6.779 + "x": 1.1505, + "y": 7.008500000000001 }, "prevControl": { - "x": 2.4672242627870786, - "y": 6.792890176740379 + "x": 2.3707242627870784, + "y": 7.022390176740379 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R2_HP1.path b/src/main/deploy/pathplanner/paths/R2_HP1.path index 001fcf2..68de026 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP1.path +++ b/src/main/deploy/pathplanner/paths/R2_HP1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.722528569965569, + "x": 3.723, "y": 4.991511920463674 }, "prevControl": null, "nextControl": { - "x": 3.415528569965569, + "x": 3.416, "y": 5.428761920463674 }, "isLocked": true, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.247, - "y": 6.779 + "x": 1.1505, + "y": 7.008500000000001 }, "prevControl": { - "x": 2.554500000000001, - "y": 7.603249999999999 + "x": 2.158749600130653, + "y": 7.644102090177952 }, "nextControl": null, "isLocked": true, diff --git a/src/main/deploy/pathplanner/paths/R3_HP1.path b/src/main/deploy/pathplanner/paths/R3_HP1.path index 3afbede..2c60fee 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/R3_HP1.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.247, - "y": 6.779 + "x": 1.1505, + "y": 7.008500000000001 }, "prevControl": { - "x": 1.9222661575487776, - "y": 6.969162308622982 + "x": 1.8257661575487776, + "y": 7.198662308622983 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R4_HP1.path b/src/main/deploy/pathplanner/paths/R4_HP1.path index 4e8e664..70c8c56 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP1.path +++ b/src/main/deploy/pathplanner/paths/R4_HP1.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.247, - "y": 6.779 + "x": 1.1505, + "y": 7.008500000000001 }, "prevControl": { - "x": 2.266705650839546, - "y": 7.380445551407007 + "x": 2.170205650839546, + "y": 7.609945551407008 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R5_HP1.path b/src/main/deploy/pathplanner/paths/R5_HP1.path index ab001d7..0f4c785 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP1.path +++ b/src/main/deploy/pathplanner/paths/R5_HP1.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 1.247, - "y": 6.779 + "x": 1.1505, + "y": 7.008500000000001 }, "prevControl": { - "x": 1.9834831721873583, - "y": 6.897147827026787 + "x": 1.8869831721873582, + "y": 7.126647827026788 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R6_HP1.path b/src/main/deploy/pathplanner/paths/R6_HP1.path index 1c85dfc..644bc8c 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP1.path +++ b/src/main/deploy/pathplanner/paths/R6_HP1.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.247, - "y": 6.779 + "x": 1.1505, + "y": 7.008500000000001 }, "prevControl": { - "x": 2.40247572815534, - "y": 7.177184466019417 + "x": 2.3059757281553397, + "y": 7.406684466019418 }, "nextControl": null, "isLocked": false, From 990b330aa27a16e4544034e4dade906ad23e7985 Mon Sep 17 00:00:00 2001 From: area5188 Date: Fri, 28 Mar 2025 12:17:37 -0400 Subject: [PATCH 069/106] changed setpoint --- src/main/java/frc/robot/subsystems/elevator/Elevator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index a1bdccf..e7526a4 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -37,7 +37,7 @@ public enum ElevatorPosition { L1(7, 5), L2(13, 18), L3(29, 36), - L4(52, 50), + L4(51.8, 50), Stow(0.5, 0.5); double coralHeight, algaeHeight; From a784ff855af50ddf13fde6fb8c9a1d3e9dec6e94 Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 28 Mar 2025 13:46:42 -0400 Subject: [PATCH 070/106] Faster paths for Autos --- src/main/deploy/pathplanner/paths/A_R3.path | 15 ++++-- src/main/deploy/pathplanner/paths/C_R5.path | 26 +++++++--- src/main/deploy/pathplanner/paths/HP1_R1.path | 48 ++++++++++++------- src/main/deploy/pathplanner/paths/HP1_R2.path | 46 +++++++++--------- src/main/deploy/pathplanner/paths/HP1_R3.path | 8 ++-- src/main/deploy/pathplanner/paths/HP1_R4.path | 8 ++-- src/main/deploy/pathplanner/paths/HP1_R5.path | 8 ++-- src/main/deploy/pathplanner/paths/HP1_R6.path | 8 ++-- src/main/deploy/pathplanner/paths/HP2_R1.path | 8 ++-- src/main/deploy/pathplanner/paths/HP2_R2.path | 20 ++++---- src/main/deploy/pathplanner/paths/HP2_R3.path | 8 ++-- src/main/deploy/pathplanner/paths/HP2_R4.path | 28 +++++------ src/main/deploy/pathplanner/paths/HP2_R5.path | 12 ++--- src/main/deploy/pathplanner/paths/HP2_R6.path | 12 ++--- src/main/deploy/pathplanner/paths/R1_HP1.path | 8 ++-- src/main/deploy/pathplanner/paths/R1_HP2.path | 8 ++-- src/main/deploy/pathplanner/paths/R2_HP1.path | 28 ++++++++--- src/main/deploy/pathplanner/paths/R2_HP2.path | 8 ++-- src/main/deploy/pathplanner/paths/R3_HP1.path | 44 +++++++++++------ src/main/deploy/pathplanner/paths/R3_HP2.path | 8 ++-- src/main/deploy/pathplanner/paths/R4_HP1.path | 8 ++-- src/main/deploy/pathplanner/paths/R4_HP2.path | 8 ++-- src/main/deploy/pathplanner/paths/R5_HP1.path | 8 ++-- src/main/deploy/pathplanner/paths/R5_HP2.path | 8 ++-- src/main/deploy/pathplanner/paths/R6_HP1.path | 8 ++-- src/main/deploy/pathplanner/paths/R6_HP2.path | 8 ++-- 26 files changed, 233 insertions(+), 174 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index ebd27c7..330b021 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.383867159681032, - "y": 6.218696771513908 + "x": 5.371895516834254, + "y": 5.469936584938475 }, "isLocked": false, "linkedName": "A" @@ -20,15 +20,20 @@ "y": 5.181813524590163 }, "prevControl": { - "x": 5.658196721311475, - "y": 5.529456967213114 + "x": 5.248038282296709, + "y": 5.161401822310868 }, "nextControl": null, "isLocked": false, "linkedName": "ScoreR3" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -121.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index 19ce3a9..88bbfd2 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.33834728384462, - "y": 1.8987244973168789 + "x": 6.158120375113414, + "y": 2.116983136133424 }, "isLocked": false, "linkedName": "C" @@ -20,8 +20,8 @@ "y": 3.036014344262295 }, "prevControl": { - "x": 5.372114171380803, - "y": 2.8246350921900167 + "x": 5.404290481090531, + "y": 2.8487828636621195 }, "nextControl": null, "isLocked": false, @@ -34,12 +34,26 @@ "rotationDegrees": 162.15573333673396 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.39865470852017926, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 3.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R1.path b/src/main/deploy/pathplanner/paths/HP1_R1.path index b3b53cb..6352637 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R1.path +++ b/src/main/deploy/pathplanner/paths/HP1_R1.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.1505, - "y": 7.008500000000001 + "x": 1.6185000000000003, + "y": 7.34975 }, "prevControl": null, "nextControl": { - "x": 2.234826704661507, - "y": 6.9395139362713385 + "x": 2.68125, + "y": 6.60875 }, "isLocked": false, "linkedName": "HP1" }, { "anchor": { - "x": 2.737096933762121, - "y": 6.442052464223761 + "x": 2.3594999999999997, + "y": 4.6587499999999995 }, "prevControl": { - "x": 2.199338419960653, - "y": 6.907793955344957 + "x": 2.3095099970009993, + "y": 4.903701014695101 }, "nextControl": { - "x": 3.2145229946722402, - "y": 6.028563658743894 + "x": 2.457, + "y": 4.181 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 3.837271284603343 }, "prevControl": { - "x": 2.13525, - "y": 3.89825 + "x": 2.9615631901299118, + "y": 4.182839400520257 }, "nextControl": null, "isLocked": false, @@ -46,16 +46,30 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.1049808429118704, + "waypointRelativePos": 0.6681904761904747, "rotationDegrees": 0.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.5964125560538172, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.0, + "maxVelocity": 10.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -71,5 +85,5 @@ "velocity": 0, "rotation": -53.8 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP1_R2.path b/src/main/deploy/pathplanner/paths/HP1_R2.path index a0ecd5c..cc15c5f 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R2.path +++ b/src/main/deploy/pathplanner/paths/HP1_R2.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.1505, - "y": 7.008500000000001 + "x": 1.6185000000000003, + "y": 7.34975 }, "prevControl": null, "nextControl": { - "x": 1.3914523599452857, - "y": 6.941851967494926 + "x": 1.8179302447703478, + "y": 7.148073150731447 }, "isLocked": false, "linkedName": "HP1" }, - { - "anchor": { - "x": 2.673258196721312, - "y": 6.452510245901639 - }, - "prevControl": { - "x": 2.311791120569321, - "y": 6.587935044803319 - }, - "nextControl": { - "x": 2.9073671721106025, - "y": 6.364800567745939 - }, - "isLocked": false, - "linkedName": null - }, { "anchor": { "x": 3.722528569965569, "y": 4.991511920463674 }, "prevControl": { - "x": 3.886774788148542, - "y": 4.803035980851767 + "x": 3.5620492827809422, + "y": 5.1832054216177665 }, "nextControl": null, "isLocked": false, @@ -45,12 +29,26 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.481165919282511, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 3.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R3.path b/src/main/deploy/pathplanner/paths/HP1_R3.path index 78d3d4d..ba9bc8d 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R3.path +++ b/src/main/deploy/pathplanner/paths/HP1_R3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.1505, - "y": 7.008500000000001 + "x": 1.6185000000000003, + "y": 7.34975 }, "prevControl": null, "nextControl": { - "x": 1.4145523892007985, - "y": 6.936681382889393 + "x": 1.8825523892007987, + "y": 7.277931382889393 }, "isLocked": false, "linkedName": "HP1" diff --git a/src/main/deploy/pathplanner/paths/HP1_R4.path b/src/main/deploy/pathplanner/paths/HP1_R4.path index 2adbba9..a7f3699 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R4.path +++ b/src/main/deploy/pathplanner/paths/HP1_R4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.1505, - "y": 7.008500000000001 + "x": 1.6185000000000003, + "y": 7.34975 }, "prevControl": null, "nextControl": { - "x": 1.5157329879569814, - "y": 6.904206662189611 + "x": 1.9837329879569816, + "y": 7.245456662189611 }, "isLocked": false, "linkedName": "HP1" diff --git a/src/main/deploy/pathplanner/paths/HP1_R5.path b/src/main/deploy/pathplanner/paths/HP1_R5.path index 3cfb0f3..cfbe7df 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R5.path +++ b/src/main/deploy/pathplanner/paths/HP1_R5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.1505, - "y": 7.008500000000001 + "x": 1.6185000000000003, + "y": 7.34975 }, "prevControl": null, "nextControl": { - "x": 1.8991867780812888, - "y": 6.098160610858648 + "x": 2.367186778081289, + "y": 6.439410610858648 }, "isLocked": false, "linkedName": "HP1" diff --git a/src/main/deploy/pathplanner/paths/HP1_R6.path b/src/main/deploy/pathplanner/paths/HP1_R6.path index a58a3c0..0550f5d 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R6.path +++ b/src/main/deploy/pathplanner/paths/HP1_R6.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.1505, - "y": 7.008500000000001 + "x": 1.6185000000000003, + "y": 7.34975 }, "prevControl": null, "nextControl": { - "x": 2.3753219443762443, - "y": 7.090942318740352 + "x": 2.8433219443762443, + "y": 7.432192318740352 }, "isLocked": false, "linkedName": "HP1" diff --git a/src/main/deploy/pathplanner/paths/HP2_R1.path b/src/main/deploy/pathplanner/paths/HP2_R1.path index 7623739..781ea8e 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R1.path +++ b/src/main/deploy/pathplanner/paths/HP2_R1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.1931710970554246, - "y": 1.000314900025919 + "x": 1.599, + "y": 0.670999999999999 }, "prevControl": null, "nextControl": { - "x": 2.3821128446282396, - "y": 0.968907133035626 + "x": 2.7879417475728143, + "y": 0.6395922330097061 }, "isLocked": false, "linkedName": "HP2" diff --git a/src/main/deploy/pathplanner/paths/HP2_R2.path b/src/main/deploy/pathplanner/paths/HP2_R2.path index 9cfb7dd..c3c6f34 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R2.path +++ b/src/main/deploy/pathplanner/paths/HP2_R2.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.1931710970554246, - "y": 1.000314900025919 + "x": 1.599, + "y": 0.670999999999999 }, "prevControl": null, "nextControl": { - "x": 3.1658992523952296, - "y": 2.1786644145890266 + "x": 2.6715, + "y": 1.5972499999999992 }, "isLocked": false, "linkedName": "HP2" }, { "anchor": { - "x": 2.535, - "y": 3.771499999999999 + "x": 2.3594999999999997, + "y": 4.278499999999999 }, "prevControl": { - "x": 2.699990748943108, - "y": 3.1115370042275656 + "x": 2.2522499999999996, + "y": 3.8982499999999995 }, "nextControl": { - "x": 2.2964563106796123, - "y": 4.725674757281553 + "x": 2.6264921553096476, + "y": 5.225108550643294 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/HP2_R3.path b/src/main/deploy/pathplanner/paths/HP2_R3.path index beab262..b2758b8 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R3.path +++ b/src/main/deploy/pathplanner/paths/HP2_R3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.1931710970554246, - "y": 1.000314900025919 + "x": 1.599, + "y": 0.670999999999999 }, "prevControl": null, "nextControl": { - "x": 1.3550246861430486, - "y": 1.1908494525339997 + "x": 1.7608535890876242, + "y": 0.8615345525080795 }, "isLocked": false, "linkedName": "HP2" diff --git a/src/main/deploy/pathplanner/paths/HP2_R4.path b/src/main/deploy/pathplanner/paths/HP2_R4.path index 1bd7658..05cbed7 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R4.path +++ b/src/main/deploy/pathplanner/paths/HP2_R4.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.1931710970554246, - "y": 1.000314900025919 + "x": 1.599, + "y": 0.670999999999999 }, "prevControl": null, "nextControl": { - "x": 2.979233186650622, - "y": 0.03372625004544627 + "x": 3.1005000000000003, + "y": 1.402249999999999 }, "isLocked": false, "linkedName": "HP2" }, { "anchor": { - "x": 4.839029126213592, - "y": 1.4862135922330106 + "x": 6.298500000000001, + "y": 2.61125 }, "prevControl": { - "x": 4.617353569020709, - "y": 1.3706283538016897 + "x": 5.664750000000001, + "y": 2.104250000000001 }, "nextControl": { - "x": 5.12442779806873, - "y": 1.6350250983462242 + "x": 6.549834567430718, + "y": 2.8123176539445742 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 4.198821721311475 }, "prevControl": { - "x": 7.208446721311473, - "y": 4.091571721311475 + "x": 6.747000000000001, + "y": 4.52225 }, "nextControl": null, "isLocked": false, @@ -59,7 +59,7 @@ "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, @@ -71,5 +71,5 @@ "velocity": 0, "rotation": 53.8 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP2_R5.path b/src/main/deploy/pathplanner/paths/HP2_R5.path index f891c9c..0f45912 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R5.path +++ b/src/main/deploy/pathplanner/paths/HP2_R5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.1931710970554246, - "y": 1.000314900025919 + "x": 1.599, + "y": 0.670999999999999 }, "prevControl": null, "nextControl": { - "x": 5.414926965555326, - "y": 2.476894615411636 + "x": 3.12975, + "y": 1.5387499999999998 }, "isLocked": false, "linkedName": "HP2" @@ -20,8 +20,8 @@ "y": 3.036014344262295 }, "prevControl": { - "x": 6.007598360655738, - "y": 2.1487643442622946 + "x": 5.9182500000000005, + "y": 2.328499999999999 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HP2_R6.path b/src/main/deploy/pathplanner/paths/HP2_R6.path index 5df6442..4b8443d 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R6.path +++ b/src/main/deploy/pathplanner/paths/HP2_R6.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.1931710970554246, - "y": 1.000314900025919 + "x": 1.599, + "y": 0.670999999999999 }, "prevControl": null, "nextControl": { - "x": 2.4744734864324043, - "y": 1.7941687665328385 + "x": 2.9838742645740246, + "y": 2.0569625625489363 }, "isLocked": false, "linkedName": "HP2" @@ -20,8 +20,8 @@ "y": 2.916137295081967 }, "prevControl": { - "x": 3.823992678906107, - "y": 2.78945525667752 + "x": 3.8588082879463577, + "y": 2.743737979143208 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R1_HP1.path b/src/main/deploy/pathplanner/paths/R1_HP1.path index 7723b2e..863af19 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP1.path +++ b/src/main/deploy/pathplanner/paths/R1_HP1.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.1505, - "y": 7.008500000000001 + "x": 1.6185000000000003, + "y": 7.34975 }, "prevControl": { - "x": 2.3707242627870784, - "y": 7.022390176740379 + "x": 2.8387242627870783, + "y": 7.363640176740379 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R1_HP2.path b/src/main/deploy/pathplanner/paths/R1_HP2.path index a37bc66..0b0ec8a 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP2.path +++ b/src/main/deploy/pathplanner/paths/R1_HP2.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.1931710970554246, - "y": 1.000314900025919 + "x": 1.599, + "y": 0.670999999999999 }, "prevControl": { - "x": 1.92206430093892, - "y": 1.2244896573074708 + "x": 2.3278932038834954, + "y": 0.8951747572815507 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R2_HP1.path b/src/main/deploy/pathplanner/paths/R2_HP1.path index 68de026..42f2a45 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP1.path +++ b/src/main/deploy/pathplanner/paths/R2_HP1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 3.416, - "y": 5.428761920463674 + "x": 2.665728368105369, + "y": 6.223712505356056 }, "isLocked": true, "linkedName": "ScoreR2" }, { "anchor": { - "x": 1.1505, - "y": 7.008500000000001 + "x": 1.6185000000000003, + "y": 7.34975 }, "prevControl": { - "x": 2.158749600130653, - "y": 7.644102090177952 + "x": 1.7860635948907588, + "y": 7.164217141273343 }, "nextControl": null, "isLocked": true, @@ -29,7 +29,21 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.45067264573991034, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/R2_HP2.path b/src/main/deploy/pathplanner/paths/R2_HP2.path index c24c178..d821e2e 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP2.path +++ b/src/main/deploy/pathplanner/paths/R2_HP2.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.1931710970554246, - "y": 1.000314900025919 + "x": 1.599, + "y": 0.670999999999999 }, "prevControl": { - "x": 2.5695400290942594, - "y": 1.3267226670162107 + "x": 2.975368932038834, + "y": 0.9974077669902908 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R3_HP1.path b/src/main/deploy/pathplanner/paths/R3_HP1.path index 2c60fee..2ebcbff 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/R3_HP1.path @@ -8,36 +8,36 @@ }, "prevControl": null, "nextControl": { - "x": 5.778073770491804, - "y": 6.320645491803279 + "x": 4.96275, + "y": 6.062749999999999 }, "isLocked": false, "linkedName": "ScoreR3" }, { "anchor": { - "x": 2.940519751329567, - "y": 6.779 + "x": 4.085249999999999, + "y": 6.238249999999999 }, "prevControl": { - "x": 3.380532786885246, - "y": 6.728227459016393 + "x": 4.391379510922372, + "y": 6.100276135922311 }, "nextControl": { - "x": 2.400148726044413, - "y": 6.841352720966541 + "x": 3.3929999999999993, + "y": 6.550249999999999 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.1505, - "y": 7.008500000000001 + "x": 1.6185000000000003, + "y": 7.34975 }, "prevControl": { - "x": 1.8257661575487776, - "y": 7.198662308622983 + "x": 1.8781417813890309, + "y": 7.256893925639328 }, "nextControl": null, "isLocked": false, @@ -46,16 +46,30 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.0681992337164712, + "waypointRelativePos": 1.417904761904762, "rotationDegrees": -53.8 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6860986547085202, + "maxWaypointRelativePos": 1.626008968609865, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.5, + "maxAngularVelocity": 800.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 3.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R3_HP2.path b/src/main/deploy/pathplanner/paths/R3_HP2.path index 7be6591..cac1ec6 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP2.path +++ b/src/main/deploy/pathplanner/paths/R3_HP2.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 1.1931710970554246, - "y": 1.000314900025919 + "x": 1.599, + "y": 0.670999999999999 }, "prevControl": { - "x": 1.4199617770152804, - "y": 1.1055098974513968 + "x": 1.8257906799598558, + "y": 0.7761949974254767 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R4_HP1.path b/src/main/deploy/pathplanner/paths/R4_HP1.path index 70c8c56..aac7b48 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP1.path +++ b/src/main/deploy/pathplanner/paths/R4_HP1.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.1505, - "y": 7.008500000000001 + "x": 1.6185000000000003, + "y": 7.34975 }, "prevControl": { - "x": 2.170205650839546, - "y": 7.609945551407008 + "x": 2.638205650839546, + "y": 7.951195551407007 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R4_HP2.path b/src/main/deploy/pathplanner/paths/R4_HP2.path index b702f39..8c1f078 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP2.path +++ b/src/main/deploy/pathplanner/paths/R4_HP2.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.1931710970554246, - "y": 1.000314900025919 + "x": 1.599, + "y": 0.670999999999999 }, "prevControl": { - "x": 1.9858712554173255, - "y": 1.0891686502126687 + "x": 2.3917001583619006, + "y": 0.7598537501867488 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R5_HP1.path b/src/main/deploy/pathplanner/paths/R5_HP1.path index 0f4c785..99dbcdb 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP1.path +++ b/src/main/deploy/pathplanner/paths/R5_HP1.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 1.1505, - "y": 7.008500000000001 + "x": 1.6185000000000003, + "y": 7.34975 }, "prevControl": { - "x": 1.8869831721873582, - "y": 7.126647827026788 + "x": 2.3549831721873584, + "y": 7.467897827026787 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index 2f87d94..e3846d1 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.1931710970554246, - "y": 1.000314900025919 + "x": 1.599, + "y": 0.670999999999999 }, "prevControl": { - "x": 1.670863353587962, - "y": 1.3563993778935173 + "x": 2.076692256532537, + "y": 1.0270844778675974 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R6_HP1.path b/src/main/deploy/pathplanner/paths/R6_HP1.path index 644bc8c..f30d26d 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP1.path +++ b/src/main/deploy/pathplanner/paths/R6_HP1.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.1505, - "y": 7.008500000000001 + "x": 1.6185000000000003, + "y": 7.34975 }, "prevControl": { - "x": 2.3059757281553397, - "y": 7.406684466019418 + "x": 2.7739757281553397, + "y": 7.747934466019418 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index bb49b7c..14fa1c8 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.1931710970554246, - "y": 1.000314900025919 + "x": 1.599, + "y": 0.670999999999999 }, "prevControl": { - "x": 1.354446253433668, - "y": 1.191339306673795 + "x": 1.7602751563782435, + "y": 0.8620244066478748 }, "nextControl": null, "isLocked": false, From ea52a5a8e1cbb31c653b908aa635e4c4edfbb4b2 Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 28 Mar 2025 15:57:40 -0400 Subject: [PATCH 071/106] Fast Autos 3P --- .../{A-3PieceL4.auto => A-3PieceL4-GOOD.auto} | 2 +- .../deploy/pathplanner/autos/C-3PieceL4.auto | 2 +- .../autos/{C-3P0.auto => Fast-A-3Piece.auto} | 26 +++--- .../{A-2.5Piece.auto => Fast-C-3PieceL4.auto} | 83 ++++++++++++++--- src/main/deploy/pathplanner/autos/LeaveD.auto | 19 ---- .../deploy/pathplanner/autos/TestAuto.auto | 19 ---- .../deploy/pathplanner/paths/Fast_A_R3.path | 59 ++++++++++++ .../deploy/pathplanner/paths/Fast_C_R5.path | 73 +++++++++++++++ .../pathplanner/paths/Fast_HP1_L.R2.path | 68 ++++++++++++++ .../pathplanner/paths/Fast_HP1_R.R2.path | 68 ++++++++++++++ .../deploy/pathplanner/paths/Fast_HP1_R1.path | 84 +++++++++++++++++ .../pathplanner/paths/Fast_HP2_L.R6.path | 68 ++++++++++++++ .../pathplanner/paths/Fast_HP2_R.R6.path | 73 +++++++++++++++ .../pathplanner/paths/Fast_L.R2_HP1.path | 68 ++++++++++++++ .../pathplanner/paths/Fast_L.R6_HP2.path | 68 ++++++++++++++ .../pathplanner/paths/Fast_R.R2_HP1.path | 68 ++++++++++++++ .../pathplanner/paths/Fast_R.R6_HP2.path | 68 ++++++++++++++ .../deploy/pathplanner/paths/Fast_R3_HP1.path | 89 +++++++++++++++++++ .../deploy/pathplanner/paths/Fast_R5_HP2.path | 89 +++++++++++++++++++ src/main/deploy/pathplanner/paths/HP1_R1.path | 2 +- src/main/deploy/pathplanner/paths/HP1_R2.path | 2 +- src/main/deploy/pathplanner/paths/HP2_R2.path | 20 ++--- .../deploy/pathplanner/paths/PushScore.path | 2 +- src/main/deploy/pathplanner/paths/R2_HP1.path | 2 +- src/main/deploy/pathplanner/settings.json | 5 +- 25 files changed, 1049 insertions(+), 78 deletions(-) rename src/main/deploy/pathplanner/autos/{A-3PieceL4.auto => A-3PieceL4-GOOD.auto} (98%) rename src/main/deploy/pathplanner/autos/{C-3P0.auto => Fast-A-3Piece.auto} (90%) rename src/main/deploy/pathplanner/autos/{A-2.5Piece.auto => Fast-C-3PieceL4.auto} (53%) delete mode 100644 src/main/deploy/pathplanner/autos/LeaveD.auto delete mode 100644 src/main/deploy/pathplanner/autos/TestAuto.auto create mode 100644 src/main/deploy/pathplanner/paths/Fast_A_R3.path create mode 100644 src/main/deploy/pathplanner/paths/Fast_C_R5.path create mode 100644 src/main/deploy/pathplanner/paths/Fast_HP1_L.R2.path create mode 100644 src/main/deploy/pathplanner/paths/Fast_HP1_R.R2.path create mode 100644 src/main/deploy/pathplanner/paths/Fast_HP1_R1.path create mode 100644 src/main/deploy/pathplanner/paths/Fast_HP2_L.R6.path create mode 100644 src/main/deploy/pathplanner/paths/Fast_HP2_R.R6.path create mode 100644 src/main/deploy/pathplanner/paths/Fast_L.R2_HP1.path create mode 100644 src/main/deploy/pathplanner/paths/Fast_L.R6_HP2.path create mode 100644 src/main/deploy/pathplanner/paths/Fast_R.R2_HP1.path create mode 100644 src/main/deploy/pathplanner/paths/Fast_R.R6_HP2.path create mode 100644 src/main/deploy/pathplanner/paths/Fast_R3_HP1.path create mode 100644 src/main/deploy/pathplanner/paths/Fast_R5_HP2.path diff --git a/src/main/deploy/pathplanner/autos/A-3PieceL4.auto b/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto similarity index 98% rename from src/main/deploy/pathplanner/autos/A-3PieceL4.auto rename to src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto index f793cf1..bde3313 100644 --- a/src/main/deploy/pathplanner/autos/A-3PieceL4.auto +++ b/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto @@ -99,7 +99,7 @@ { "type": "path", "data": { - "pathName": "HP1_R1" + "pathName": "Fast_HP1_L.R2" } }, { diff --git a/src/main/deploy/pathplanner/autos/C-3PieceL4.auto b/src/main/deploy/pathplanner/autos/C-3PieceL4.auto index a950447..1277524 100644 --- a/src/main/deploy/pathplanner/autos/C-3PieceL4.auto +++ b/src/main/deploy/pathplanner/autos/C-3PieceL4.auto @@ -99,7 +99,7 @@ { "type": "path", "data": { - "pathName": "HP2_R1" + "pathName": "Fast_HP2_L.R6" } }, { diff --git a/src/main/deploy/pathplanner/autos/C-3P0.auto b/src/main/deploy/pathplanner/autos/Fast-A-3Piece.auto similarity index 90% rename from src/main/deploy/pathplanner/autos/C-3P0.auto rename to src/main/deploy/pathplanner/autos/Fast-A-3Piece.auto index 931842f..258a359 100644 --- a/src/main/deploy/pathplanner/autos/C-3P0.auto +++ b/src/main/deploy/pathplanner/autos/Fast-A-3Piece.auto @@ -9,15 +9,15 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "L4" + "pathName": "Fast_A_R3" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "C_R5" + "name": "L4" } } ] @@ -36,7 +36,7 @@ { "type": "path", "data": { - "pathName": "R5_HP2" + "pathName": "Fast_R3_HP1" } }, { @@ -53,15 +53,15 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "L4" + "pathName": "Fast_HP1_R.R2" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "HP2_R6" + "name": "L4" } } ] @@ -80,7 +80,7 @@ { "type": "path", "data": { - "pathName": "R6_HP2" + "pathName": "Fast_R.R2_HP1" } }, { @@ -99,13 +99,13 @@ { "type": "path", "data": { - "pathName": "HP2_R6" + "pathName": "Fast_HP1_L.R2" } }, { "type": "named", "data": { - "name": "L1" + "name": "L4" } } ] @@ -134,6 +134,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "FAST AUTOS", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-2.5Piece.auto b/src/main/deploy/pathplanner/autos/Fast-C-3PieceL4.auto similarity index 53% rename from src/main/deploy/pathplanner/autos/A-2.5Piece.auto rename to src/main/deploy/pathplanner/autos/Fast-C-3PieceL4.auto index 7604d6a..9269f0b 100644 --- a/src/main/deploy/pathplanner/autos/A-2.5Piece.auto +++ b/src/main/deploy/pathplanner/autos/Fast-C-3PieceL4.auto @@ -4,6 +4,31 @@ "type": "sequential", "data": { "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "C_R5" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, { "type": "parallel", "data": { @@ -11,18 +36,43 @@ { "type": "path", "data": { - "pathName": "A_R3" + "pathName": "R5_HP2" } }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ { "type": "named", "data": { "name": "L4" } + }, + { + "type": "path", + "data": { + "pathName": "Fast_HP2_L.R6" + } } ] } }, + { + "type": "named", + "data": { + "name": "Score" + } + }, { "type": "parallel", "data": { @@ -30,7 +80,7 @@ { "type": "path", "data": { - "pathName": "R3_HP1" + "pathName": "Fast_L.R6_HP2" } }, { @@ -49,7 +99,7 @@ { "type": "path", "data": { - "pathName": "HP1_R2" + "pathName": "Fast_HP2_R.R6" } }, { @@ -62,25 +112,38 @@ } }, { - "type": "named", + "type": "parallel", "data": { - "name": "Score" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Fast_R.R6_HP2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] } }, { - "type": "parallel", + "type": "sequential", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "R2_HP1" + "name": "Score" } }, { "type": "named", "data": { - "name": "Intake" + "name": "Stow" } } ] @@ -90,6 +153,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "FAST AUTOS", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeaveD.auto b/src/main/deploy/pathplanner/autos/LeaveD.auto deleted file mode 100644 index 90b6ccc..0000000 --- a/src/main/deploy/pathplanner/autos/LeaveD.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": null - } - } - ] - } - }, - "resetOdom": true, - "folder": "LeaveStart", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TestAuto.auto b/src/main/deploy/pathplanner/autos/TestAuto.auto deleted file mode 100644 index 35b4eaf..0000000 --- a/src/main/deploy/pathplanner/autos/TestAuto.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ScoreR2" - } - } - ] - } - }, - "resetOdom": true, - "folder": "Test", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_A_R3.path b/src/main/deploy/pathplanner/paths/Fast_A_R3.path new file mode 100644 index 0000000..2b4ddaa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_A_R3.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.5282786885245905, + "y": 6.188780737704917 + }, + "prevControl": null, + "nextControl": { + "x": 5.371895516834254, + "y": 5.469936584938475 + }, + "isLocked": false, + "linkedName": "A" + }, + { + "anchor": { + "x": 4.998872950819672, + "y": 5.181813524590163 + }, + "prevControl": { + "x": 5.226, + "y": 5.49725 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ScoreR3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -121.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -121.5 + }, + "reversed": false, + "folder": "Fast A 3 Piece", + "idealStartingState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_C_R5.path b/src/main/deploy/pathplanner/paths/Fast_C_R5.path new file mode 100644 index 0000000..0910852 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_C_R5.path @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.564241803278687, + "y": 1.9810963114754099 + }, + "prevControl": null, + "nextControl": { + "x": 6.379939714782926, + "y": 2.224089085857935 + }, + "isLocked": false, + "linkedName": "C" + }, + { + "anchor": { + "x": 5.238627049180328, + "y": 3.036014344262295 + }, + "prevControl": { + "x": 5.503406048705033, + "y": 2.7758299406484523 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ScoreR5" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3, + "rotationDegrees": 162.15573333673396 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.39865470852017926, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.9 + }, + "reversed": false, + "folder": "Fast C 3 Piece", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_HP1_L.R2.path b/src/main/deploy/pathplanner/paths/Fast_HP1_L.R2.path new file mode 100644 index 0000000..d9d85e9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_HP1_L.R2.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.6185000000000003, + "y": 7.34975 + }, + "prevControl": null, + "nextControl": { + "x": 1.8179302447703478, + "y": 7.148073150731447 + }, + "isLocked": false, + "linkedName": "HP1" + }, + { + "anchor": { + "x": 4.00725, + "y": 5.1365 + }, + "prevControl": { + "x": 3.846770712815373, + "y": 5.328193501154092 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "L.R2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.531390134529148, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.5 + }, + "reversed": false, + "folder": "Fast A 3 Piece", + "idealStartingState": { + "velocity": 0, + "rotation": -53.8 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_HP1_R.R2.path b/src/main/deploy/pathplanner/paths/Fast_HP1_R.R2.path new file mode 100644 index 0000000..8fca783 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_HP1_R.R2.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.6185000000000003, + "y": 7.34975 + }, + "prevControl": null, + "nextControl": { + "x": 1.8179302447703478, + "y": 7.148073150731447 + }, + "isLocked": false, + "linkedName": "HP1" + }, + { + "anchor": { + "x": 3.722528569965569, + "y": 4.991511920463674 + }, + "prevControl": { + "x": 3.5620492827809422, + "y": 5.1832054216177665 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ScoreR2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.531390134529148, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.5 + }, + "reversed": false, + "folder": "Fast A 3 Piece", + "idealStartingState": { + "velocity": 0, + "rotation": -53.8 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_HP1_R1.path b/src/main/deploy/pathplanner/paths/Fast_HP1_R1.path new file mode 100644 index 0000000..be6ada5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_HP1_R1.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.6185000000000003, + "y": 7.34975 + }, + "prevControl": null, + "nextControl": { + "x": 2.68125, + "y": 6.60875 + }, + "isLocked": false, + "linkedName": "HP1" + }, + { + "anchor": { + "x": 2.3985000000000003, + "y": 5.67275 + }, + "prevControl": { + "x": 2.348509997001, + "y": 5.917701014695101 + }, + "nextControl": { + "x": 2.4960000000000004, + "y": 5.195 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.2841721390676866, + "y": 3.837271284603343 + }, + "prevControl": { + "x": 2.27175, + "y": 3.7812499999999996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ScoreR1" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.141704035874439, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Fast A 3 Piece", + "idealStartingState": { + "velocity": 0, + "rotation": -53.8 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_HP2_L.R6.path b/src/main/deploy/pathplanner/paths/Fast_HP2_L.R6.path new file mode 100644 index 0000000..78a387d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_HP2_L.R6.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.599, + "y": 0.670999999999999 + }, + "prevControl": null, + "nextControl": { + "x": 3.051221402882968, + "y": 2.2464597642937965 + }, + "isLocked": false, + "linkedName": "HP2" + }, + { + "anchor": { + "x": 3.7342500000000003, + "y": 3.0694999999999997 + }, + "prevControl": { + "x": 3.565088621835697, + "y": 2.885298566368326 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "L.R6" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.48654708520179374, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": "Fast C 3 Piece", + "idealStartingState": { + "velocity": 0, + "rotation": 53.8 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_HP2_R.R6.path b/src/main/deploy/pathplanner/paths/Fast_HP2_R.R6.path new file mode 100644 index 0000000..ff7bea2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_HP2_R.R6.path @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.599, + "y": 0.670999999999999 + }, + "prevControl": null, + "nextControl": { + "x": 2.9838742645740246, + "y": 2.0569625625489363 + }, + "isLocked": false, + "linkedName": "HP2" + }, + { + "anchor": { + "x": 4.039856557377049, + "y": 2.916137295081967 + }, + "prevControl": { + "x": 3.8588082879463577, + "y": 2.743737979143208 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ScoreR6" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.45, + "rotationDegrees": 59.99999999999999 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.48654708520179374, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": "Fast C 3 Piece", + "idealStartingState": { + "velocity": 0, + "rotation": 53.8 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_L.R2_HP1.path b/src/main/deploy/pathplanner/paths/Fast_L.R2_HP1.path new file mode 100644 index 0000000..da9a09d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_L.R2_HP1.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.00725, + "y": 5.1365 + }, + "prevControl": null, + "nextControl": { + "x": 2.7798189688994217, + "y": 6.2871263127274535 + }, + "isLocked": true, + "linkedName": "L.R2" + }, + { + "anchor": { + "x": 1.6185000000000003, + "y": 7.34975 + }, + "prevControl": { + "x": 1.8023234741866845, + "y": 7.180312901529986 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "HP1" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.45067264573991034, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -53.8 + }, + "reversed": false, + "folder": "Fast A 3 Piece", + "idealStartingState": { + "velocity": 0, + "rotation": -59.5 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_L.R6_HP2.path b/src/main/deploy/pathplanner/paths/Fast_L.R6_HP2.path new file mode 100644 index 0000000..4f1943f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_L.R6_HP2.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.7342500000000003, + "y": 3.0694999999999997 + }, + "prevControl": null, + "nextControl": { + "x": 3.2656055412247555, + "y": 2.5711636033703953 + }, + "isLocked": false, + "linkedName": "L.R6" + }, + { + "anchor": { + "x": 1.599, + "y": 0.670999999999999 + }, + "prevControl": { + "x": 1.7602751563782437, + "y": 0.8620244066478749 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.5600896860986545, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 800.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 800.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 53.8 + }, + "reversed": false, + "folder": "Fast C 3 Piece", + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_R.R2_HP1.path b/src/main/deploy/pathplanner/paths/Fast_R.R2_HP1.path new file mode 100644 index 0000000..8eb3af2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_R.R2_HP1.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.723, + "y": 4.991511920463674 + }, + "prevControl": null, + "nextControl": { + "x": 2.665728368105369, + "y": 6.223712505356056 + }, + "isLocked": true, + "linkedName": "ScoreR2" + }, + { + "anchor": { + "x": 1.6185000000000003, + "y": 7.34975 + }, + "prevControl": { + "x": 1.7860635948907588, + "y": 7.164217141273343 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "HP1" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.45067264573991034, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -53.8 + }, + "reversed": false, + "folder": "Fast A 3 Piece", + "idealStartingState": { + "velocity": 0, + "rotation": -59.5 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_R.R6_HP2.path b/src/main/deploy/pathplanner/paths/Fast_R.R6_HP2.path new file mode 100644 index 0000000..c2b4ae9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_R.R6_HP2.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.039856557377049, + "y": 2.916137295081967 + }, + "prevControl": null, + "nextControl": { + "x": 3.571212098601804, + "y": 2.4178008984523625 + }, + "isLocked": false, + "linkedName": "ScoreR6" + }, + { + "anchor": { + "x": 1.599, + "y": 0.670999999999999 + }, + "prevControl": { + "x": 1.7602751563782437, + "y": 0.8620244066478749 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.5493273542600897, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 800.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 800.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 53.8 + }, + "reversed": false, + "folder": "Fast C 3 Piece", + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_R3_HP1.path b/src/main/deploy/pathplanner/paths/Fast_R3_HP1.path new file mode 100644 index 0000000..2d25210 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_R3_HP1.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.998872950819672, + "y": 5.181813524590163 + }, + "prevControl": null, + "nextControl": { + "x": 5.226, + "y": 6.286999999999999 + }, + "isLocked": false, + "linkedName": "ScoreR3" + }, + { + "anchor": { + "x": 3.36375, + "y": 6.51125 + }, + "prevControl": { + "x": 3.6698795109223714, + "y": 6.373276135922312 + }, + "nextControl": { + "x": 2.6715, + "y": 6.823250000000001 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.6185000000000003, + "y": 7.34975 + }, + "prevControl": { + "x": 1.8781417813890309, + "y": 7.256893925639328 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.417904761904762, + "rotationDegrees": -53.8 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.1452914798206282, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 800.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 800.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.8 + }, + "reversed": false, + "folder": "Fast A 3 Piece", + "idealStartingState": { + "velocity": 0, + "rotation": -121.5 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_R5_HP2.path b/src/main/deploy/pathplanner/paths/Fast_R5_HP2.path new file mode 100644 index 0000000..92da981 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fast_R5_HP2.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.238627049180328, + "y": 3.036014344262295 + }, + "prevControl": null, + "nextControl": { + "x": 5.8305, + "y": 2.4065 + }, + "isLocked": false, + "linkedName": "ScoreR5" + }, + { + "anchor": { + "x": 3.7635, + "y": 1.938499999999999 + }, + "prevControl": { + "x": 4.104750000000001, + "y": 1.996999999999999 + }, + "nextControl": { + "x": 2.6867941010049408, + "y": 1.7539218458865604 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.599, + "y": 0.670999999999999 + }, + "prevControl": { + "x": 2.076692256532537, + "y": 1.0270844778675974 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6, + "rotationDegrees": 85.85745820949654 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.4, + "maxWaypointRelativePos": 1.4, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 800.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 800.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 53.8 + }, + "reversed": false, + "folder": "Fast C 3 Piece", + "idealStartingState": { + "velocity": 0, + "rotation": 120.9 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP1_R1.path b/src/main/deploy/pathplanner/paths/HP1_R1.path index 6352637..6a54316 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R1.path +++ b/src/main/deploy/pathplanner/paths/HP1_R1.path @@ -57,7 +57,7 @@ "maxWaypointRelativePos": 0.5964125560538172, "constraints": { "maxVelocity": 10.0, - "maxAcceleration": 4.0, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R2.path b/src/main/deploy/pathplanner/paths/HP1_R2.path index cc15c5f..73dcfa1 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R2.path +++ b/src/main/deploy/pathplanner/paths/HP1_R2.path @@ -36,7 +36,7 @@ "maxWaypointRelativePos": 0.481165919282511, "constraints": { "maxVelocity": 10.0, - "maxAcceleration": 5.0, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R2.path b/src/main/deploy/pathplanner/paths/HP2_R2.path index c3c6f34..266c29a 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R2.path +++ b/src/main/deploy/pathplanner/paths/HP2_R2.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 2.6715, - "y": 1.5972499999999992 + "x": 2.91525, + "y": 3.362 }, "isLocked": false, "linkedName": "HP2" }, { "anchor": { - "x": 2.3594999999999997, - "y": 4.278499999999999 + "x": 2.58375, + "y": 4.688 }, "prevControl": { - "x": 2.2522499999999996, - "y": 3.8982499999999995 + "x": 2.3205000000000005, + "y": 4.200499999999999 }, "nextControl": { - "x": 2.6264921553096476, - "y": 5.225108550643294 + "x": 2.852242393984349, + "y": 5.185208137008054 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 4.991511920463674 }, "prevControl": { - "x": 3.191278569965569, - "y": 5.7528446664523525 + "x": 3.22725, + "y": 5.5264999999999995 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/PushScore.path b/src/main/deploy/pathplanner/paths/PushScore.path index bd78b8e..5804bcb 100644 --- a/src/main/deploy/pathplanner/paths/PushScore.path +++ b/src/main/deploy/pathplanner/paths/PushScore.path @@ -61,7 +61,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Fast C 3 Piece", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/R2_HP1.path b/src/main/deploy/pathplanner/paths/R2_HP1.path index 42f2a45..3ec6045 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP1.path +++ b/src/main/deploy/pathplanner/paths/R2_HP1.path @@ -48,7 +48,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 3.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 6d24c1d..a3eb481 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,14 +3,17 @@ "robotLength": 0.9, "holonomicMode": true, "pathFolders": [ + "Fast A 3 Piece", "HP to Reef", "Leave Start", - "Move to Score", "Move back from Score", + "Move to Score", + "Fast C 3 Piece", "Reef to HP", "Start to Reef" ], "autoFolders": [ + "FAST AUTOS", "LeaveStart", "StartToReef", "Test" From 16d635ba0d2a79bf0da40bf559cf8febabbf75e5 Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 28 Mar 2025 16:14:22 -0400 Subject: [PATCH 072/106] Added intake logic for autos and made autos faster --- .../pathplanner/autos/A-3PieceL4-GOOD.auto | 18 ------------------ .../{C-3PieceL4.auto => C-3PieceL4-GOOD.auto} | 18 ------------------ .../pathplanner/autos/Fast-A-3Piece.auto | 18 ------------------ .../pathplanner/autos/Fast-C-3PieceL4.auto | 18 ------------------ .../frc/robot/subsystems/arm/ArmCommands.java | 9 +++++++++ 5 files changed, 9 insertions(+), 72 deletions(-) rename src/main/deploy/pathplanner/autos/{C-3PieceL4.auto => C-3PieceL4-GOOD.auto} (87%) diff --git a/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto b/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto index bde3313..f82b017 100644 --- a/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto +++ b/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto @@ -23,12 +23,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "parallel", "data": { @@ -67,12 +61,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "parallel", "data": { @@ -115,12 +103,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/C-3PieceL4.auto b/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto similarity index 87% rename from src/main/deploy/pathplanner/autos/C-3PieceL4.auto rename to src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto index 1277524..752f62a 100644 --- a/src/main/deploy/pathplanner/autos/C-3PieceL4.auto +++ b/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto @@ -23,12 +23,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "parallel", "data": { @@ -67,12 +61,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "parallel", "data": { @@ -115,12 +103,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/Fast-A-3Piece.auto b/src/main/deploy/pathplanner/autos/Fast-A-3Piece.auto index 258a359..fd00d75 100644 --- a/src/main/deploy/pathplanner/autos/Fast-A-3Piece.auto +++ b/src/main/deploy/pathplanner/autos/Fast-A-3Piece.auto @@ -23,12 +23,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "parallel", "data": { @@ -67,12 +61,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "parallel", "data": { @@ -115,12 +103,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/Fast-C-3PieceL4.auto b/src/main/deploy/pathplanner/autos/Fast-C-3PieceL4.auto index 9269f0b..4658167 100644 --- a/src/main/deploy/pathplanner/autos/Fast-C-3PieceL4.auto +++ b/src/main/deploy/pathplanner/autos/Fast-C-3PieceL4.auto @@ -23,12 +23,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "parallel", "data": { @@ -67,12 +61,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "parallel", "data": { @@ -134,12 +122,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "Score" - } - }, { "type": "named", "data": { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 5d26839..7824e21 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -2,6 +2,7 @@ import org.littletonrobotics.junction.Logger; +import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; @@ -56,6 +57,14 @@ private Command intakeCoralWithAdjust() { return intakeCoral().andThen(moveArm(ArmPosition.Stow)); } + + // private Command intakeCoralWithAdjust() { + // if (RobotState.isAutonomous()) { + // return intakeCoral(); + // } else { + // return intakeCoral().andThen(moveArm(ArmPosition.Stow)); + // } + // } private Command intakeCoral() { Command c = new Command() { From e611fc026d7aac22c2be288b8a0a67faa7084d74 Mon Sep 17 00:00:00 2001 From: area5188 Date: Fri, 28 Mar 2025 16:29:43 -0400 Subject: [PATCH 073/106] Changed auto logic --- .../java/frc/robot/subsystems/arm/ArmCommands.java | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 7824e21..be14e5d 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -54,17 +54,12 @@ public Command intake() { } private Command intakeCoralWithAdjust() { + if (RobotState.isAutonomous()) { + return intakeCoral(); + } else { return intakeCoral().andThen(moveArm(ArmPosition.Stow)); - + } } - - // private Command intakeCoralWithAdjust() { - // if (RobotState.isAutonomous()) { - // return intakeCoral(); - // } else { - // return intakeCoral().andThen(moveArm(ArmPosition.Stow)); - // } - // } private Command intakeCoral() { Command c = new Command() { From 798a9df836120771a7999d2b8b48989f7b3fe0a6 Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 28 Mar 2025 18:09:19 -0400 Subject: [PATCH 074/106] Very slight adjustions to autos --- src/main/deploy/pathplanner/paths/Fast_C_R5.path | 6 +++--- src/main/deploy/pathplanner/paths/Fast_HP2_R.R6.path | 4 ++-- src/main/deploy/pathplanner/paths/Fast_R5_HP2.path | 8 ++++---- src/main/deploy/pathplanner/paths/PushScore.path | 2 +- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Fast_C_R5.path b/src/main/deploy/pathplanner/paths/Fast_C_R5.path index 0910852..0cfa84d 100644 --- a/src/main/deploy/pathplanner/paths/Fast_C_R5.path +++ b/src/main/deploy/pathplanner/paths/Fast_C_R5.path @@ -38,10 +38,10 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0.39865470852017926, + "maxWaypointRelativePos": 0.2605381165919283, "constraints": { "maxVelocity": 10.0, - "maxAcceleration": 5.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -53,7 +53,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 3.5, + "maxAcceleration": 3.25, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Fast_HP2_R.R6.path b/src/main/deploy/pathplanner/paths/Fast_HP2_R.R6.path index ff7bea2..b2885d2 100644 --- a/src/main/deploy/pathplanner/paths/Fast_HP2_R.R6.path +++ b/src/main/deploy/pathplanner/paths/Fast_HP2_R.R6.path @@ -38,7 +38,7 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0.48654708520179374, + "maxWaypointRelativePos": 0.28565022421524683, "constraints": { "maxVelocity": 10.0, "maxAcceleration": 5.0, @@ -53,7 +53,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Fast_R5_HP2.path b/src/main/deploy/pathplanner/paths/Fast_R5_HP2.path index 92da981..e70270c 100644 --- a/src/main/deploy/pathplanner/paths/Fast_R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/Fast_R5_HP2.path @@ -46,15 +46,15 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.6, - "rotationDegrees": 85.85745820949654 + "waypointRelativePos": 1.2350476190476192, + "rotationDegrees": 50.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.4, - "maxWaypointRelativePos": 1.4, + "minWaypointRelativePos": 0.5318385650224343, + "maxWaypointRelativePos": 2.0, "constraints": { "maxVelocity": 10.0, "maxAcceleration": 5.0, diff --git a/src/main/deploy/pathplanner/paths/PushScore.path b/src/main/deploy/pathplanner/paths/PushScore.path index 5804bcb..bd78b8e 100644 --- a/src/main/deploy/pathplanner/paths/PushScore.path +++ b/src/main/deploy/pathplanner/paths/PushScore.path @@ -61,7 +61,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Fast C 3 Piece", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 180.0 From 20587c9810ab9ce97517af98fefb4e26a90b86a9 Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 28 Mar 2025 19:43:17 -0400 Subject: [PATCH 075/106] Auto Changes --- .../pathplanner/autos/A-3PieceL4-GOOD.auto | 18 ++++++++++++++++++ src/main/deploy/pathplanner/paths/A_R3.path | 2 +- .../pathplanner/paths/Fast_L.R2_HP1.path | 2 +- src/main/deploy/pathplanner/paths/HP1_R2.path | 15 ++++++++++++++- 4 files changed, 34 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto b/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto index f82b017..88294e0 100644 --- a/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto +++ b/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto @@ -23,6 +23,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Score" + } + }, { "type": "parallel", "data": { @@ -61,6 +67,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Score" + } + }, { "type": "parallel", "data": { @@ -99,6 +111,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Score" + } + }, { "type": "sequential", "data": { diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index 330b021..a2747f4 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -30,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.5, + "waypointRelativePos": 0.18780952380952365, "rotationDegrees": -121.0 } ], diff --git a/src/main/deploy/pathplanner/paths/Fast_L.R2_HP1.path b/src/main/deploy/pathplanner/paths/Fast_L.R2_HP1.path index da9a09d..f3b273a 100644 --- a/src/main/deploy/pathplanner/paths/Fast_L.R2_HP1.path +++ b/src/main/deploy/pathplanner/paths/Fast_L.R2_HP1.path @@ -48,7 +48,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R2.path b/src/main/deploy/pathplanner/paths/HP1_R2.path index 73dcfa1..16c1a6a 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R2.path +++ b/src/main/deploy/pathplanner/paths/HP1_R2.path @@ -42,13 +42,26 @@ "nominalVoltage": 12.0, "unlimited": false } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7484304932735449, + "maxWaypointRelativePos": 0.9959641255605383, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } } ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 3.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, From 736f5aa6fa35c842da149d39fc148dc72ff5adf5 Mon Sep 17 00:00:00 2001 From: area5188 Date: Sat, 29 Mar 2025 08:23:46 -0400 Subject: [PATCH 076/106] Changed coral spit speed --- src/main/java/frc/robot/subsystems/arm/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index d692c27..d5c0600 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -96,7 +96,7 @@ public void setIntakeSpeed(double speed) { } public void spit() { - double speed = (_currentMode == GamepieceMode.ALGAE) ? -0.5 : 0.5; + double speed = (_currentMode == GamepieceMode.ALGAE) ? -0.5 : 0.4; setIntakeSpeed(speed); } From 2446d3c29e60e2e71878f1fd366959b38180245e Mon Sep 17 00:00:00 2001 From: shlap Date: Sat, 29 Mar 2025 08:33:05 -0400 Subject: [PATCH 077/106] Slowed down auto for precision --- src/main/deploy/pathplanner/paths/A_R3.path | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index a2747f4..c250c80 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -39,7 +39,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, From dcc785c26613f0980122b81fea974ac594ef6f7e Mon Sep 17 00:00:00 2001 From: shlap Date: Sat, 29 Mar 2025 09:56:59 -0400 Subject: [PATCH 078/106] Slowed down C piece auto --- src/main/deploy/pathplanner/paths/C_R5.path | 18 ++---------------- 1 file changed, 2 insertions(+), 16 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index 88bbfd2..a0c8922 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -34,26 +34,12 @@ "rotationDegrees": 162.15573333673396 } ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0.39865470852017926, - "constraints": { - "maxVelocity": 10.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 3.5, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, From 1806bce3949f822dc72c7d6ee6d41186847d9ee2 Mon Sep 17 00:00:00 2001 From: shlap Date: Sat, 29 Mar 2025 13:55:02 -0400 Subject: [PATCH 079/106] test auto --- .../deploy/pathplanner/autos/Test Auto.auto | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/Test Auto.auto diff --git a/src/main/deploy/pathplanner/autos/Test Auto.auto b/src/main/deploy/pathplanner/autos/Test Auto.auto new file mode 100644 index 0000000..5e2a2e3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "named", + "data": { + "name": "L2" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Test", + "choreoAuto": false +} \ No newline at end of file From af5a72fddade1eaa3ced570869c07f6d286e00a2 Mon Sep 17 00:00:00 2001 From: shlap Date: Sat, 29 Mar 2025 14:02:28 -0400 Subject: [PATCH 080/106] Made autos wait --- .../pathplanner/autos/A-3PieceL4-GOOD.auto | 19 -------------- .../pathplanner/autos/C-3PieceL4-GOOD.auto | 13 ---------- .../deploy/pathplanner/autos/Test Auto.auto | 25 ------------------- 3 files changed, 57 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Test Auto.auto diff --git a/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto b/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto index 88294e0..ec86307 100644 --- a/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto +++ b/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto @@ -102,25 +102,6 @@ "pathName": "Fast_HP1_L.R2" } }, - { - "type": "named", - "data": { - "name": "L4" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Score" - } - }, - { - "type": "sequential", - "data": { - "commands": [ { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto b/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto index 752f62a..17dc1a6 100644 --- a/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto +++ b/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto @@ -90,19 +90,6 @@ "pathName": "Fast_HP2_L.R6" } }, - { - "type": "named", - "data": { - "name": "L4" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/Test Auto.auto b/src/main/deploy/pathplanner/autos/Test Auto.auto deleted file mode 100644 index 5e2a2e3..0000000 --- a/src/main/deploy/pathplanner/autos/Test Auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L2" - } - } - ] - } - }, - "resetOdom": true, - "folder": "Test", - "choreoAuto": false -} \ No newline at end of file From 6fe99b86eb3c16e6af9fce7c2c5b9865f777d1ff Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Mon, 31 Mar 2025 18:10:51 -0400 Subject: [PATCH 081/106] Cross-eyed camera constants --- .../subsystems/vision/VisionConstants.java | 26 ++++++------------- 1 file changed, 8 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 2c116fa..7bac767 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -44,26 +44,16 @@ public class VisionConstants { // Note: 0.0254 multiplier converts inches to meters // Camera 5 - // Facing forward, not tilted + // Front right camera public static Transform3d robotToCamera5 = - new Transform3d(Units.inchesToMeters(13.25), Units.inchesToMeters(-11.5), Units.inchesToMeters(8.5), - new Rotation3d(0.0, 0, 0)); + new Transform3d(Units.inchesToMeters(9.287), Units.inchesToMeters(10.9704),Units.inchesToMeters(7.9167), + new Rotation3d(0.0, Units.degreesToRadians(-15), Units.degreesToRadians(30))); - // Camera 6 (Back left swerve module) - // Tilted up 10 degrees, facing backwards - public static Transform3d robotToCamera6 = - new Transform3d(Units.inchesToMeters(-12.75), Units.inchesToMeters(12.5), Units.inchesToMeters(8.75), - new Rotation3d(0.0, Units.degreesToRadians(+20.0), Units.degreesToRadians(135))); - -// // Camera 7 -// public static Transform3d robotToCamera7 = -// new Transform3d(Units.inchesToMeters(14.125), Units.inchesToMeters(-8.5), Units.inchesToMeters(7.75), -// new Rotation3d(0.0, 0, 0)); - -// // Camera 8 -// public static Transform3d robotToCamera8 = -// new Transform3d(Units.inchesToMeters(14.125), Units.inchesToMeters(11.5), Units.inchesToMeters(8.25), -// new Rotation3d(0, Units.degreesToRadians(-10), 0)); + // Camera 6 + // Front left camera + public static Transform3d robotToCamera6 = + new Transform3d(Units.inchesToMeters(9.287), Units.inchesToMeters(-10.9704), Units.inchesToMeters(7.9167), + new Rotation3d(0.0, Units.degreesToRadians(-15), Units.degreesToRadians(-30))); // Basic filtering thresholds public static double maxAmbiguity = 0.3; From f5c743d40d3cf9e4bb6762c87f48c3e8dc974b4e Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Mon, 31 Mar 2025 19:19:15 -0400 Subject: [PATCH 082/106] oops (swap Y negative signs for cameras) --- .../java/frc/robot/subsystems/vision/VisionConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 7bac767..f0bc07e 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -46,14 +46,14 @@ public class VisionConstants { // Camera 5 // Front right camera public static Transform3d robotToCamera5 = - new Transform3d(Units.inchesToMeters(9.287), Units.inchesToMeters(10.9704),Units.inchesToMeters(7.9167), + new Transform3d(Units.inchesToMeters(9.287), Units.inchesToMeters(-10.9704),Units.inchesToMeters(7.9167), new Rotation3d(0.0, Units.degreesToRadians(-15), Units.degreesToRadians(30))); // Camera 6 // Front left camera public static Transform3d robotToCamera6 = - new Transform3d(Units.inchesToMeters(9.287), Units.inchesToMeters(-10.9704), Units.inchesToMeters(7.9167), - new Rotation3d(0.0, Units.degreesToRadians(-15), Units.degreesToRadians(-30))); + new Transform3d(Units.inchesToMeters(9.287), Units.inchesToMeters(10.9704), Units.inchesToMeters(7.9167), + new Rotation3d(0.0, Units.degreesToRadians(-15), Units.degreesToRadians(-30))); // Basic filtering thresholds public static double maxAmbiguity = 0.3; From 02e9026f8ed4f70d26a737069d7778d182e81354 Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Mon, 31 Mar 2025 20:20:43 -0400 Subject: [PATCH 083/106] (Untested!) LEDs for aligning --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 18 +- .../java/frc/robot/subsystems/leds/LEDs.java | 14 ++ .../robot/subsystems/leds/LEDsCommands.java | 174 ++++++++++-------- 4 files changed, 127 insertions(+), 81 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 55e282b..1af306c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -244,7 +244,7 @@ private Command joystickApproach(Supplier approachPose) return DriveCommands.joystickApproach( drive, () -> -joystick.getLeftY() * speedMultiplier, - approachPose); + approachPose).alongWith(LEDsCommands.aligningWithReef()); } private void configureBindings() { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9f7b52d..130b710 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -42,6 +42,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; @@ -49,6 +50,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.FieldConstants; import frc.robot.HardwareConstants; import frc.robot.HardwareConstants.Mode; import frc.robot.subsystems.drive.io.GyroIO; @@ -79,7 +81,8 @@ public class Drive extends SubsystemBase { private static final double ROBOT_MOI = 5.10; private static final double WHEEL_COF = 1.13; - + // For use in getCloseToReef() method: if robot is less than this distance from the reef, LEDs turn green + private static final double MAX_DISTANCE_TO_REEF = Units.inchesToMeters(50); static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; @@ -393,6 +396,19 @@ public void addVisionMeasurement( visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } + public boolean getCloseToReef() { + + Translation2d reefTranslation = FieldConstants.Reef.centerOfReef; + double distanceToReef = getPose().getTranslation().getDistance(reefTranslation); + + // If robot is within MAX_DISTANCE_TO_REEF from center of reef, LEDs can turn green. + if (distanceToReef <= MAX_DISTANCE_TO_REEF) { + return true; + } + + return false; + } + /** Returns the maximum linear speed in meters per sec. */ public double getMaxLinearSpeedMetersPerSec() { return TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java index 11da210..336fa0b 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java @@ -11,6 +11,8 @@ import frc.robot.HardwareConstants; import frc.robot.HardwareConstants.CAN; import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveCommands; import com.ctre.phoenix.led.RainbowAnimation; import com.ctre.phoenix.led.StrobeAnimation; @@ -26,6 +28,7 @@ public class LEDs extends SubsystemBase { boolean _alreadyRunning = false; LEDAnimation _currentAnimation = LEDAnimation.None; + Drive _drive; Elevator _elevator; public enum LEDAnimation { @@ -156,6 +159,17 @@ public void robotHasClimbed() { } } + public void aligningWithReefAnimation() { + // If close enough to reef: + if (_drive.getCloseToReef()) { + runAnimation(LEDAnimation.SolidGreen); + } + + else { + runAnimation(LEDAnimation.SolidRed); + } + } + public void disabledAnimation1() { if (!_alreadyRunning) { runAnimation(LEDAnimation.PartyMode); diff --git a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java index 1649a9d..c300e02 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java @@ -8,96 +8,112 @@ //import frc.robot.subsystems.elevator.Elevator; public class LEDsCommands { - private LEDs _leds; - - public LEDsCommands(LEDs leds) { - _leds = leds; - } - - //Runs when intake button is pushed - public Command intaking() { + private static LEDs _leds; + + public LEDsCommands(LEDs leds) { + _leds = leds; + } + + //Runs when intake button is pushed + public Command intaking() { + return new StartEndCommand( + () -> { + _leds.intaking(); + }, + () -> { + _leds.reset(); + }, + _leds); + } + + //Runs when we have just acquired a piece + public Command hasPiece() { + return new StartEndCommand( + () -> { + _leds.hasPiece(); + }, + () -> { + _leds.reset(); + }, + _leds).withTimeout(3); + } + + // Runs when we have no gamepiece and we are toggled to pick up coral + public Command pickingUpCoral() { + return new StartEndCommand( + () -> { + _leds.pickingUpCoral(); + }, + () -> { + _leds.reset(); + }, + _leds); + } + + // Runs when we have no gamepiece and we are toggled to pick up coral + public Command pickingUpAlgae() { + return new StartEndCommand( + () -> { + _leds.pickingUpAlgae(); + }, + () -> { + _leds.reset(); + }, + _leds); + } + + //We have a piece and hasPiece animation has already run + public Command elevatorOrArmIsMoving() { + return new StartEndCommand( + () -> { + _leds.elevatorOrArmIsMoving(); + }, + () -> { + _leds.reset(); + }, + _leds); + } + + //Runs when robot has finished climbing + public Command robotHasClimbed() { return new StartEndCommand( () -> { - _leds.intaking(); + _leds.robotHasClimbed(); }, () -> { _leds.reset(); }, _leds); - } - - //Runs when we have just acquired a piece - public Command hasPiece() { - return new StartEndCommand( - () -> { - _leds.hasPiece(); - }, - () -> { - _leds.reset(); - }, - _leds).withTimeout(3); - } + } - // Runs when we have no gamepiece and we are toggled to pick up coral - public Command pickingUpCoral() { - return new StartEndCommand( - () -> { - _leds.pickingUpCoral(); - }, - () -> { - _leds.reset(); - }, - _leds); - } - - // Runs when we have no gamepiece and we are toggled to pick up coral - public Command pickingUpAlgae() { - return new StartEndCommand( - () -> { - _leds.pickingUpAlgae(); - }, - () -> { - _leds.reset(); - }, - _leds); - } + public Command disabledAnimation1() { + return new StartEndCommand( + () -> { + _leds.disabledAnimation1(); + }, + () -> { + _leds.reset(); + }, + _leds) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }; + } - //We have a piece and hasPiece animation has already run - public Command elevatorOrArmIsMoving() { - return new StartEndCommand( - () -> { - _leds.elevatorOrArmIsMoving(); - }, - () -> { - _leds.reset(); - }, - _leds); - } - - //Runs when robot has finished climbing - public Command robotHasClimbed() { - return new StartEndCommand( - () -> { - _leds.robotHasClimbed(); - }, - () -> { - _leds.reset(); - }, - _leds); - } - - public Command disabledAnimation1() { - return new StartEndCommand( - () -> { - _leds.disabledAnimation1(); - }, - () -> { - _leds.reset(); - }, - _leds) { + public static Command aligningWithReef() { + return new StartEndCommand( + () -> { + _leds.aligningWithReefAnimation(); + }, + () -> { + _leds.reset(); + }, + _leds) { @Override public boolean runsWhenDisabled() { - return true; + return false; } }; } From 3e3f23b8125c0af9f6343e5a9af5bb4bc4654bcb Mon Sep 17 00:00:00 2001 From: area5188 Date: Mon, 31 Mar 2025 20:23:48 -0400 Subject: [PATCH 084/106] Changed L4 Setpoint --- src/main/java/frc/robot/subsystems/arm/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index d5c0600..50c2a49 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -27,7 +27,7 @@ public class Arm extends SubsystemBase { public enum ArmPosition { Stow(114, 114), Loading(140, 78), - L4_Score(100, 102), // 86 + L4_Score(97, 102), // 86 Algae_Score(100, 100), Transient(108, 108), Climbing(50, 50); From e9ccfba1e7c6d1ce6785dcf493189be1a2110118 Mon Sep 17 00:00:00 2001 From: area5188 Date: Tue, 1 Apr 2025 13:51:15 -0400 Subject: [PATCH 085/106] switched encoder reading to secondary --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/elevator/io/RealElevatorIO.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1af306c..87f74f1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -271,7 +271,7 @@ private void configureBindings() { // drive.registerTelemetry(logger::telemeterize); - intakeButton.onTrue(multiSubsystemCommands.loadCoral().unless(() -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE));//.raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving())); + intakeButton.onTrue(multiSubsystemCommands.loadCoral().raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving()).unless(() -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); spitButton.onTrue(armCommands.spit()); StowButton.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.Stow)); diff --git a/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java index 1bf6639..9d6172b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java @@ -35,7 +35,7 @@ public RealElevatorIO() { public void updateInputs(ElevatorIOInputs inputs) { inputs._elevatorMotorVoltage = _primaryMotor.getAppliedOutput() * _primaryMotor.getBusVoltage(); inputs._elevatorMotorCurrent = _primaryMotor.getOutputCurrent(); - inputs._elevatorPosition = _primaryMotor.getEncoder().getPosition(); + inputs._elevatorPosition = _secondaryMotor.getEncoder().getPosition(); inputs._elevatorSpeed = _primaryMotor.get(); inputs._elevatorVelocity = _primaryMotor.getEncoder().getVelocity(); } From 6bc7ca0e8d0e85be76a9242d4968db5abe56ef5b Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Tue, 1 Apr 2025 16:25:31 -0400 Subject: [PATCH 086/106] Revert "Merge remote-tracking branch 'origin/main' into REEF-251-implement-transient-position" This reverts commit 5c638d3c621d0fde6fb3a5aeafbf28b9b513b51f, reversing changes made to e9ccfba1e7c6d1ce6785dcf493189be1a2110118. --- .vscode/settings.json | 3 +- build.gradle | 6 +- gradlew | 0 simgui-ds.json | 5 + .../deploy/pathplanner/autos/3Piece-A.auto | 73 ------ src/main/deploy/pathplanner/paths/A_R1.path | 2 +- src/main/deploy/pathplanner/paths/A_R2.path | 2 +- src/main/deploy/pathplanner/paths/A_R4.path | 2 +- src/main/deploy/pathplanner/paths/B_R2.path | 2 +- src/main/deploy/pathplanner/paths/B_R3.path | 2 +- src/main/deploy/pathplanner/paths/B_R4.path | 2 +- src/main/deploy/pathplanner/paths/B_R5.path | 2 +- src/main/deploy/pathplanner/paths/C_R3.path | 2 +- src/main/deploy/pathplanner/paths/C_R4.path | 2 +- src/main/deploy/pathplanner/paths/C_R6.path | 2 +- src/main/deploy/pathplanner/paths/HP1_R3.path | 2 +- src/main/deploy/pathplanner/paths/HP1_R4.path | 2 +- src/main/deploy/pathplanner/paths/HP1_R5.path | 2 +- src/main/deploy/pathplanner/paths/HP1_R6.path | 2 +- src/main/deploy/pathplanner/paths/HP2_R1.path | 2 +- src/main/deploy/pathplanner/paths/HP2_R2.path | 2 +- src/main/deploy/pathplanner/paths/HP2_R3.path | 2 +- src/main/deploy/pathplanner/paths/HP2_R4.path | 2 +- src/main/deploy/pathplanner/paths/LeaveA.path | 2 +- src/main/deploy/pathplanner/paths/LeaveB.path | 2 +- src/main/deploy/pathplanner/paths/LeaveC.path | 2 +- src/main/deploy/pathplanner/paths/R1_HP1.path | 2 +- src/main/deploy/pathplanner/paths/R1_HP2.path | 2 +- src/main/deploy/pathplanner/paths/R2_HP2.path | 2 +- src/main/deploy/pathplanner/paths/R3_HP2.path | 2 +- src/main/deploy/pathplanner/paths/R4_HP1.path | 2 +- src/main/deploy/pathplanner/paths/R4_HP2.path | 2 +- src/main/deploy/pathplanner/paths/R5_HP1.path | 2 +- src/main/deploy/pathplanner/paths/R6_HP1.path | 2 +- src/main/deploy/pathplanner/settings.json | 10 +- src/main/java/frc/robot/FieldConstants.java | 241 ++++++++++++++++++ .../java/frc/robot/HardwareConstants.java | 3 + src/main/java/frc/robot/Robot.java | 14 +- src/main/java/frc/robot/RobotContainer.java | 97 +++---- .../frc/robot/subsystems/arm/ArmCommands.java | 11 - .../robot/subsystems/arm/{ => io}/ArmIO.java | 6 +- .../subsystems/arm/{ => io}/RealArmIO.java | 7 +- .../robot/subsystems/climber/ClimberIO.java | 16 -- .../subsystems/climber/RealClimberIO.java | 30 --- ....java => CommandSwerveDrivetrain.java.old} | 0 .../frc/robot/subsystems/drive/Drive.java | 57 ++++- .../robot/subsystems/drive/DriveCommands.java | 111 +++++++- .../subsystems/drive/TunerConstants.java | 9 +- .../subsystems/elevator/ElevatorCommands.java | 1 - .../elevator/{ => io}/ElevatorIO.java | 2 +- .../elevator/{ => io}/RealElevatorIO.java | 4 +- .../frc/robot/subsystems/vision/VisionIO.java | 52 ++++ .../subsystems/vision/VisionIOLimelight.java | 159 ++++++++++++ .../vision/VisionIOPhotonVision.java | 149 +++++++++++ .../vision/VisionIOPhotonVisionSim.java | 63 +++++ .../frc/robot/util/LoggedTunableNumber.java | 133 ++++++++++ .../frc/robot/util/TuneableProfiledPID.java | 51 ++++ ....2.1.json => PathplannerLib-2025.2.5.json} | 8 +- ...enix6-25.2.1.json => Phoenix6-25.3.1.json} | 88 ++++--- .../{REVLib-2025.0.1.json => REVLib.json} | 12 +- vendordeps/photonlib.json | 71 ++++++ 61 files changed, 1243 insertions(+), 307 deletions(-) mode change 100644 => 100755 gradlew delete mode 100644 src/main/deploy/pathplanner/autos/3Piece-A.auto create mode 100644 src/main/java/frc/robot/FieldConstants.java rename src/main/java/frc/robot/subsystems/arm/{ => io}/ArmIO.java (85%) rename src/main/java/frc/robot/subsystems/arm/{ => io}/RealArmIO.java (97%) delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIO.java delete mode 100644 src/main/java/frc/robot/subsystems/climber/RealClimberIO.java rename src/main/java/frc/robot/subsystems/drive/{CommandSwerveDrivetrain.java => CommandSwerveDrivetrain.java.old} (100%) rename src/main/java/frc/robot/subsystems/elevator/{ => io}/ElevatorIO.java (94%) rename src/main/java/frc/robot/subsystems/elevator/{ => io}/RealElevatorIO.java (96%) create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java create mode 100644 src/main/java/frc/robot/util/LoggedTunableNumber.java create mode 100644 src/main/java/frc/robot/util/TuneableProfiledPID.java rename vendordeps/{PathplannerLib-2025.2.1.json => PathplannerLib-2025.2.5.json} (87%) rename vendordeps/{Phoenix6-25.2.1.json => Phoenix6-25.3.1.json} (86%) rename vendordeps/{REVLib-2025.0.1.json => REVLib.json} (90%) create mode 100644 vendordeps/photonlib.json diff --git a/.vscode/settings.json b/.vscode/settings.json index 612cdd0..1745ba0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/build.gradle b/build.gradle index 0ab3b81..dcbfde6 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.3.1" } java { @@ -33,7 +33,7 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + deleteOldFiles = true // Change to true to delete files on roboRIO that no // longer exist in deploy directory of this project } } @@ -47,7 +47,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..4a63cc1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/deploy/pathplanner/autos/3Piece-A.auto b/src/main/deploy/pathplanner/autos/3Piece-A.auto deleted file mode 100644 index cc12487..0000000 --- a/src/main/deploy/pathplanner/autos/3Piece-A.auto +++ /dev/null @@ -1,73 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "A_R1" - } - }, - { - "type": "named", - "data": { - "name": "L1" - } - }, - { - "type": "path", - "data": { - "pathName": "R1_HP1" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "path", - "data": { - "pathName": "HP1_R2" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "path", - "data": { - "pathName": "R2_HP1" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "path", - "data": { - "pathName": "HP1_R3" - } - }, - { - "type": "named", - "data": { - "name": "L1" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A_R1.path b/src/main/deploy/pathplanner/paths/A_R1.path index 6304aa5..96197e9 100644 --- a/src/main/deploy/pathplanner/paths/A_R1.path +++ b/src/main/deploy/pathplanner/paths/A_R1.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/A_R2.path b/src/main/deploy/pathplanner/paths/A_R2.path index 2832b6e..e252522 100644 --- a/src/main/deploy/pathplanner/paths/A_R2.path +++ b/src/main/deploy/pathplanner/paths/A_R2.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/A_R4.path b/src/main/deploy/pathplanner/paths/A_R4.path index a6f2ca4..4f0ed62 100644 --- a/src/main/deploy/pathplanner/paths/A_R4.path +++ b/src/main/deploy/pathplanner/paths/A_R4.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/B_R2.path b/src/main/deploy/pathplanner/paths/B_R2.path index 0bfec60..2467a94 100644 --- a/src/main/deploy/pathplanner/paths/B_R2.path +++ b/src/main/deploy/pathplanner/paths/B_R2.path @@ -38,7 +38,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/B_R3.path b/src/main/deploy/pathplanner/paths/B_R3.path index 9211d39..d371ede 100644 --- a/src/main/deploy/pathplanner/paths/B_R3.path +++ b/src/main/deploy/pathplanner/paths/B_R3.path @@ -38,7 +38,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/B_R4.path b/src/main/deploy/pathplanner/paths/B_R4.path index 2b12667..97f04b6 100644 --- a/src/main/deploy/pathplanner/paths/B_R4.path +++ b/src/main/deploy/pathplanner/paths/B_R4.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/B_R5.path b/src/main/deploy/pathplanner/paths/B_R5.path index 3855ee8..e257ca0 100644 --- a/src/main/deploy/pathplanner/paths/B_R5.path +++ b/src/main/deploy/pathplanner/paths/B_R5.path @@ -38,7 +38,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/C_R3.path b/src/main/deploy/pathplanner/paths/C_R3.path index 978a532..56c00d0 100644 --- a/src/main/deploy/pathplanner/paths/C_R3.path +++ b/src/main/deploy/pathplanner/paths/C_R3.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/C_R4.path b/src/main/deploy/pathplanner/paths/C_R4.path index aeb6f69..c68b571 100644 --- a/src/main/deploy/pathplanner/paths/C_R4.path +++ b/src/main/deploy/pathplanner/paths/C_R4.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/C_R6.path b/src/main/deploy/pathplanner/paths/C_R6.path index 1ae2c38..e1442c3 100644 --- a/src/main/deploy/pathplanner/paths/C_R6.path +++ b/src/main/deploy/pathplanner/paths/C_R6.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R3.path b/src/main/deploy/pathplanner/paths/HP1_R3.path index dabf0a0..ba9bc8d 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R3.path +++ b/src/main/deploy/pathplanner/paths/HP1_R3.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R4.path b/src/main/deploy/pathplanner/paths/HP1_R4.path index 01efd97..a7f3699 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R4.path +++ b/src/main/deploy/pathplanner/paths/HP1_R4.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R5.path b/src/main/deploy/pathplanner/paths/HP1_R5.path index 3d5fd31..cfbe7df 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R5.path +++ b/src/main/deploy/pathplanner/paths/HP1_R5.path @@ -70,7 +70,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R6.path b/src/main/deploy/pathplanner/paths/HP1_R6.path index 682423d..0550f5d 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R6.path +++ b/src/main/deploy/pathplanner/paths/HP1_R6.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R1.path b/src/main/deploy/pathplanner/paths/HP2_R1.path index 028e198..781ea8e 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R1.path +++ b/src/main/deploy/pathplanner/paths/HP2_R1.path @@ -38,7 +38,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R2.path b/src/main/deploy/pathplanner/paths/HP2_R2.path index 68ecf19..266c29a 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R2.path +++ b/src/main/deploy/pathplanner/paths/HP2_R2.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R3.path b/src/main/deploy/pathplanner/paths/HP2_R3.path index 05c46c8..b2758b8 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R3.path +++ b/src/main/deploy/pathplanner/paths/HP2_R3.path @@ -70,7 +70,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R4.path b/src/main/deploy/pathplanner/paths/HP2_R4.path index ea711b9..05cbed7 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R4.path +++ b/src/main/deploy/pathplanner/paths/HP2_R4.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/LeaveA.path b/src/main/deploy/pathplanner/paths/LeaveA.path index 6fc4c5c..c4f8eef 100644 --- a/src/main/deploy/pathplanner/paths/LeaveA.path +++ b/src/main/deploy/pathplanner/paths/LeaveA.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/LeaveB.path b/src/main/deploy/pathplanner/paths/LeaveB.path index 6626cee..93fc446 100644 --- a/src/main/deploy/pathplanner/paths/LeaveB.path +++ b/src/main/deploy/pathplanner/paths/LeaveB.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/LeaveC.path b/src/main/deploy/pathplanner/paths/LeaveC.path index b4792b0..d696859 100644 --- a/src/main/deploy/pathplanner/paths/LeaveC.path +++ b/src/main/deploy/pathplanner/paths/LeaveC.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R1_HP1.path b/src/main/deploy/pathplanner/paths/R1_HP1.path index c277832..863af19 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP1.path +++ b/src/main/deploy/pathplanner/paths/R1_HP1.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R1_HP2.path b/src/main/deploy/pathplanner/paths/R1_HP2.path index 2c91815..0b0ec8a 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP2.path +++ b/src/main/deploy/pathplanner/paths/R1_HP2.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R2_HP2.path b/src/main/deploy/pathplanner/paths/R2_HP2.path index 122af47..d821e2e 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP2.path +++ b/src/main/deploy/pathplanner/paths/R2_HP2.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R3_HP2.path b/src/main/deploy/pathplanner/paths/R3_HP2.path index 63619c9..cac1ec6 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP2.path +++ b/src/main/deploy/pathplanner/paths/R3_HP2.path @@ -70,7 +70,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R4_HP1.path b/src/main/deploy/pathplanner/paths/R4_HP1.path index d692105..aac7b48 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP1.path +++ b/src/main/deploy/pathplanner/paths/R4_HP1.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R4_HP2.path b/src/main/deploy/pathplanner/paths/R4_HP2.path index b65b49e..8c1f078 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP2.path +++ b/src/main/deploy/pathplanner/paths/R4_HP2.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R5_HP1.path b/src/main/deploy/pathplanner/paths/R5_HP1.path index b56c1c3..99dbcdb 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP1.path +++ b/src/main/deploy/pathplanner/paths/R5_HP1.path @@ -70,7 +70,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R6_HP1.path b/src/main/deploy/pathplanner/paths/R6_HP1.path index 4992825..f30d26d 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP1.path +++ b/src/main/deploy/pathplanner/paths/R6_HP1.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 7d731aa..a3eb481 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -23,12 +23,12 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, - "robotMOI": 6.883, + "robotMass": 50.0, + "robotMOI": 5.1, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, - "maxDriveSpeed": 5.45, + "driveWheelRadius": 0.049, + "driveGearing": 6.122, + "maxDriveSpeed": 5.0, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java new file mode 100644 index 0000000..3c0df25 --- /dev/null +++ b/src/main/java/frc/robot/FieldConstants.java @@ -0,0 +1,241 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.util.Units; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * Contains various field dimensions and useful reference points. All units are in meters and poses + * have a blue alliance origin. + */ +public class FieldConstants { + public static final double fieldLength = Units.inchesToMeters(690.876); + public static final double fieldWidth = Units.inchesToMeters(317); + public static final Translation2d fieldCenter = + new Translation2d(fieldLength / 2, fieldWidth / 2); + public static final double startingLineX = + Units.inchesToMeters(299.438); // Measured from the inside of starting + // line + + public static class Processor { + public static final Pose2d centerFace = + new Pose2d(Units.inchesToMeters(235.726), 0, Rotation2d.fromDegrees(90)); + } + + public static class Barge { + public static final Translation2d farCage = + new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(286.779)); + public static final Translation2d middleCage = + new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(242.855)); + public static final Translation2d closeCage = + new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(199.947)); + + // Measured from floor to bottom of cage + public static final double deepHeight = Units.inchesToMeters(3.125); + public static final double shallowHeight = Units.inchesToMeters(30.125); + } + + public static class CoralStation { + public static final Pose2d leftCenterFace = + new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(291.176), + Rotation2d.fromDegrees(90 - 144.011)); + public static final Pose2d rightCenterFace = + new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(25.824), + Rotation2d.fromDegrees(144.011 - 90)); + } + + public static class Reef { + public static final Translation2d centerOfReef = + new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); + public static final double faceToZoneLine = + Units.inchesToMeters(12); // Side of the reef to the inside of the + // reef zone line + + public static final Pose2d[] centerFaces = + new Pose2d[12]; // Starting facing the driver station in clockwise + // order + public static final List> branchPositions = + new ArrayList<>(); // Starting at the right + // branch facing the + // driver station in + // clockwise + + static { + // Initialize faces + centerFaces[0] = + new Pose2d( + Units.inchesToMeters(144.003), + Units.inchesToMeters(158.500), + Rotation2d.fromDegrees(180)); + centerFaces[1] = + new Pose2d( + Units.inchesToMeters(160.373), + Units.inchesToMeters(186.857), + Rotation2d.fromDegrees(120)); + centerFaces[2] = + new Pose2d( + Units.inchesToMeters(193.116), + Units.inchesToMeters(186.858), + Rotation2d.fromDegrees(60)); + centerFaces[3] = + new Pose2d( + Units.inchesToMeters(209.489), + Units.inchesToMeters(158.502), + Rotation2d.fromDegrees(0)); + centerFaces[4] = + new Pose2d( + Units.inchesToMeters(193.118), + Units.inchesToMeters(130.145), + Rotation2d.fromDegrees(-60)); + centerFaces[5] = + new Pose2d( + Units.inchesToMeters(160.375), + Units.inchesToMeters(130.144), + Rotation2d.fromDegrees(-120)); + + centerFaces[6] = centerFaces[0].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[7] = centerFaces[1].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[8] = centerFaces[2].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[9] = centerFaces[3].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[10] = centerFaces[4].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[11] = centerFaces[5].rotateAround(fieldCenter, Rotation2d.k180deg); + + // Initialize branch positions + for (int face = 0; face < centerFaces.length; face++) { + Map fillRight = new HashMap<>(); + Map fillLeft = new HashMap<>(); + for (var level : ReefHeight.values()) { + Pose2d poseDirection = new Pose2d(); + if (face < 6) { + poseDirection = + new Pose2d(centerOfReef, centerFaces[face].getRotation()); + } else { + poseDirection = + new Pose2d(centerOfReef.rotateAround(fieldCenter, Rotation2d.k180deg), + centerFaces[face].getRotation()); + } + + double adjustX = Units.inchesToMeters(30.738); // Depth of branch from reef face + double adjustY = Units.inchesToMeters(6.469); // Offset from reef face + // centerline to branch + + fillRight.put( + level, + new Pose3d( + new Translation3d( + poseDirection + .transformBy( + new Transform2d(adjustX, adjustY, new Rotation2d())) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustX, adjustY, new Rotation2d())) + .getY(), + level.height), + new Rotation3d( + 0, + Units.degreesToRadians(level.pitch), + poseDirection.getRotation().getRadians()))); + fillLeft.put( + level, + new Pose3d( + new Translation3d( + poseDirection + .transformBy( + new Transform2d(adjustX, -adjustY, new Rotation2d())) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustX, -adjustY, new Rotation2d())) + .getY(), + level.height), + new Rotation3d( + 0, + Units.degreesToRadians(level.pitch), + poseDirection.getRotation().getRadians()))); + } + branchPositions.add(fillRight); + branchPositions.add(fillLeft); + } + + + } + } + + public static class StagingPositions { + // Measured from the center of the ice cream + public static final Pose2d leftIceCream = + new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(230.5), new Rotation2d()); + public static final Pose2d middleIceCream = + new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(158.5), new Rotation2d()); + public static final Pose2d rightIceCream = + new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(86.5), new Rotation2d()); + } + + public enum ReefHeight { + L4(Units.inchesToMeters(72), -90), + L3(Units.inchesToMeters(47.625), -35), + L2(Units.inchesToMeters(31.875), -35), + L1(Units.inchesToMeters(18), 0); + + ReefHeight(double height, double pitch) + { + this.height = height; + this.pitch = pitch; // in degrees + } + + public final double height; + public final double pitch; + } + + public static Pose2d getNearestReefFace(Pose2d currentPose) + { + return currentPose.nearest(List.of(FieldConstants.Reef.centerFaces)); + } + + public enum ReefSide { + LEFT, + RIGHT + } + + public static Pose2d getNearestReefBranch(Pose2d currentPose, ReefSide side) + { + return FieldConstants.Reef.branchPositions + .get(List.of(FieldConstants.Reef.centerFaces).indexOf(getNearestReefFace(currentPose)) + * 2 + (side == ReefSide.LEFT ? 1 : 0)) + .get(FieldConstants.ReefHeight.L1).toPose2d(); + } + + public static Pose2d getNearestCoralStation(Pose2d currentPose) + { + if (currentPose.getTranslation().getX() > FieldConstants.fieldLength / 2) { + if (currentPose.getTranslation().getY() > FieldConstants.fieldWidth / 2) { + return FieldConstants.CoralStation.rightCenterFace + .rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg); + } else { + return FieldConstants.CoralStation.leftCenterFace + .rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg); + } + } else { + if (currentPose.getTranslation().getY() > FieldConstants.fieldWidth / 2) { + return FieldConstants.CoralStation.leftCenterFace; + } else { + return FieldConstants.CoralStation.rightCenterFace; + } + } + } +} diff --git a/src/main/java/frc/robot/HardwareConstants.java b/src/main/java/frc/robot/HardwareConstants.java index 558e7f0..648d9bc 100644 --- a/src/main/java/frc/robot/HardwareConstants.java +++ b/src/main/java/frc/robot/HardwareConstants.java @@ -40,4 +40,7 @@ public class DIO { // Add digitial I/O ports used here public static final int LIGHT_SENSOR_CHANNEL = 0; } + + // Use LoggedTunableNumbers + public static final boolean tuningMode = true; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2b1cc66..744bc74 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -31,6 +31,8 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.HardwareConstants.CAN; @@ -74,17 +76,6 @@ public Robot() { Logger.recordMetadata("ProjectName", "ReefCode"); // Set a metadata value - if (isReal()) { - Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") - Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables - new PowerDistribution(CAN.PDH_ID, ModuleType.kRev); // Enables power distribution logging - } else { - setUseTiming(false); // Run as fast as possible - String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) - Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log - Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log - } - Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may // be added. @@ -183,6 +174,7 @@ public void disabledExit() { @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + System.out.println(m_autonomousCommand); if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 528fb73..87f74f1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,11 @@ import com.pathplanner.lib.util.FileVersionException; import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; @@ -39,7 +43,6 @@ import frc.robot.subsystems.drive.DriveCommands; import frc.robot.subsystems.drive.Telemetry; import frc.robot.subsystems.drive.TunerConstants; -import frc.robot.subsystems.elevator.CmdElevatorCalibrate; import frc.robot.subsystems.drive.io.GyroIO; import frc.robot.subsystems.drive.io.GyroIOPigeon2; import frc.robot.subsystems.drive.io.ModuleIO; @@ -48,9 +51,6 @@ import frc.robot.subsystems.elevator.CmdElevatorCalibrate; import frc.robot.subsystems.elevator.Elevator; import frc.robot.subsystems.elevator.ElevatorCommands; -import frc.robot.subsystems.elevator.RealElevatorIO; -import frc.robot.subsystems.leds.LEDs; -import frc.robot.subsystems.leds.LEDsCommands; import frc.robot.subsystems.elevator.Elevator.ElevatorPosition; import frc.robot.subsystems.elevator.io.RealElevatorIO; import frc.robot.subsystems.leds.LEDs; @@ -89,7 +89,7 @@ public class RobotContainer { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private double MaxAngularRate = RotationsPerSecond.of(0.5).in(RadiansPerSecond); // 1/2 of a rotation per second max // angular velocity - + /* Setting up bindings for necessary control of the swerve drive platform */ // private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() // .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband @@ -97,9 +97,6 @@ public class RobotContainer { private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final CommandXboxController joystick = new CommandXboxController(0); - private final CommandXboxController climberstick = new CommandXboxController(1); - private final GenericHID buttonbox1 = new GenericHID(1); private final GenericHID buttonbox2 = new GenericHID(2); @@ -160,11 +157,22 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); - // vision = - // new Vision( - // drive::addVisionMeasurement, - // new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose), - // new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose)); + /* We should be using this VisionIOPhotonVisionSim here but it's running too slow and + causing the a loop overrun. + vision = + new Vision( + drive::addVisionMeasurement, + new VisionIOPhotonVisionSim(camera7Name, robotToCamera7, drive::getPose), + new VisionIOPhotonVisionSim(camera8Name, robotToCamera8, drive::getPose) + ); */ + vision = + new Vision( + drive::addVisionMeasurement, + new VisionIO() {}, + new VisionIO() {}, + new VisionIO() {}, + new VisionIO() {}); + break; default: @@ -177,10 +185,15 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); - // vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); + vision = + new Vision( + drive::addVisionMeasurement, + new VisionIO() {}, + new VisionIO() {}, + new VisionIO() {}, + new VisionIO() {}); break; } - // All AutoAligns for reef will align to Left position //TODO: Add AutoAligns to all the commands. @@ -238,25 +251,14 @@ private void configureBindings() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. + drive.setDefaultCommand( DriveCommands.joystickDrive( drive, () -> -joystick.getLeftY(), () -> -joystick.getLeftX(), - () -> -joystick.getRightX())); - - climber.setDefaultCommand( - ClimberCommands.runClimber( - () -> -climberstick.getLeftY())); + () -> -joystick.getRightX())); - - // drivetrain.setDefaultCommand( - // // Drivetrain will execute this command periodically - // drivetrain.applyRequest(() -> drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with - // // negative Y (forward) - // .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left) - // .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) - // )); //climberstick.start().and(climberstick.y()).onTrue(getAutonomousCommand()); @@ -340,44 +342,11 @@ private void configureBindings() { } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } - - public void calibrateAndStartPIDs() { - // PID commands: we only want one of them so start/stop works correctly - Command elevatorPIDCommand = elevatorCommands.runElevatorPID(); - Command armPIDCommand = armCommands.runArmPID(); - // Start elevator pid - if (elevatorSubsystem.isCalibrated()) { - elevatorCommands.runElevatorPID(); - if (!CommandScheduler.getInstance().isScheduled(elevatorPIDCommand)) { - CommandScheduler.getInstance().schedule(elevatorPIDCommand); - } - } else { - Command calibCommand = new CmdElevatorCalibrate(elevatorSubsystem).andThen(elevatorPIDCommand); - CommandScheduler.getInstance().schedule(calibCommand); - } - - // Start arm pid - if (!CommandScheduler.getInstance().isScheduled(armPIDCommand)) { - CommandScheduler.getInstance().schedule(armPIDCommand); - } - - // Set initial positions - CommandScheduler.getInstance().schedule(elevatorCommands.setElevatorSetpoint(ElevatorPosition.Stow)); - CommandScheduler.getInstance().schedule(armCommands.setArmPosition(ArmPosition.Stow)); - } - - public void startIdleAnimations() { - Command disabled1 = LEDCommands.disabledAnimation1(); - if (!CommandScheduler.getInstance().isScheduled(disabled1)) - CommandScheduler.getInstance().schedule(disabled1); + return autoChooser.get(); } - public void startEnabledLEDs() { - Command initialLEDs = LEDCommands.pickingUpCoral(); - if (!CommandScheduler.getInstance().isScheduled(initialLEDs)) - CommandScheduler.getInstance().schedule(initialLEDs); + public Drive getDrive() { + return drive; } public void calibrateAndStartPIDs() { // PID commands: we only want one of them so start/stop works correctly diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 6c0eb46..be14e5d 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -184,17 +184,6 @@ public boolean isFinished() { }; } - public Command moveGamepieceToLightSensor() { - return new StartEndCommand( - () -> { - _arm.setIntakeSpeed(-0.3); - }, - () -> { - _arm.setIntakeSpeed(0); - }, - _arm).until(() -> _arm.lightSensorSeesGamepiece()); - } - public Command runArmPID() { return Commands.run(() -> { _arm.runArmPID(); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java similarity index 85% rename from src/main/java/frc/robot/subsystems/arm/ArmIO.java rename to src/main/java/frc/robot/subsystems/arm/io/ArmIO.java index 32a593e..f21d2d4 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.arm; +package frc.robot.subsystems.arm.io; import org.littletonrobotics.junction.AutoLog; @@ -11,10 +11,6 @@ public static class ArmIOInputs { public double _armMotorVoltage = 0.0; public double _armMotorSpeed = 0.0; - public double _feederMotorCurrent = 0.0; - public double _feederMotorVoltage = 0.0; - public double _feederMotorSpeed = 0.0; - public double _armEncoderPositionDegrees = 0.0; public double _armEncoderVelocity = 0.0; diff --git a/src/main/java/frc/robot/subsystems/arm/RealArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java similarity index 97% rename from src/main/java/frc/robot/subsystems/arm/RealArmIO.java rename to src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java index 5dad172..ea77a7d 100644 --- a/src/main/java/frc/robot/subsystems/arm/RealArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.arm; +package frc.robot.subsystems.arm.io; import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkBase.PersistMode; @@ -35,7 +35,6 @@ public class RealArmIO implements ArmIO { private LaserCan _algaeLaserCan; private SparkFlex _intakeMotor; private SparkAbsoluteEncoder _armEncoder; - private SparkMax _feederMotor; public RealArmIO() { _armMotor = new SparkFlex(CAN.ARM_MTR_ID, MotorType.kBrushless); @@ -107,10 +106,6 @@ public void setIntakeMotorSpeed(double speed) { _intakeMotor.set(speed); } - public void setFeederMotorSpeed(double speed){ - _feederMotor.set(speed); - - } public void resetIntakeEncoders() { _intakeMotor.getEncoder().setPosition(0); } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java deleted file mode 100644 index 90bcb9f..0000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.subsystems.climber; - -import org.littletonrobotics.junction.AutoLog; - -import frc.robot.subsystems.arm.ArmIO.ArmIOInputs; - -public interface ClimberIO { - @AutoLog - public static class ClimberIOInputs { - public double _climberMotorSpeed = 0.0; - } - - public default void updateInputs(ClimberIOInputs inputs) {} - - public default void setClimberMotorSpeed(double speed) {} -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/RealClimberIO.java b/src/main/java/frc/robot/subsystems/climber/RealClimberIO.java deleted file mode 100644 index 16b4b90..0000000 --- a/src/main/java/frc/robot/subsystems/climber/RealClimberIO.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot.subsystems.climber; - -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import frc.robot.HardwareConstants.CAN; - -public class RealClimberIO implements ClimberIO{ - private SparkFlex _climberMotor; - - public RealClimberIO() { - _climberMotor = new SparkFlex(CAN.CLIMBER_MTR_ID, MotorType.kBrushless); - - SparkFlexConfig climberConfig = new SparkFlexConfig(); - climberConfig.idleMode(IdleMode.kBrake); - _climberMotor.configure(climberConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - } - - public void updateInputs(ClimberIOInputs inputs) { - inputs._climberMotorSpeed = _climberMotor.get(); - } - - public void setClimberMotorSpeed(double speed) { - _climberMotor.set(speed); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java.old similarity index 100% rename from src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java rename to src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java.old diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index b2ff480..130b710 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -122,6 +122,53 @@ public Drive( // Start odometry thread PhoenixOdometryThread.getInstance().start(); + + RobotConfig config; + try{ + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + e.printStackTrace(); + config = new RobotConfig( + ROBOT_MASS_KG, + ROBOT_MOI, + new ModuleConfig( + TunerConstants.FrontLeft.WheelRadius, + TunerConstants.kSpeedAt12Volts.in(MetersPerSecond), + WHEEL_COF, + DCMotor.getKrakenX60Foc(1) + .withReduction(TunerConstants.FrontLeft.DriveMotorGearRatio), + TunerConstants.FrontLeft.SlipCurrent, + 1), + getModuleTranslations()); + } + + // Configure AutoBuilder last + AutoBuilder.configure( + this::getPose, // Robot pose supplier + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + + /* // Configure AutoBuilder for PathPlanner AutoBuilder.configure( this::getPose, @@ -129,16 +176,19 @@ public Drive( this::getChassisSpeeds, this::runVelocity, new PPHolonomicDriveController( - new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), - PP_CONFIG, + new PIDConstants(3, 0.0, 0.0), new PIDConstants(5.5, 0.0, 0.0)), + PP_CONFIG, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this); + this); */ + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback( (activePath) -> { Logger.recordOutput( "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); }); + PathPlannerLogging.setLogTargetPoseCallback( (targetPose) -> { Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); @@ -221,6 +271,7 @@ public void periodic() { * @param speeds Speeds in meters/sec */ public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java index 707488d..e672165 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.drive.Drive; @@ -35,6 +36,8 @@ import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import frc.robot.util.TuneableProfiledPID; +import org.littletonrobotics.junction.Logger; public class DriveCommands { private static final double DEADBAND = 0.1; @@ -47,6 +50,14 @@ public class DriveCommands { private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + public enum DriveMode { + dmJoystick, + dmAngle, + dmApproach +} + +private static DriveMode currentDriveMode = DriveMode.dmJoystick; + private DriveCommands() {} private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { @@ -89,9 +100,8 @@ public static Command joystickDrive( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega * drive.getMaxAngularSpeedRadPerSec()); - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; + boolean isFlipped = DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, @@ -156,6 +166,101 @@ public static Command joystickDriveAtAngle( .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } + /** + * Robot relative drive command using joystick for linear control towards the approach target, + * PID for aligning with the target laterally, and PID for angular control. Used for approaching + * a known target, usually from a short distance. The approachSupplier must supply a Pose2d with + * a rotation facing away from the target + */ + public static Command joystickApproach( + Drive drive, + DoubleSupplier ySupplier, + Supplier approachSupplier) + { + + // Create PID controller + TuneableProfiledPID angleController = + new TuneableProfiledPID( + "angleController", + ANGLE_KP, + 0.0, + ANGLE_KD, + ANGLE_MAX_VELOCITY, + ANGLE_MAX_ACCELERATION); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + TuneableProfiledPID alignController = + new TuneableProfiledPID( + "alignController", + 0.3, + 0.0, + 0, + 20, + 8); + alignController.setGoal(0); + + // Construct command + return Commands.run( + () -> { + + //System.out.println("JOYSTICK APPROACH!!!"); + currentDriveMode = DriveMode.dmApproach; + // Name constants + Translation2d currentTranslation = drive.getPose().getTranslation(); + Translation2d approachTranslation = approachSupplier.get().getTranslation(); + double distanceToApproach = currentTranslation.getDistance(approachTranslation); + + Rotation2d alignmentDirection = approachSupplier.get().getRotation(); + + // Find lateral distance from goal + Translation2d goalTranslation = new Translation2d( + alignmentDirection.getCos() * distanceToApproach + approachTranslation.getX(), + alignmentDirection.getSin() * distanceToApproach + approachTranslation.getY()); + + Translation2d robotToGoal = currentTranslation.minus(goalTranslation); + double distanceToGoal = + Math.hypot(robotToGoal.getX(), robotToGoal.getY()); + + // Calculate lateral linear velocity + Translation2d offsetVector = + new Translation2d(alignController.calculate(distanceToGoal), 0) + .rotateBy(robotToGoal.getAngle()); + + Logger.recordOutput("AlignDebug/Current", distanceToGoal); + + // Calculate total linear velocity + Translation2d linearVelocity = + getLinearVelocityFromJoysticks(0, + ySupplier.getAsDouble()).rotateBy( + approachSupplier.get().getRotation()).rotateBy(Rotation2d.kCCW_90deg) + .plus(offsetVector); + + SmartDashboard.putData(alignController); // TODO: Calibrate PID + Logger.recordOutput("AlignDebug/approachTarget", approachTranslation); + + // Calculate angular speed + double omega = + angleController.calculate( + drive.getRotation().getRadians(), approachSupplier.get().getRotation() + .rotateBy(Rotation2d.k180deg).getRadians()); + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = + new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega); + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + drive.getRotation())); + }, + drive) + + // Reset PID controller when command starts + .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); + } + /** * Measures the velocity feedforward constants for the drive motors. * diff --git a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java index 51ff3ba..b015024 100644 --- a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java @@ -22,7 +22,7 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) + .withKP(107).withKI(0).withKD(0.7) .withKS(0.1).withKV(2.66).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control @@ -72,8 +72,8 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21*0.7); - + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21); + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.5714285714285716; @@ -193,11 +193,12 @@ public class TunerConstants { * Creates a CommandSwerveDrivetrain instance. * This should only be called once in your robot program,. */ + /* public static CommandSwerveDrivetrain createDrivetrain() { return new CommandSwerveDrivetrain( DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight ); - } + } */ /** diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java index d460d81..0a7c24c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.subsystems.elevator.Elevator.ElevatorPosition; -import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; public final class ElevatorCommands { private Elevator _elevator; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/io/ElevatorIO.java similarity index 94% rename from src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java rename to src/main/java/frc/robot/subsystems/elevator/io/ElevatorIO.java index a681447..e354123 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/io/ElevatorIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.elevator; +package frc.robot.subsystems.elevator.io; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/elevator/RealElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java similarity index 96% rename from src/main/java/frc/robot/subsystems/elevator/RealElevatorIO.java rename to src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java index 939a684..9d6172b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/RealElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.elevator; +package frc.robot.subsystems.elevator.io; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -41,7 +41,7 @@ public void updateInputs(ElevatorIOInputs inputs) { } public void setElevatorSpeed(double speed) { - _primaryMotor.set(speed); + _primaryMotor.set(speed); } public void setElevatorVoltage(Voltage voltage) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..b1e70f6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,52 @@ +// Code modified from AdvantageKit Vision Template (v4.0.1): +// https://github.com/Mechanical-Advantage/AdvantageKit/releases +// +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface VisionIO { + @AutoLog + public static class VisionIOInputs { + public boolean connected = false; + public TargetObservation latestTargetObservation = + new TargetObservation(new Rotation2d(), new Rotation2d()); + public PoseObservation[] poseObservations = new PoseObservation[0]; + public int[] tagIds = new int[0]; + } + + /** Represents the angle to a simple target, not used for pose estimation. */ + public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} + + /** Represents a robot pose sample used for pose estimation. */ + public static record PoseObservation( + double timestamp, + Pose3d pose, + double ambiguity, + int tagCount, + double averageTagDistance, + PoseObservationType type) {} + + public static enum PoseObservationType { + MEGATAG_1, + MEGATAG_2, + PHOTONVISION + } + + public default void updateInputs(VisionIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java new file mode 100644 index 0000000..a09d102 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -0,0 +1,159 @@ +// Code modified from AdvantageKit Vision Template (v4.0.1): +// https://github.com/Mechanical-Advantage/AdvantageKit/releases +// +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoubleArraySubscriber; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.RobotController; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import java.util.function.Supplier; + +/** IO implementation for real Limelight hardware. */ +public class VisionIOLimelight implements VisionIO { + private final Supplier rotationSupplier; + private final DoubleArrayPublisher orientationPublisher; + + private final DoubleSubscriber latencySubscriber; + private final DoubleSubscriber txSubscriber; + private final DoubleSubscriber tySubscriber; + private final DoubleArraySubscriber megatag1Subscriber; + private final DoubleArraySubscriber megatag2Subscriber; + + /** + * Creates a new VisionIOLimelight. + * + * @param name The configured name of the Limelight. + * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. + */ + public VisionIOLimelight(String name, Supplier rotationSupplier) { + var table = NetworkTableInstance.getDefault().getTable(name); + this.rotationSupplier = rotationSupplier; + orientationPublisher = table.getDoubleArrayTopic("robot_orientation_set").publish(); + latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0); + txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); + tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); + megatag1Subscriber = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); + megatag2Subscriber = + table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + // Update connection status based on whether an update has been seen in the last 250ms + inputs.connected = + ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250; + + // Update target observation + inputs.latestTargetObservation = + new TargetObservation( + Rotation2d.fromDegrees(txSubscriber.get()), Rotation2d.fromDegrees(tySubscriber.get())); + + // Update orientation for MegaTag 2 + orientationPublisher.accept( + new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); + NetworkTableInstance.getDefault() + .flush(); // Increases network traffic but recommended by Limelight + + // Read new pose observations from NetworkTables + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + for (var rawSample : megatag1Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 11; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add( + new PoseObservation( + // Timestamp, based on server timestamp of publish and latency + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, + + // 3D pose estimate + parsePose(rawSample.value), + + // Ambiguity, using only the first tag because ambiguity isn't applicable for multitag + rawSample.value.length >= 18 ? rawSample.value[17] : 0.0, + + // Tag count + (int) rawSample.value[7], + + // Average tag distance + rawSample.value[9], + + // Observation type + PoseObservationType.MEGATAG_1)); + } + for (var rawSample : megatag2Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 11; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add( + new PoseObservation( + // Timestamp, based on server timestamp of publish and latency + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, + + // 3D pose estimate + parsePose(rawSample.value), + + // Ambiguity, zeroed because the pose is already disambiguated + 0.0, + + // Tag count + (int) rawSample.value[7], + + // Average tag distance + rawSample.value[9], + + // Observation type + PoseObservationType.MEGATAG_2)); + } + + // Save pose observations to inputs object + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + // Save tag IDs to inputs objects + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } + + /** Parses the 3D pose from a Limelight botpose array. */ + private static Pose3d parsePose(double[] rawLLArray) { + return new Pose3d( + rawLLArray[0], + rawLLArray[1], + rawLLArray[2], + new Rotation3d( + Units.degreesToRadians(rawLLArray[3]), + Units.degreesToRadians(rawLLArray[4]), + Units.degreesToRadians(rawLLArray[5]))); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java new file mode 100644 index 0000000..94a8d53 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -0,0 +1,149 @@ +// Code modified from AdvantageKit Vision Template (v4.0.1): +// https://github.com/Mechanical-Advantage/AdvantageKit/releases +// +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.*; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; + +/** IO implementation for real PhotonVision hardware. */ +public class VisionIOPhotonVision implements VisionIO { + protected final PhotonCamera camera; + protected final Transform3d robotToCamera; + + /** + * Creates a new VisionIOPhotonVision. + * + * @param name The configured name of the camera. + * @param rotationSupplier The 3D position of the camera relative to the robot. + */ + public VisionIOPhotonVision(String name, Transform3d robotToCamera) { + camera = new PhotonCamera(name); + this.robotToCamera = robotToCamera; + } + + // NEW COMMENT + // SECTION 2 + @Override + public void updateInputs(VisionIOInputs inputs) { + + inputs.connected = camera.isConnected(); + + // Read new camera observations + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + + for (var result : camera.getAllUnreadResults()) { + // Update latest target observation + if (result.hasTargets()) { + + inputs.latestTargetObservation = + new TargetObservation( + Rotation2d.fromDegrees(result.getBestTarget().getYaw()), + Rotation2d.fromDegrees(result.getBestTarget().getPitch())); + } else { + inputs.latestTargetObservation = new TargetObservation(new Rotation2d(), new Rotation2d()); + } + + // Add pose observation + if (result.multitagResult.isPresent()) { // Multitag result + var multitagResult = result.multitagResult.get(); + + // Calculate robot pose + Transform3d fieldToCamera = multitagResult.estimatedPose.best; + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + + // Calculate average tag distance + double totalTagDistance = 0.0; + for (var target : result.targets) { + totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm(); + } + + // Add tag IDs + tagIds.addAll(multitagResult.fiducialIDsUsed); + + // Add observation + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + multitagResult.estimatedPose.ambiguity, // Ambiguity + multitagResult.fiducialIDsUsed.size(), // Tag count + totalTagDistance / result.targets.size(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + + } else if (!result.targets.isEmpty()) { // Single tag result + var target = result.targets.get(0); + + // Calculate robot pose + var tagPose = aprilTagLayout.getTagPose(target.fiducialId); + //System.out.println("****** " + camera.getName() + " *********************************"); + //System.out.println("tagPose:" + tagPose); + if (tagPose.isPresent()) { + Transform3d fieldToTarget = + new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); + //System.out.println("fieldToTarget:" + fieldToTarget); + Transform3d cameraToTarget = target.bestCameraToTarget; + //System.out.println("cameraToTarget:" + cameraToTarget); + Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); + //System.out.println("fieldToCamera:" + fieldToCamera); + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + //System.out.println("fieldToRobot:" + fieldToRobot); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + //System.out.println("robotPose:" + robotPose); + + // Add tag ID + tagIds.add((short) target.fiducialId); + + // Add observation + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + target.poseAmbiguity, // Ambiguity + 1, // Tag count + cameraToTarget.getTranslation().getNorm(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + } + } + } + + // Save pose observations to inputs object + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + // Save tag IDs to inputs objects + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java new file mode 100644 index 0000000..a0fd43c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -0,0 +1,63 @@ +// Code modified from AdvantageKit Vision Template (v4.0.1): +// https://github.com/Mechanical-Advantage/AdvantageKit/releases +// +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.function.Supplier; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +/** IO implementation for physics sim using PhotonVision simulator. */ +public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { + private static VisionSystemSim visionSim; + + private final Supplier poseSupplier; + private final PhotonCameraSim cameraSim; + + /** + * Creates a new VisionIOPhotonVisionSim. + * + * @param name The name of the camera. + * @param poseSupplier Supplier for the robot pose to use in simulation. + */ + public VisionIOPhotonVisionSim( + String name, Transform3d robotToCamera, Supplier poseSupplier) { + super(name, robotToCamera); + this.poseSupplier = poseSupplier; + + // Initialize vision sim + if (visionSim == null) { + visionSim = new VisionSystemSim("main"); + visionSim.addAprilTags(aprilTagLayout); + } + + // Add sim camera + var cameraProperties = new SimCameraProperties(); + cameraSim = new PhotonCameraSim(camera, cameraProperties); + visionSim.addCamera(cameraSim, robotToCamera); + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + visionSim.update(poseSupplier.get()); + super.updateInputs(inputs); + } +} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java new file mode 100644 index 0000000..5574778 --- /dev/null +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -0,0 +1,133 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +import frc.robot.HardwareConstants; +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +@SuppressWarnings("UnusedVariable") +public class LoggedTunableNumber implements DoubleSupplier { + private static final String tableKey = "TunableNumbers"; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedNetworkNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) + { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) + { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) + { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (HardwareConstants.tuningMode) { + dashboardNumber = new LoggedNetworkNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() + { + if (!hasDefault) { + return 0.0; + } else { + return HardwareConstants.tuningMode ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) + { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * + * objects. Recommended approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable + * numbers in order inputted in method + * @param tunableNumbers All tunable numbers to check + */ + public static void ifChanged( + int id, Consumer action, LoggedTunableNumber... tunableNumbers) + { + if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept( + Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); + } + } + + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) + { + ifChanged(id, values -> action.run(), tunableNumbers); + } + + @Override + public double getAsDouble() + { + return get(); + } +} diff --git a/src/main/java/frc/robot/util/TuneableProfiledPID.java b/src/main/java/frc/robot/util/TuneableProfiledPID.java new file mode 100644 index 0000000..1972576 --- /dev/null +++ b/src/main/java/frc/robot/util/TuneableProfiledPID.java @@ -0,0 +1,51 @@ +// 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 frc.robot.util; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; + +/** Add your docs here. */ +public class TuneableProfiledPID extends ProfiledPIDController { + + // Instance name for tagging log values + String m_name; + + // Tunable numbers + private LoggedTunableNumber m_kP, m_kI, m_kD, m_maxV, m_maxA; + + public TuneableProfiledPID(String name, double kP, double kI, + double kD, double maxV, double maxA) + { + super(kP, kI, kD, new TrapezoidProfile.Constraints(maxV, maxA)); + + m_name = name; + + // Tunable numbers for PID and motion gain constants + m_kP = new LoggedTunableNumber(m_name + "/kP", kP); + m_kI = new LoggedTunableNumber(m_name + "/kI", kI); + m_kD = new LoggedTunableNumber(m_name + "/kD", kD); + + m_maxV = new LoggedTunableNumber(m_name + "/maxV", maxV); + m_maxA = new LoggedTunableNumber(m_name + "/maxA", maxA); + + } + + public void updatePID() + { + // If changed, update controller constants from Tuneable Numbers + if (m_kP.hasChanged(hashCode()) + || m_kI.hasChanged(hashCode()) + || m_kD.hasChanged(hashCode())) { + this.setPID(m_kP.get(), m_kI.get(), m_kD.get()); + } + + if (m_maxV.hasChanged(hashCode()) + || m_maxA.hasChanged(hashCode())) { + this.setConstraints(new TrapezoidProfile.Constraints(m_maxV.get(), m_maxA.get())); + } + } + +} diff --git a/vendordeps/PathplannerLib-2025.2.1.json b/vendordeps/PathplannerLib-2025.2.5.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.1.json rename to vendordeps/PathplannerLib-2025.2.5.json index 71e25f3..b2a7265 100644 --- a/vendordeps/PathplannerLib-2025.2.1.json +++ b/vendordeps/PathplannerLib-2025.2.5.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.1.json", + "fileName": "PathplannerLib-2025.2.5.json", "name": "PathplannerLib", - "version": "2025.2.1", + "version": "2025.2.5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.1" + "version": "2025.2.5" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.1", + "version": "2025.2.5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.2.1.json b/vendordeps/Phoenix6-25.3.1.json similarity index 86% rename from vendordeps/Phoenix6-25.2.1.json rename to vendordeps/Phoenix6-25.3.1.json index 1397da1..a216d97 100644 --- a/vendordeps/Phoenix6-25.2.1.json +++ b/vendordeps/Phoenix6-25.3.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.2.1.json", + "fileName": "Phoenix6-25.3.1.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", + "version": "25.3.1", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.1" + "version": "25.3.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -414,6 +428,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file diff --git a/vendordeps/REVLib-2025.0.1.json b/vendordeps/REVLib.json similarity index 90% rename from vendordeps/REVLib-2025.0.1.json rename to vendordeps/REVLib.json index c998054..ac62be8 100644 --- a/vendordeps/REVLib-2025.0.1.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.1.json", + "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.1", + "version": "2025.0.3", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.1" + "version": "2025.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.1", + "version": "2025.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..1219919 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.2.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.2.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.2.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.2.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.2.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.2.1" + } + ] +} \ No newline at end of file From db403f966db27b3d01b9ce5919ce3cfbde727b3c Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Tue, 1 Apr 2025 17:00:41 -0400 Subject: [PATCH 087/106] calibration should run the same, but you can trigger it via sad face --- src/main/java/frc/robot/Robot.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 30 ++-------- .../java/frc/robot/subsystems/arm/Arm.java | 3 + .../frc/robot/subsystems/arm/ArmCommands.java | 6 -- .../elevator/CmdElevatorCalibrate.java | 46 --------------- .../robot/subsystems/elevator/Elevator.java | 3 + .../subsystems/elevator/ElevatorCommands.java | 58 ++++++++++++++----- .../MultiSubsystemCommands.java | 12 ++-- 8 files changed, 64 insertions(+), 98 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/elevator/CmdElevatorCalibrate.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 744bc74..ff62b0c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -180,7 +180,7 @@ public void autonomousInit() { m_autonomousCommand.schedule(); } - m_robotContainer.calibrateAndStartPIDs(); + m_robotContainer.calibrate(); m_robotContainer.startEnabledLEDs(); } @@ -197,7 +197,7 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - m_robotContainer.calibrateAndStartPIDs(); + m_robotContainer.calibrate(); m_robotContainer.startEnabledLEDs(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 87f74f1..a6f178d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -48,7 +48,6 @@ import frc.robot.subsystems.drive.io.ModuleIO; import frc.robot.subsystems.drive.io.ModuleIOSim; import frc.robot.subsystems.drive.io.ModuleIOTalonFX; -import frc.robot.subsystems.elevator.CmdElevatorCalibrate; import frc.robot.subsystems.elevator.Elevator; import frc.robot.subsystems.elevator.ElevatorCommands; import frc.robot.subsystems.elevator.Elevator.ElevatorPosition; @@ -116,6 +115,8 @@ public class RobotContainer { private final JoystickButton incrementElevatorButton = new JoystickButton(buttonbox2, 4); private final JoystickButton decrementElevatorButton = new JoystickButton(buttonbox2, 7); + private final JoystickButton recalibrateButton = new JoystickButton(buttonbox2, 7); + private final JoystickButton dynamic = new JoystickButton(buttonbox2, 8); private final JoystickButton qstatic = new JoystickButton(buttonbox2, 9); @@ -318,6 +319,8 @@ private void configureBindings() { incrementElevatorButton.onTrue(elevatorCommands.incrementElevatorPosition()); decrementElevatorButton.onTrue(elevatorCommands.decrementElevatorPosition()); + + recalibrateButton.onTrue(multiSubsystemCommands.calibrate()); /* // Driver Left Bumper and Algae mode: Approach Nearest Reef Face joystick.rightBumper() @@ -348,29 +351,8 @@ public Command getAutonomousCommand() { public Drive getDrive() { return drive; } - public void calibrateAndStartPIDs() { - // PID commands: we only want one of them so start/stop works correctly - Command elevatorPIDCommand = elevatorCommands.runElevatorPID(); - Command armPIDCommand = armCommands.runArmPID(); - // Start elevator pid - if (elevatorSubsystem.isCalibrated()) { - elevatorCommands.runElevatorPID(); - if (!CommandScheduler.getInstance().isScheduled(elevatorPIDCommand)) { - CommandScheduler.getInstance().schedule(elevatorPIDCommand); - } - } else { - Command calibCommand = new CmdElevatorCalibrate(elevatorSubsystem).andThen(elevatorPIDCommand); - CommandScheduler.getInstance().schedule(calibCommand); - } - - // Start arm pid - if (!CommandScheduler.getInstance().isScheduled(armPIDCommand)) { - CommandScheduler.getInstance().schedule(armPIDCommand); - } - - // Reset PIDs - CommandScheduler.getInstance().schedule(elevatorCommands.resetElevatorPID()); - CommandScheduler.getInstance().schedule(armCommands.resetArmPID()); + public void calibrate() { + CommandScheduler.getInstance().schedule(multiSubsystemCommands.calibrate()); } public void startIdleAnimations() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 50c2a49..19e7413 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -216,5 +216,8 @@ public void periodic() { Logger.recordOutput("Arm/currentPosEnum", _currentPos); Logger.recordOutput("Arm/desiredPosEnum", _desiredPos); Logger.recordOutput("Arm/intakeSpikeCounter", _intakeSpikeCounter); + + // Run pid + runArmPID(); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index be14e5d..86713a1 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -184,12 +184,6 @@ public boolean isFinished() { }; } - public Command runArmPID() { - return Commands.run(() -> { - _arm.runArmPID(); - }); - } - public Command intakeForNumberOfRotations() { return new StartEndCommand(() -> { _arm.resetIntakeEncoders(); diff --git a/src/main/java/frc/robot/subsystems/elevator/CmdElevatorCalibrate.java b/src/main/java/frc/robot/subsystems/elevator/CmdElevatorCalibrate.java deleted file mode 100644 index d9163f6..0000000 --- a/src/main/java/frc/robot/subsystems/elevator/CmdElevatorCalibrate.java +++ /dev/null @@ -1,46 +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 frc.robot.subsystems.elevator; - -import edu.wpi.first.wpilibj2.command.Command; - -public class CmdElevatorCalibrate extends Command { - - private Elevator _elevator; - private int _spikeCounter; - - public CmdElevatorCalibrate(Elevator elevator) { - _elevator = elevator; - } - - - @Override - public void initialize() { - _elevator.runMotorsDown(); - _spikeCounter = 0; - } - - - @Override - public void execute() { - if (_elevator.isAboveCurrentLimit()) { - _spikeCounter++; - } - } - - - @Override - public void end(boolean interrupted) { - _elevator.resetEncoders(); - _elevator.stopMotors(); - _elevator.setIsCalibrated(true); - } - - - @Override - public boolean isFinished() { - return _spikeCounter >= 3; - } -} diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index e7526a4..2378be4 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -235,6 +235,9 @@ public void periodic() { Logger.recordOutput("Elevator/currentPosEnum", _currentPos); Logger.recordOutput("Elevator/desiredPosEnum", _desiredPos); Logger.recordOutput("Elevator/currentGamepieceMode", _currentMode); + + // Run pid + runMotorsWithPID(); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java index 0a7c24c..1825d55 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java @@ -13,33 +13,59 @@ public ElevatorCommands(Elevator elevator) { _elevator = elevator; } - public Command runElevatorPID() { - return Commands.run(() -> { - _elevator.runMotorsWithPID(); - }); + public Command calibrateElevator() { + return new Command() { + int spikeCounter = 0; + + @Override + public void initialize() { + spikeCounter = 0; + _elevator.runMotorsDown(); + _elevator.setIsCalibrated(false); + } + + @Override + public void execute() { + if (_elevator.isAboveCurrentLimit()) { + spikeCounter++; + } + } + + @Override + public void end(boolean interrupted) { + _elevator.resetEncoders(); + _elevator.stopMotors(); + _elevator.setIsCalibrated(true); + } + + @Override + public boolean isFinished() { + return spikeCounter >= 3; + } + }; } public Command decrementElevatorPosition() { return new InstantCommand( - () -> { - _elevator.decrementElevatorPosition(); - - } , _elevator); + () -> { + _elevator.decrementElevatorPosition(); + + }, _elevator); } public Command incrementElevatorPosition() { return new InstantCommand( - () -> { - _elevator.incrementElevatorPosition(); - - }, _elevator); - } + () -> { + _elevator.incrementElevatorPosition(); + + }, _elevator); + } public Command setElevatorSetpoint(ElevatorPosition setpoint) { return new InstantCommand( - () -> { - _elevator.setSetpoint(setpoint); - }, _elevator); + () -> { + _elevator.setSetpoint(setpoint); + }, _elevator); } public Command waitUntilAtSetpoint() { diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 6806edc..3212eb2 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -51,10 +51,8 @@ public enum GamepieceMode { private Elevator _elevator; private Arm _arm; - private Climber _climber; private ElevatorCommands _elevatorCommands; private ArmCommands _armCommands; - private ClimberCommands _climberCommands; public MultiSubsystemCommands(Elevator elevator, Arm arm, ElevatorCommands elevatorCommands, ArmCommands armCommands) { @@ -64,6 +62,12 @@ public MultiSubsystemCommands(Elevator elevator, Arm arm, ElevatorCommands eleva _armCommands = armCommands; } + public Command calibrate() { + return _elevatorCommands.calibrateElevator() + .andThen(_elevatorCommands.setElevatorSetpoint(ElevatorPosition.Stow) + .alongWith(_armCommands.setArmPosition(ArmPosition.Stow))); + } + public Command moveToPosition(OverallPosition setpoint) { return _armCommands.moveArm(ArmPosition.Transient) .andThen(_elevatorCommands.moveElevator(setpoint.getElevatorPosition())) @@ -82,7 +86,7 @@ public Command setGamepieceMode(GamepieceMode mode) { public Command scoreGamepieceAtPosition(OverallPosition setpoint) { return moveToPosition(setpoint) .andThen(_armCommands.spit()); - } + } public Command loadCoral() { return moveToPosition(OverallPosition.Coral_Loading) @@ -97,5 +101,5 @@ public Command loadAlgae(OverallPosition position) { .alongWith(_armCommands.intake()) .andThen(_armCommands.setArmPosition(ArmPosition.Stow)); } - + } \ No newline at end of file From 325a8a396ddeb28f7352527c78050d920a50f7c4 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Tue, 1 Apr 2025 17:03:45 -0400 Subject: [PATCH 088/106] elevator pid shouldn't run when not calibrated --- src/main/java/frc/robot/subsystems/elevator/Elevator.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 2378be4..63a7955 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -183,7 +183,8 @@ public void resetEncoders() { // Runs motors with PID public void runMotorsWithPID() { - _io.setElevatorVoltage(Voltage.ofBaseUnits(_elevatorMotorPID.calculate(getCurrentPosInches()) + FEEDFORWARD_CONSTANT, Volt)); + if (_isCalibrated) + _io.setElevatorVoltage(Voltage.ofBaseUnits(_elevatorMotorPID.calculate(getCurrentPosInches()) + FEEDFORWARD_CONSTANT, Volt)); } public boolean isCalibrated() { From 04ea0e79a630aa7e6fd1d96d1889497235f474ce Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Tue, 1 Apr 2025 17:05:28 -0400 Subject: [PATCH 089/106] Tell the arm to go to stow when calibrating --- .../multisubsystemcommands/MultiSubsystemCommands.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 3212eb2..a024275 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -63,9 +63,9 @@ public MultiSubsystemCommands(Elevator elevator, Arm arm, ElevatorCommands eleva } public Command calibrate() { - return _elevatorCommands.calibrateElevator() - .andThen(_elevatorCommands.setElevatorSetpoint(ElevatorPosition.Stow) - .alongWith(_armCommands.setArmPosition(ArmPosition.Stow))); + return _armCommands.setArmPosition(ArmPosition.Stow) + .andThen(_elevatorCommands.calibrateElevator()) + .andThen(_elevatorCommands.setElevatorSetpoint(ElevatorPosition.Stow)); } public Command moveToPosition(OverallPosition setpoint) { From 98c9b9b22a92dc705a8ebe8bd84c96376cc72243 Mon Sep 17 00:00:00 2001 From: area5188 Date: Tue, 1 Apr 2025 17:51:20 -0400 Subject: [PATCH 090/106] added LED code --- src/main/java/frc/robot/RobotContainer.java | 8 +++---- .../java/frc/robot/subsystems/leds/LEDs.java | 24 +++++++++---------- .../robot/subsystems/leds/LEDsCommands.java | 7 ++++-- 3 files changed, 20 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 87f74f1..15e6df6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -244,7 +244,7 @@ private Command joystickApproach(Supplier approachPose) return DriveCommands.joystickApproach( drive, () -> -joystick.getLeftY() * speedMultiplier, - approachPose).alongWith(LEDsCommands.aligningWithReef()); + approachPose).alongWith(LEDsCommands.aligningWithReef(() -> drive.getCloseToReef())); } private void configureBindings() { @@ -272,7 +272,7 @@ private void configureBindings() { // drive.registerTelemetry(logger::telemeterize); intakeButton.onTrue(multiSubsystemCommands.loadCoral().raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving()).unless(() -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); - spitButton.onTrue(armCommands.spit()); + spitButton.onTrue(armCommands.spit().andThen(Commands.either(LEDCommands.pickingUpAlgae(), LEDCommands.pickingUpCoral(), () -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE))); StowButton.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.Stow)); L1Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L1)); @@ -280,8 +280,8 @@ private void configureBindings() { L3Button.onTrue(Commands.either(multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L3), multiSubsystemCommands.moveToPosition(OverallPosition.L3), () -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); L4Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L4)); - gamepieceModeToggle.whileTrue(multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE)); - gamepieceModeToggle.whileFalse(multiSubsystemCommands.setGamepieceMode(GamepieceMode.CORAL)); + gamepieceModeToggle.whileTrue(multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE).alongWith(LEDCommands.pickingUpAlgae())); + gamepieceModeToggle.whileFalse(multiSubsystemCommands.setGamepieceMode(GamepieceMode.CORAL).alongWith(LEDCommands.pickingUpCoral())); // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java index 336fa0b..baf4efc 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java @@ -10,13 +10,15 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.HardwareConstants; import frc.robot.HardwareConstants.CAN; -import frc.robot.subsystems.elevator.Elevator; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.drive.DriveCommands; +// import frc.robot.subsystems.elevator.Elevator; +// import frc.robot.subsystems.drive.Drive; +// import frc.robot.subsystems.drive.DriveCommands; import com.ctre.phoenix.led.RainbowAnimation; import com.ctre.phoenix.led.StrobeAnimation; +import java.util.function.BooleanSupplier; + import org.littletonrobotics.junction.Logger; import com.ctre.phoenix.led.Animation; @@ -28,8 +30,8 @@ public class LEDs extends SubsystemBase { boolean _alreadyRunning = false; LEDAnimation _currentAnimation = LEDAnimation.None; - Drive _drive; - Elevator _elevator; + // Drive _drive; + // Elevator _elevator; public enum LEDAnimation { None(null, null, 0), @@ -48,7 +50,7 @@ public enum LEDAnimation { Bounce(null, new LarsonAnimation(0, 255, 0), 3), - SolidTeal(new LEDColor(0, 225, 174), null, 0), + SolidTeal(new LEDColor(0, 225, 100), null, 0), SolidCoral(new LEDColor(255, 80, 15), null, 0), @@ -90,11 +92,7 @@ public void runAnimation(LEDAnimation animation) { _candle.clearAnimation(0); _candle.setLEDs(0, 0, 0); _candle.animate(animation.getAnimation()); - } else if (animation == LEDAnimation.SolidRed) { - _candle.clearAnimation(0); - LEDColor color = animation.getColor(); - _candle.setLEDs(color.getR(), color.getG(), color.getB(),0, 0, - (int) Math.round(_numLEDs/_elevator.getElevatorMaxHeight() * _elevator.getCurrentPosInches())); + } else if (animation.getAnimation() == null) { LEDColor color = animation.getColor(); _candle.clearAnimation(0); @@ -159,9 +157,9 @@ public void robotHasClimbed() { } } - public void aligningWithReefAnimation() { + public void aligningWithReefAnimation(BooleanSupplier closeToReef) { // If close enough to reef: - if (_drive.getCloseToReef()) { + if (closeToReef.getAsBoolean()) { runAnimation(LEDAnimation.SolidGreen); } diff --git a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java index c300e02..8a59c68 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.leds; +import java.util.function.BooleanSupplier; + import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.StartEndCommand; @@ -62,6 +64,7 @@ public Command pickingUpAlgae() { _leds); } + //We have a piece and hasPiece animation has already run public Command elevatorOrArmIsMoving() { return new StartEndCommand( @@ -102,10 +105,10 @@ public boolean runsWhenDisabled() { }; } - public static Command aligningWithReef() { + public static Command aligningWithReef(BooleanSupplier closeToReef) { return new StartEndCommand( () -> { - _leds.aligningWithReefAnimation(); + _leds.aligningWithReefAnimation(closeToReef); }, () -> { _leds.reset(); From 3828a38ee0fb8149c1e5cf93a54a7f4f5e3363a5 Mon Sep 17 00:00:00 2001 From: area5188 Date: Wed, 2 Apr 2025 00:48:58 -0400 Subject: [PATCH 091/106] finished swapping to secondary encoder --- .../frc/robot/subsystems/elevator/io/RealElevatorIO.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java index 9d6172b..4f497d9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java @@ -33,11 +33,11 @@ public RealElevatorIO() { } public void updateInputs(ElevatorIOInputs inputs) { - inputs._elevatorMotorVoltage = _primaryMotor.getAppliedOutput() * _primaryMotor.getBusVoltage(); - inputs._elevatorMotorCurrent = _primaryMotor.getOutputCurrent(); + inputs._elevatorMotorVoltage = _secondaryMotor.getAppliedOutput() * _secondaryMotor.getBusVoltage(); + inputs._elevatorMotorCurrent = _secondaryMotor.getOutputCurrent(); inputs._elevatorPosition = _secondaryMotor.getEncoder().getPosition(); - inputs._elevatorSpeed = _primaryMotor.get(); - inputs._elevatorVelocity = _primaryMotor.getEncoder().getVelocity(); + inputs._elevatorSpeed = _secondaryMotor.get(); + inputs._elevatorVelocity = _secondaryMotor.getEncoder().getVelocity(); } public void setElevatorSpeed(double speed) { From ca70c10982e10f821d9595ddf2fed0c746e0e9a6 Mon Sep 17 00:00:00 2001 From: area5188 Date: Wed, 2 Apr 2025 16:09:50 -0400 Subject: [PATCH 092/106] fixed sadface button number --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 37ca443..f50efbd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -115,7 +115,7 @@ public class RobotContainer { private final JoystickButton incrementElevatorButton = new JoystickButton(buttonbox2, 4); private final JoystickButton decrementElevatorButton = new JoystickButton(buttonbox2, 7); - private final JoystickButton recalibrateButton = new JoystickButton(buttonbox2, 7); + private final JoystickButton recalibrateButton = new JoystickButton(buttonbox2, 3); private final JoystickButton dynamic = new JoystickButton(buttonbox2, 8); private final JoystickButton qstatic = new JoystickButton(buttonbox2, 9); From 6fde527ceaf8c2ee9c7fd91a71bcdbc0421d5663 Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Wed, 2 Apr 2025 19:08:35 -0400 Subject: [PATCH 093/106] LED auto-align works but doesn't auto-clear --- src/main/java/frc/robot/subsystems/drive/Drive.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 130b710..3e189ea 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -82,7 +82,7 @@ public class Drive extends SubsystemBase { private static final double WHEEL_COF = 1.13; // For use in getCloseToReef() method: if robot is less than this distance from the reef, LEDs turn green - private static final double MAX_DISTANCE_TO_REEF = Units.inchesToMeters(50); + private static final double MAX_DISTANCE_TO_REEF = Units.inchesToMeters(19.5); static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; @@ -398,8 +398,9 @@ public void addVisionMeasurement( public boolean getCloseToReef() { - Translation2d reefTranslation = FieldConstants.Reef.centerOfReef; - double distanceToReef = getPose().getTranslation().getDistance(reefTranslation); + Pose2d currentPose = getPose(); + Pose2d reefFacePose = FieldConstants.getNearestReefFace(currentPose); + double distanceToReef = currentPose.getTranslation().getDistance(reefFacePose.getTranslation()); // If robot is within MAX_DISTANCE_TO_REEF from center of reef, LEDs can turn green. if (distanceToReef <= MAX_DISTANCE_TO_REEF) { From d206dd9e089d52583be831b7f699268165b325fe Mon Sep 17 00:00:00 2001 From: area5188 Date: Wed, 2 Apr 2025 23:39:23 -0400 Subject: [PATCH 094/106] changed LED command to runcommand --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../frc/robot/subsystems/arm/ArmCommands.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../robot/subsystems/leds/LEDsCommands.java | 37 +++++++++++-------- 4 files changed, 26 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f50efbd..6ba0266 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -245,7 +245,7 @@ private Command joystickApproach(Supplier approachPose) return DriveCommands.joystickApproach( drive, () -> -joystick.getLeftY() * speedMultiplier, - approachPose).alongWith(LEDsCommands.aligningWithReef(() -> drive.getCloseToReef())); + approachPose).alongWith(LEDCommands.aligningWithReef(() -> drive.getCloseToReef())); } private void configureBindings() { @@ -296,6 +296,7 @@ private void configureBindings() { .whileTrue( joystickApproach( () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.RIGHT))); + // Driver Left Bumper: Approach Nearest Left-Side Reef Branch joystick.leftBumper() diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 86713a1..945d9da 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -116,7 +116,7 @@ public void execute() { counter = 0; } else { // Run the motors backwards until we see the piece in the upper light sensor - _arm.setIntakeSpeed(-0.09); + _arm.setIntakeSpeed(-0.09); // changed to -0.11 to prevent dropping piece while moving, seemed to work well done = _arm.upperLightSensorSeesGamepiece(); } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3e189ea..f85ce57 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -82,7 +82,7 @@ public class Drive extends SubsystemBase { private static final double WHEEL_COF = 1.13; // For use in getCloseToReef() method: if robot is less than this distance from the reef, LEDs turn green - private static final double MAX_DISTANCE_TO_REEF = Units.inchesToMeters(19.5); + private static final double MAX_DISTANCE_TO_REEF = Units.inchesToMeters(20.5); // was 19.5 static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; diff --git a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java index 8a59c68..538660d 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.subsystems.leds.LEDs; @@ -105,20 +106,26 @@ public boolean runsWhenDisabled() { }; } - public static Command aligningWithReef(BooleanSupplier closeToReef) { - return new StartEndCommand( - () -> { - _leds.aligningWithReefAnimation(closeToReef); - }, - () -> { - _leds.reset(); - }, - _leds) { - @Override - public boolean runsWhenDisabled() { - return false; - } - }; - } + // public Command aligningWithReef(BooleanSupplier closeToReef) { + // return new StartEndCommand( + // () -> { + // _leds.aligningWithReefAnimation(closeToReef); + // }, + // () -> { + // _leds.reset(); + // }, + // _leds).repeatedly(); //{ + // // @Override + // // public boolean runsWhenDisabled() { + // // return false; + // // } + // // }; + // } + + public Command aligningWithReef(BooleanSupplier closeToReef) { + return new RunCommand( + () -> _leds.aligningWithReefAnimation(closeToReef), + _leds).finallyDo((interrupted) -> _leds.reset()); + } } \ No newline at end of file From 8879f1323c466c7a1aa1b6bd46b2bf87d7834478 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Thu, 3 Apr 2025 20:54:37 -0400 Subject: [PATCH 095/106] Changes from practice at states --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/subsystems/arm/Arm.java | 2 +- src/main/java/frc/robot/subsystems/arm/ArmCommands.java | 2 +- src/main/java/frc/robot/subsystems/elevator/Elevator.java | 4 ++-- .../java/frc/robot/subsystems/elevator/io/RealElevatorIO.java | 2 +- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6ba0266..1b41c1e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -277,8 +277,8 @@ private void configureBindings() { StowButton.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.Stow)); L1Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L1)); - L2Button.onTrue(Commands.either(multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L2), multiSubsystemCommands.moveToPosition(OverallPosition.L2), () -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); - L3Button.onTrue(Commands.either(multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L3), multiSubsystemCommands.moveToPosition(OverallPosition.L3), () -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); + L2Button.onTrue(Commands.either(multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L2).raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving()), multiSubsystemCommands.moveToPosition(OverallPosition.L2), () -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); + L3Button.onTrue(Commands.either(multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L3).raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving()), multiSubsystemCommands.moveToPosition(OverallPosition.L3), () -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); L4Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L4)); gamepieceModeToggle.whileTrue(multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE).alongWith(LEDCommands.pickingUpAlgae())); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 19e7413..1810677 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -96,7 +96,7 @@ public void setIntakeSpeed(double speed) { } public void spit() { - double speed = (_currentMode == GamepieceMode.ALGAE) ? -0.5 : 0.4; + double speed = (_currentMode == GamepieceMode.ALGAE) ? -0.5 : 0.3; setIntakeSpeed(speed); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 945d9da..b80721f 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -40,7 +40,7 @@ public Command setArmPosition(ArmPosition setpoint) { return new InstantCommand( () -> { _arm.setArmSetpoint(setpoint); - }).andThen(intakeForNumberOfRotations()); + });//.andThen(intakeForNumberOfRotations()); } return new InstantCommand( () -> { diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 63a7955..197de28 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -35,8 +35,8 @@ public class Elevator extends SubsystemBase { public enum ElevatorPosition { L1(7, 5), - L2(13, 18), - L3(29, 36), + L2(12, 18), + L3(28, 36), L4(51.8, 50), Stow(0.5, 0.5); diff --git a/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java index 4f497d9..cdb6536 100644 --- a/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java @@ -49,6 +49,6 @@ public void setElevatorVoltage(Voltage voltage) { } public void resetEncoder() { - _primaryMotor.getEncoder().setPosition(0); + _secondaryMotor.getEncoder().setPosition(0); } } From 4c76f9b91aa728432c1e557418c7c0bbf4673ca6 Mon Sep 17 00:00:00 2001 From: area5188 Date: Fri, 4 Apr 2025 09:40:36 -0400 Subject: [PATCH 096/106] Fixed requirements issue --- .../java/frc/robot/subsystems/elevator/ElevatorCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java index 1825d55..482bb76 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java @@ -65,7 +65,7 @@ public Command setElevatorSetpoint(ElevatorPosition setpoint) { return new InstantCommand( () -> { _elevator.setSetpoint(setpoint); - }, _elevator); + }); } public Command waitUntilAtSetpoint() { From 69245f8c170874571bcf1502a172bf70a8ce2542 Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 4 Apr 2025 09:51:40 -0400 Subject: [PATCH 097/106] Added auto changes --- .../pathplanner/autos/A-2PieceAlgae.auto | 107 ++++++++++++++++++ .../pathplanner/autos/A-3PieceL4-GOOD.auto | 2 +- .../deploy/pathplanner/autos/B-Score.auto | 38 ------- .../pathplanner/autos/C-2PieceAlgae.auto | 95 ++++++++++++++++ .../pathplanner/autos/C-3PieceL4-GOOD.auto | 2 +- .../pathplanner/autos/Fast-A-3Piece.auto | 2 +- .../deploy/pathplanner/autos/PushScore.auto | 51 --------- .../deploy/pathplanner/paths/Fast_R3_HP1.path | 6 +- src/main/deploy/pathplanner/settings.json | 4 +- 9 files changed, 210 insertions(+), 97 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/A-2PieceAlgae.auto delete mode 100644 src/main/deploy/pathplanner/autos/B-Score.auto create mode 100644 src/main/deploy/pathplanner/autos/C-2PieceAlgae.auto delete mode 100644 src/main/deploy/pathplanner/autos/PushScore.auto diff --git a/src/main/deploy/pathplanner/autos/A-2PieceAlgae.auto b/src/main/deploy/pathplanner/autos/A-2PieceAlgae.auto new file mode 100644 index 0000000..ce492d6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-2PieceAlgae.auto @@ -0,0 +1,107 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A_R3" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "R3_HP1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HP1_R2" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BackR2" + } + }, + { + "type": "named", + "data": { + "name": "AlgaeL2" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Algae Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto b/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto index ec86307..dbc301e 100644 --- a/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto +++ b/src/main/deploy/pathplanner/autos/A-3PieceL4-GOOD.auto @@ -115,6 +115,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Reliable Autos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/B-Score.auto b/src/main/deploy/pathplanner/autos/B-Score.auto deleted file mode 100644 index 334e6ae..0000000 --- a/src/main/deploy/pathplanner/autos/B-Score.auto +++ /dev/null @@ -1,38 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "path", - "data": { - "pathName": "B_R4" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Stow" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/C-2PieceAlgae.auto b/src/main/deploy/pathplanner/autos/C-2PieceAlgae.auto new file mode 100644 index 0000000..f96793d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/C-2PieceAlgae.auto @@ -0,0 +1,95 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "C_R5" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "R5_HP2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "HP2_R6" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BackR6" + } + }, + { + "type": "named", + "data": { + "name": "AlgaeL2" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Reliable Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto b/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto index 17dc1a6..647fd0a 100644 --- a/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto +++ b/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto @@ -103,6 +103,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Algae Autos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Fast-A-3Piece.auto b/src/main/deploy/pathplanner/autos/Fast-A-3Piece.auto index fd00d75..0ca4862 100644 --- a/src/main/deploy/pathplanner/autos/Fast-A-3Piece.auto +++ b/src/main/deploy/pathplanner/autos/Fast-A-3Piece.auto @@ -93,7 +93,7 @@ { "type": "named", "data": { - "name": "L4" + "name": "L1" } } ] diff --git a/src/main/deploy/pathplanner/autos/PushScore.auto b/src/main/deploy/pathplanner/autos/PushScore.auto deleted file mode 100644 index beff776..0000000 --- a/src/main/deploy/pathplanner/autos/PushScore.auto +++ /dev/null @@ -1,51 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "PushScore" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "BackR4" - } - }, - { - "type": "named", - "data": { - "name": "Stow" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fast_R3_HP1.path b/src/main/deploy/pathplanner/paths/Fast_R3_HP1.path index 2d25210..4e273f5 100644 --- a/src/main/deploy/pathplanner/paths/Fast_R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/Fast_R3_HP1.path @@ -53,8 +53,8 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 1.1452914798206282, + "minWaypointRelativePos": 0.21135265700483089, + "maxWaypointRelativePos": 0.9553140096618359, "constraints": { "maxVelocity": 10.0, "maxAcceleration": 5.0, @@ -69,7 +69,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 800.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index a3eb481..eb12f29 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -15,8 +15,8 @@ "autoFolders": [ "FAST AUTOS", "LeaveStart", - "StartToReef", - "Test" + "Algae Autos", + "Reliable Autos" ], "defaultMaxVel": 5.0, "defaultMaxAccel": 3.0, From eb3441b0490bb733ab3fe4f31ed6c573c91fa50b Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 4 Apr 2025 11:14:46 -0400 Subject: [PATCH 098/106] Added B-Score Auto --- .../deploy/pathplanner/autos/B-Score.auto | 50 +++++++++++++++++++ .../frc/robot/subsystems/arm/ArmCommands.java | 2 +- 2 files changed, 51 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/pathplanner/autos/B-Score.auto diff --git a/src/main/deploy/pathplanner/autos/B-Score.auto b/src/main/deploy/pathplanner/autos/B-Score.auto new file mode 100644 index 0000000..0bca30e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/B-Score.auto @@ -0,0 +1,50 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "B_R4" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "path", + "data": { + "pathName": "BackR4" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index b80721f..945d9da 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -40,7 +40,7 @@ public Command setArmPosition(ArmPosition setpoint) { return new InstantCommand( () -> { _arm.setArmSetpoint(setpoint); - });//.andThen(intakeForNumberOfRotations()); + }).andThen(intakeForNumberOfRotations()); } return new InstantCommand( () -> { From 95430287c3636b465416597eac618df2ec58e5bf Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 4 Apr 2025 15:39:40 -0400 Subject: [PATCH 099/106] Sepd up some auto Paths --- .../pathplanner/autos/C-2PieceAlgae.auto | 2 +- .../pathplanner/autos/C-3PieceL4-GOOD.auto | 2 +- src/main/deploy/pathplanner/paths/A_R3.path | 18 +- src/main/deploy/pathplanner/paths/C_R5.path | 18 +- src/main/deploy/pathplanner/paths/R2_HP1.path | 2 +- src/main/deploy/pathplanner/paths/R3_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R5_HP2.path | 40 ++--- src/main/deploy/pathplanner/paths/R6_HP2.path | 22 ++- src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/subsystems/elevator/Elevator.java | 4 +- .../java/frc/robot/subsystems/leds/LEDs.java | 11 +- .../robot/subsystems/leds/LEDsCommands.java | 169 ++++++++++-------- 12 files changed, 179 insertions(+), 115 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/C-2PieceAlgae.auto b/src/main/deploy/pathplanner/autos/C-2PieceAlgae.auto index f96793d..488cbd0 100644 --- a/src/main/deploy/pathplanner/autos/C-2PieceAlgae.auto +++ b/src/main/deploy/pathplanner/autos/C-2PieceAlgae.auto @@ -90,6 +90,6 @@ } }, "resetOdom": true, - "folder": "Reliable Autos", + "folder": "Algae Autos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto b/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto index 647fd0a..a10abd9 100644 --- a/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto +++ b/src/main/deploy/pathplanner/autos/C-3PieceL4-GOOD.auto @@ -103,6 +103,6 @@ } }, "resetOdom": true, - "folder": "Algae Autos", + "folder": "Reliable Autos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index c250c80..aedd910 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -34,12 +34,26 @@ "rotationDegrees": -121.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.22644927536231882, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 2.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index a0c8922..0bc25d6 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -34,12 +34,26 @@ "rotationDegrees": 162.15573333673396 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.4655797101449276, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 2.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R2_HP1.path b/src/main/deploy/pathplanner/paths/R2_HP1.path index 3ec6045..95167f3 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP1.path +++ b/src/main/deploy/pathplanner/paths/R2_HP1.path @@ -48,7 +48,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 3.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R3_HP1.path b/src/main/deploy/pathplanner/paths/R3_HP1.path index 2ebcbff..6a7e121 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/R3_HP1.path @@ -53,8 +53,8 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.6860986547085202, - "maxWaypointRelativePos": 1.626008968609865, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.7282608695652175, "constraints": { "maxVelocity": 10.0, "maxAcceleration": 4.5, diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index e3846d1..7a3f9f2 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -8,28 +8,12 @@ }, "prevControl": null, "nextControl": { - "x": 6.144098360655739, - "y": 1.9927643442622944 + "x": 4.4354508196721305, + "y": 0.49462090163934347 }, "isLocked": false, "linkedName": "ScoreR5" }, - { - "anchor": { - "x": 4.244138093171296, - "y": 1.7139442274305554 - }, - "prevControl": { - "x": 4.794722222222221, - "y": 1.697697482638889 - }, - "nextControl": { - "x": 3.8469329960827787, - "y": 1.7256650335741508 - }, - "isLocked": false, - "linkedName": null - }, { "anchor": { "x": 1.599, @@ -46,16 +30,30 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.55, + "waypointRelativePos": 0.3, "rotationDegrees": 85.85745820949654 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 800.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 3.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 800.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index 14fa1c8..0ac5890 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -20,8 +20,8 @@ "y": 0.670999999999999 }, "prevControl": { - "x": 1.7602751563782435, - "y": 0.8620244066478748 + "x": 1.7602751563782437, + "y": 0.8620244066478749 }, "nextControl": null, "isLocked": false, @@ -29,12 +29,26 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.42210144927536214, + "constraints": { + "maxVelocity": 10.0, + "maxAcceleration": 4.5, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 800.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 10.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 800.0, "nominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1b41c1e..de12470 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -357,7 +357,7 @@ public void calibrate() { } public void startIdleAnimations() { - Command disabled1 = LEDCommands.disabledAnimation1(); + Command disabled1 = ((LEDCommands.disabledAnimation1().withTimeout(20)).andThen(LEDCommands.disabledAnimation2().withTimeout(20)).repeatedly()); if (!CommandScheduler.getInstance().isScheduled(disabled1)) CommandScheduler.getInstance().schedule(disabled1); } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 197de28..63a7955 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -35,8 +35,8 @@ public class Elevator extends SubsystemBase { public enum ElevatorPosition { L1(7, 5), - L2(12, 18), - L3(28, 36), + L2(13, 18), + L3(29, 36), L4(51.8, 50), Stow(0.5, 0.5); diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java index baf4efc..e589382 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.leds; import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.FireAnimation; import com.ctre.phoenix.led.LarsonAnimation; import com.ctre.phoenix.led.LarsonAnimation.BounceMode; @@ -16,6 +17,7 @@ import com.ctre.phoenix.led.RainbowAnimation; import com.ctre.phoenix.led.StrobeAnimation; +import com.ctre.phoenix.led.TwinkleAnimation; import java.util.function.BooleanSupplier; @@ -48,7 +50,7 @@ public enum LEDAnimation { PartyMode(null, new RainbowAnimation(100, 1, _numLEDs), 3), - Bounce(null, new LarsonAnimation(0, 255, 0), 3), + Bounce(null, new TwinkleAnimation(0, 255, 0), 3), SolidTeal(new LEDColor(0, 225, 100), null, 0), @@ -175,6 +177,13 @@ public void disabledAnimation1() { } } + public void disabledAnimation2() { + if (!_alreadyRunning) { + runAnimation(LEDAnimation.Bounce); + _alreadyRunning = false; + } + } + public void reset() { _candle.clearAnimation(0); _alreadyRunning = false; diff --git a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java index 538660d..315355b 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java @@ -12,63 +12,62 @@ public class LEDsCommands { private static LEDs _leds; - - public LEDsCommands(LEDs leds) { - _leds = leds; - } - - //Runs when intake button is pushed - public Command intaking() { - return new StartEndCommand( - () -> { - _leds.intaking(); - }, - () -> { - _leds.reset(); - }, - _leds); - } - - //Runs when we have just acquired a piece - public Command hasPiece() { - return new StartEndCommand( + + public LEDsCommands(LEDs leds) { + _leds = leds; + } + + // Runs when intake button is pushed + public Command intaking() { + return new StartEndCommand( + () -> { + _leds.intaking(); + }, + () -> { + _leds.reset(); + }, + _leds); + } + + // Runs when we have just acquired a piece + public Command hasPiece() { + return new StartEndCommand( () -> { _leds.hasPiece(); - }, + }, () -> { _leds.reset(); }, _leds).withTimeout(3); - } - - // Runs when we have no gamepiece and we are toggled to pick up coral - public Command pickingUpCoral() { - return new StartEndCommand( + } + + // Runs when we have no gamepiece and we are toggled to pick up coral + public Command pickingUpCoral() { + return new StartEndCommand( () -> { _leds.pickingUpCoral(); - }, + }, () -> { _leds.reset(); }, _leds); - } - - // Runs when we have no gamepiece and we are toggled to pick up coral - public Command pickingUpAlgae() { - return new StartEndCommand( + } + + // Runs when we have no gamepiece and we are toggled to pick up coral + public Command pickingUpAlgae() { + return new StartEndCommand( () -> { _leds.pickingUpAlgae(); - }, + }, () -> { _leds.reset(); }, _leds); - } - - - //We have a piece and hasPiece animation has already run - public Command elevatorOrArmIsMoving() { - return new StartEndCommand( + } + + // We have a piece and hasPiece animation has already run + public Command elevatorOrArmIsMoving() { + return new StartEndCommand( () -> { _leds.elevatorOrArmIsMoving(); }, @@ -76,56 +75,72 @@ public Command elevatorOrArmIsMoving() { _leds.reset(); }, _leds); - } - - //Runs when robot has finished climbing - public Command robotHasClimbed() { - return new StartEndCommand( + } + + // Runs when robot has finished climbing + public Command robotHasClimbed() { + return new StartEndCommand( () -> { _leds.robotHasClimbed(); - }, + }, () -> { _leds.reset(); }, - _leds); - } - - public Command disabledAnimation1() { - return new StartEndCommand( + _leds); + } + + public Command disabledAnimation1() { + return new StartEndCommand( () -> { _leds.disabledAnimation1(); - }, + }, () -> { _leds.reset(); }, - _leds) { - @Override - public boolean runsWhenDisabled() { - return true; - } - }; - } - - // public Command aligningWithReef(BooleanSupplier closeToReef) { - // return new StartEndCommand( - // () -> { - // _leds.aligningWithReefAnimation(closeToReef); - // }, - // () -> { - // _leds.reset(); - // }, - // _leds).repeatedly(); //{ - // // @Override - // // public boolean runsWhenDisabled() { - // // return false; - // // } - // // }; + _leds) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }; + } + + public Command disabledAnimation2() { + return new StartEndCommand( + () -> { + _leds.disabledAnimation2(); + }, + () -> { + _leds.reset(); + }, + _leds) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }; + } + + // public Command aligningWithReef(BooleanSupplier closeToReef) { + // return new StartEndCommand( + // () -> { + // _leds.aligningWithReefAnimation(closeToReef); + // }, + // () -> { + // _leds.reset(); + // }, + // _leds).repeatedly(); //{ + // // @Override + // // public boolean runsWhenDisabled() { + // // return false; + // // } + // // }; // } - public Command aligningWithReef(BooleanSupplier closeToReef) { - return new RunCommand( - () -> _leds.aligningWithReefAnimation(closeToReef), + public Command aligningWithReef(BooleanSupplier closeToReef) { + return new RunCommand( + () -> _leds.aligningWithReefAnimation(closeToReef), _leds).finallyDo((interrupted) -> _leds.reset()); - } + } } \ No newline at end of file From b5f6c09a6722fab3d298005a4fb0a0dc0e638995 Mon Sep 17 00:00:00 2001 From: area5188 Date: Fri, 4 Apr 2025 15:48:08 -0400 Subject: [PATCH 100/106] Raised L2, L3 setpoints and made spit faster --- src/main/java/frc/robot/subsystems/arm/Arm.java | 2 +- src/main/java/frc/robot/subsystems/elevator/Elevator.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 1810677..e2ec2a3 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -96,7 +96,7 @@ public void setIntakeSpeed(double speed) { } public void spit() { - double speed = (_currentMode == GamepieceMode.ALGAE) ? -0.5 : 0.3; + double speed = (_currentMode == GamepieceMode.ALGAE) ? -0.5 : 0.35; setIntakeSpeed(speed); } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 197de28..63a7955 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -35,8 +35,8 @@ public class Elevator extends SubsystemBase { public enum ElevatorPosition { L1(7, 5), - L2(12, 18), - L3(28, 36), + L2(13, 18), + L3(29, 36), L4(51.8, 50), Stow(0.5, 0.5); From e4f0ec7e6c85080ff833d9b9baaea9e4a2c7b74f Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 4 Apr 2025 19:02:10 -0400 Subject: [PATCH 101/106] Changes added --- src/main/java/frc/robot/Robot.java | 7 ++- src/main/java/frc/robot/RobotContainer.java | 14 +++-- .../frc/robot/subsystems/arm/ArmCommands.java | 61 +++++++++++++------ .../subsystems/elevator/ElevatorCommands.java | 1 + .../MultiSubsystemCommands.java | 5 ++ 5 files changed, 63 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ff62b0c..ba50c8e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -48,6 +48,8 @@ public class Robot extends LoggedRobot { public static final double fieldWidth = Units.inchesToMeters(317); public static final Translation2d fieldCenter = new Translation2d(fieldLength / 2, fieldWidth / 2); + private boolean ranAutonomous = false; + private final RobotContainer m_robotContainer = new RobotContainer(); public Robot() { @@ -182,6 +184,8 @@ public void autonomousInit() { m_robotContainer.calibrate(); m_robotContainer.startEnabledLEDs(); + + ranAutonomous = true; } @Override @@ -197,7 +201,8 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - m_robotContainer.calibrate(); + if (!ranAutonomous) + m_robotContainer.calibrate(); m_robotContainer.startEnabledLEDs(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de12470..b2d5ada 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -201,23 +201,23 @@ public RobotContainer() { // AutoAlignToReef + ScoreL1 (Move to L1, score) NamedCommands.registerCommand("L1", - multiSubsystemCommands.scoreGamepieceAtPosition(OverallPosition.L1)); + multiSubsystemCommands.moveToPosition(OverallPosition.L1)); // AutoAlignToReef + ScoreL2 (Move to L2, score) NamedCommands.registerCommand("L2", - multiSubsystemCommands.scoreGamepieceAtPosition(OverallPosition.L2)); + multiSubsystemCommands.moveToPosition(OverallPosition.L2)); // AutoAlignToReef + ScoreL3 (Move to L3, score) NamedCommands.registerCommand("L3", - multiSubsystemCommands.scoreGamepieceAtPosition(OverallPosition.L3)); + multiSubsystemCommands.moveToPosition(OverallPosition.L3)); // AutoAlignToReef + Move to L4 + Score NamedCommands.registerCommand("L4", - multiSubsystemCommands.scoreGamepieceAtPosition(OverallPosition.L4)); + multiSubsystemCommands.moveToPosition(OverallPosition.L4)); // AutoAlign to Intake + Intake NamedCommands.registerCommand("Intake", - multiSubsystemCommands.loadCoral()); + multiSubsystemCommands.loadCoralAuto()); // AutoAlign + Algae Removal NamedCommands.registerCommand("AlgaeL2", @@ -227,6 +227,10 @@ public RobotContainer() { NamedCommands.registerCommand("AlgaeL3", multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L3)); + // Moves the gamepiece back + NamedCommands.registerCommand("MovePiece", + armCommands.moveGamepieceToLightSensor()); + NamedCommands.registerCommand("Stow", multiSubsystemCommands.moveToPosition(OverallPosition.Stow)); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 945d9da..d1b5ef0 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -30,8 +30,8 @@ public Command spit() { _arm.setIntakeSpeed(0); _arm.clearHasGamepiece(); }, _arm).withTimeout(0.5) - .andThen(Commands.runOnce(() -> _arm.setArmSetpoint(ArmPosition.Stow), _arm).unless(() -> _arm.getCurrentPos() != ArmPosition.L4_Score)); - + .andThen(Commands.runOnce(() -> _arm.setArmSetpoint(ArmPosition.Stow), _arm) + .unless(() -> _arm.getCurrentPos() != ArmPosition.L4_Score)); } @@ -54,23 +54,22 @@ public Command intake() { } private Command intakeCoralWithAdjust() { - if (RobotState.isAutonomous()) { - return intakeCoral(); - } else { return intakeCoral().andThen(moveArm(ArmPosition.Stow)); - } } private Command intakeCoral() { Command c = new Command() { /* - * This command will run the intake at a fast speed until the lower light sensor detects the coral + * This command will run the intake at a fast speed until the lower light sensor + * detects the coral * Then, the speed will decrease and run until both sensors detect the coral * Then, the motor will turn off for a few cycles (~100 ms) - * Then, the motor will run at a slow speed in reverse until the upper light sensor detects the coral + * Then, the motor will run at a slow speed in reverse until the upper light + * sensor detects the coral * Then, the arm will go back to stow * - * In the case that the lower and upper light sensors don't detect a piece when running reverse, + * In the case that the lower and upper light sensors don't detect a piece when + * running reverse, * we assume there is no piece and start running at a fast speed again */ boolean hasPieceDetected = false; @@ -116,7 +115,8 @@ public void execute() { counter = 0; } else { // Run the motors backwards until we see the piece in the upper light sensor - _arm.setIntakeSpeed(-0.09); // changed to -0.11 to prevent dropping piece while moving, seemed to work well + _arm.setIntakeSpeed(-0.09); // changed to -0.11 to prevent dropping piece while moving, seemed + // to work well done = _arm.upperLightSensorSeesGamepiece(); } } @@ -136,6 +136,29 @@ public boolean isFinished() { return c; } + public Command intakeCoralAuto() { + return new Command() { + @Override + public void execute() { + if (_arm.upperLightSensorSeesGamepiece()) { + _arm.setIntakeSpeed(0.09); + } else { + _arm.setIntakeSpeed(0.22); // 0.35 + } + } + + @Override + public void end(boolean interrupted) { + _arm.setIntakeSpeed(0); + } + + @Override + public boolean isFinished() { + return _arm.hasPiece(); + } + }; + } + private Command intakeAlgae() { Command c = new Command() { @@ -168,7 +191,7 @@ public Command moveGamepieceToLightSensor() { @Override public void execute() { - double speed = -0.1; // -0.08 + double speed = -0.09; // -0.08 _arm.setIntakeSpeed(speed); } @@ -208,14 +231,14 @@ public Command resetArmPID() { public Command manualIntake() { return Commands.either( - new StartEndCommand( - () -> _arm.setIntakeSpeed(-0.1), - () -> _arm.setIntakeSpeed(0), - _arm), - new StartEndCommand( - () -> _arm.setIntakeSpeed(0.5), - () -> _arm.setIntakeSpeed(0), - _arm), + new StartEndCommand( + () -> _arm.setIntakeSpeed(-0.1), + () -> _arm.setIntakeSpeed(0), + _arm), + new StartEndCommand( + () -> _arm.setIntakeSpeed(0.5), + () -> _arm.setIntakeSpeed(0), + _arm), () -> _arm.getCurrentMode() == GamepieceMode.CORAL); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java index 482bb76..1748a14 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java @@ -64,6 +64,7 @@ public Command incrementElevatorPosition() { public Command setElevatorSetpoint(ElevatorPosition setpoint) { return new InstantCommand( () -> { + System.out.println("RUNNING ELEVATOR CMD"); _elevator.setSetpoint(setpoint); }); } diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index a024275..5505f20 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -93,6 +93,11 @@ public Command loadCoral() { .andThen(_armCommands.intake()); } + public Command loadCoralAuto() { + return moveToPosition(OverallPosition.Coral_Loading) + .andThen(_armCommands.intakeCoralAuto()); + } + public Command loadAlgae(OverallPosition position) { if (position != OverallPosition.Algae_Loading_L2 && position != OverallPosition.Algae_Loading_L3) { throw new IllegalArgumentException("Can Only Load Algae @ L2 or L3"); From bd2ba9a5220f20d281ec660b8c4290ecffe536b3 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Fri, 4 Apr 2025 21:16:13 -0400 Subject: [PATCH 102/106] Named commands for debugging --- src/main/java/frc/robot/RobotContainer.java | 3 ++- .../frc/robot/subsystems/arm/ArmCommands.java | 17 ++++++++++------- .../subsystems/elevator/ElevatorCommands.java | 7 ++++--- .../MultiSubsystemCommands.java | 18 ++++++++++++------ 4 files changed, 28 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b2d5ada..7b3e100 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -249,7 +249,8 @@ private Command joystickApproach(Supplier approachPose) return DriveCommands.joystickApproach( drive, () -> -joystick.getLeftY() * speedMultiplier, - approachPose).alongWith(LEDCommands.aligningWithReef(() -> drive.getCloseToReef())); + approachPose).alongWith(LEDCommands.aligningWithReef(() -> drive.getCloseToReef())) + .withName("AutoAlign"); } private void configureBindings() { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index d1b5ef0..e50973a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -31,7 +31,8 @@ public Command spit() { _arm.clearHasGamepiece(); }, _arm).withTimeout(0.5) .andThen(Commands.runOnce(() -> _arm.setArmSetpoint(ArmPosition.Stow), _arm) - .unless(() -> _arm.getCurrentPos() != ArmPosition.L4_Score)); + .unless(() -> _arm.getCurrentPos() != ArmPosition.L4_Score)) + .withName("Spit"); } @@ -208,13 +209,14 @@ public boolean isFinished() { } public Command intakeForNumberOfRotations() { - return new StartEndCommand(() -> { - _arm.resetIntakeEncoders(); - _arm.setIntakeSpeed(-0.175); // -0.1 - }, + return new StartEndCommand( + () -> { + _arm.resetIntakeEncoders(); + _arm.setIntakeSpeed(-0.175); // -0.1 + }, () -> { _arm.setIntakeSpeed(0); - }).until(() -> _arm.intakeAtDesiredRotations()); + }, _arm).until(() -> _arm.intakeAtDesiredRotations()); } public Command waitUntilAtSetpoint() { @@ -239,6 +241,7 @@ public Command manualIntake() { () -> _arm.setIntakeSpeed(0.5), () -> _arm.setIntakeSpeed(0), _arm), - () -> _arm.getCurrentMode() == GamepieceMode.CORAL); + () -> _arm.getCurrentMode() == GamepieceMode.CORAL) + .withName("ManualIntake"); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java index 1748a14..0d0b6e9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java @@ -50,7 +50,8 @@ public Command decrementElevatorPosition() { () -> { _elevator.decrementElevatorPosition(); - }, _elevator); + }, _elevator) + .withName("DecrementElevator"); } public Command incrementElevatorPosition() { @@ -58,13 +59,13 @@ public Command incrementElevatorPosition() { () -> { _elevator.incrementElevatorPosition(); - }, _elevator); + }, _elevator) + .withName("IncrementElevator"); } public Command setElevatorSetpoint(ElevatorPosition setpoint) { return new InstantCommand( () -> { - System.out.println("RUNNING ELEVATOR CMD"); _elevator.setSetpoint(setpoint); }); } diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 5505f20..26dd15e 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -65,14 +65,16 @@ public MultiSubsystemCommands(Elevator elevator, Arm arm, ElevatorCommands eleva public Command calibrate() { return _armCommands.setArmPosition(ArmPosition.Stow) .andThen(_elevatorCommands.calibrateElevator()) - .andThen(_elevatorCommands.setElevatorSetpoint(ElevatorPosition.Stow)); + .andThen(_elevatorCommands.setElevatorSetpoint(ElevatorPosition.Stow)) + .withName("Calibrate"); } public Command moveToPosition(OverallPosition setpoint) { return _armCommands.moveArm(ArmPosition.Transient) .andThen(_elevatorCommands.moveElevator(setpoint.getElevatorPosition())) .unless(() -> _elevator.getCurrentPos() == setpoint.getElevatorPosition()) - .andThen(_armCommands.moveArm(setpoint.getArmPosition())); + .andThen(_armCommands.moveArm(setpoint.getArmPosition())) + .withName("MoveToPosition"); } public Command setGamepieceMode(GamepieceMode mode) { @@ -80,17 +82,20 @@ public Command setGamepieceMode(GamepieceMode mode) { () -> { _elevator.setCurrentMode(mode); _arm.setCurrentMode(mode); - }, _elevator, _arm); + }) + .withName("SetGamepieceMode"); } public Command scoreGamepieceAtPosition(OverallPosition setpoint) { return moveToPosition(setpoint) - .andThen(_armCommands.spit()); + .andThen(_armCommands.spit()) + .withName("ScoreAtPosition"); } public Command loadCoral() { return moveToPosition(OverallPosition.Coral_Loading) - .andThen(_armCommands.intake()); + .andThen(_armCommands.intake()) + .withName("LoadCoral"); } public Command loadCoralAuto() { @@ -104,7 +109,8 @@ public Command loadAlgae(OverallPosition position) { } return moveToPosition(position) .alongWith(_armCommands.intake()) - .andThen(_armCommands.setArmPosition(ArmPosition.Stow)); + .andThen(_armCommands.setArmPosition(ArmPosition.Stow) + .withName("LoadAlgae")); } } \ No newline at end of file From 386730776fdb7921933ca478ad131707a95cc58a Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Fri, 4 Apr 2025 21:30:46 -0400 Subject: [PATCH 103/106] Added requires for arm commands --- .../frc/robot/subsystems/arm/ArmCommands.java | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index e50973a..d72dd2b 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -138,7 +138,7 @@ public boolean isFinished() { } public Command intakeCoralAuto() { - return new Command() { + Command c = new Command() { @Override public void execute() { if (_arm.upperLightSensorSeesGamepiece()) { @@ -158,6 +158,8 @@ public boolean isFinished() { return _arm.hasPiece(); } }; + c.addRequirements(_arm); + return c; } private Command intakeAlgae() { @@ -188,7 +190,7 @@ public boolean isFinished() { } public Command moveGamepieceToLightSensor() { - return new Command() { + Command c = new Command() { @Override public void execute() { @@ -206,6 +208,9 @@ public boolean isFinished() { return _arm.upperLightSensorSeesGamepiece(); } }; + c.addRequirements(_arm); + + return c; } public Command intakeForNumberOfRotations() { @@ -220,17 +225,15 @@ public Command intakeForNumberOfRotations() { } public Command waitUntilAtSetpoint() { - return new WaitUntilCommand(_arm::isAtSetpoint); + Command c = new WaitUntilCommand(_arm::isAtSetpoint); + c.addRequirements(_arm); + return c; } public Command moveArm(ArmPosition pos) { return setArmPosition(pos).andThen(waitUntilAtSetpoint()); } - public Command resetArmPID() { - return Commands.runOnce(() -> _arm.resetPID()); - } - public Command manualIntake() { return Commands.either( new StartEndCommand( From bc551e0be5ed5258512390039ae31b1bc71b2d1a Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Fri, 4 Apr 2025 22:09:21 -0400 Subject: [PATCH 104/106] Revert "Added requires for arm commands" This reverts commit 386730776fdb7921933ca478ad131707a95cc58a. --- .../frc/robot/subsystems/arm/ArmCommands.java | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index d72dd2b..e50973a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -138,7 +138,7 @@ public boolean isFinished() { } public Command intakeCoralAuto() { - Command c = new Command() { + return new Command() { @Override public void execute() { if (_arm.upperLightSensorSeesGamepiece()) { @@ -158,8 +158,6 @@ public boolean isFinished() { return _arm.hasPiece(); } }; - c.addRequirements(_arm); - return c; } private Command intakeAlgae() { @@ -190,7 +188,7 @@ public boolean isFinished() { } public Command moveGamepieceToLightSensor() { - Command c = new Command() { + return new Command() { @Override public void execute() { @@ -208,9 +206,6 @@ public boolean isFinished() { return _arm.upperLightSensorSeesGamepiece(); } }; - c.addRequirements(_arm); - - return c; } public Command intakeForNumberOfRotations() { @@ -225,15 +220,17 @@ public Command intakeForNumberOfRotations() { } public Command waitUntilAtSetpoint() { - Command c = new WaitUntilCommand(_arm::isAtSetpoint); - c.addRequirements(_arm); - return c; + return new WaitUntilCommand(_arm::isAtSetpoint); } public Command moveArm(ArmPosition pos) { return setArmPosition(pos).andThen(waitUntilAtSetpoint()); } + public Command resetArmPID() { + return Commands.runOnce(() -> _arm.resetPID()); + } + public Command manualIntake() { return Commands.either( new StartEndCommand( From 7d2def8f053978902727740ddbfb2dfb7de0517f Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Fri, 4 Apr 2025 22:10:34 -0400 Subject: [PATCH 105/106] Should fix auto align cancelling issue --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7b3e100..3501f9c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -282,8 +282,8 @@ private void configureBindings() { StowButton.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.Stow)); L1Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L1)); - L2Button.onTrue(Commands.either(multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L2).raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving()), multiSubsystemCommands.moveToPosition(OverallPosition.L2), () -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); - L3Button.onTrue(Commands.either(multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L3).raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving()), multiSubsystemCommands.moveToPosition(OverallPosition.L3), () -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); + L2Button.onTrue(Commands.either(multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L2), multiSubsystemCommands.moveToPosition(OverallPosition.L2), () -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); + L3Button.onTrue(Commands.either(multiSubsystemCommands.loadAlgae(OverallPosition.Algae_Loading_L3), multiSubsystemCommands.moveToPosition(OverallPosition.L3), () -> armSubsystem.getCurrentMode() == GamepieceMode.ALGAE)); L4Button.onTrue(multiSubsystemCommands.moveToPosition(OverallPosition.L4)); gamepieceModeToggle.whileTrue(multiSubsystemCommands.setGamepieceMode(GamepieceMode.ALGAE).alongWith(LEDCommands.pickingUpAlgae())); From 0656d168e5d3eaa5d5b4609d707f65b1055d9a2e Mon Sep 17 00:00:00 2001 From: area5188 Date: Sat, 5 Apr 2025 09:52:52 -0400 Subject: [PATCH 106/106] sat morning of states --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/arm/ArmCommands.java | 2 +- src/main/java/frc/robot/subsystems/elevator/Elevator.java | 2 +- .../multisubsystemcommands/MultiSubsystemCommands.java | 3 ++- 4 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b2d5ada..d3cb3cb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -103,7 +103,7 @@ public class RobotContainer { private final JoystickButton L1Button = new JoystickButton(buttonbox2, 2); private final JoystickButton L2Button = new JoystickButton(buttonbox1, 8); private final JoystickButton L3Button = new JoystickButton(buttonbox1,5); - private final JoystickButton L4Button = new JoystickButton(buttonbox1, 2); + private final JoystickButton L4Button = new JoystickButton(buttonbox1, 1); private final JoystickButton intakeButton = new JoystickButton(buttonbox1, 7); private final JoystickButton spitButton = new JoystickButton(buttonbox1, 9); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index d1b5ef0..ac67c8f 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -40,7 +40,7 @@ public Command setArmPosition(ArmPosition setpoint) { return new InstantCommand( () -> { _arm.setArmSetpoint(setpoint); - }).andThen(intakeForNumberOfRotations()); + });//.andThen(intakeForNumberOfRotations()); } return new InstantCommand( () -> { diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 63a7955..23f2343 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -40,7 +40,7 @@ public enum ElevatorPosition { L4(51.8, 50), Stow(0.5, 0.5); - double coralHeight, algaeHeight; + final double coralHeight, algaeHeight; ElevatorPosition(double coralHeight, double algaeHeight) { this.coralHeight = coralHeight; diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 5505f20..a6c1b88 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -72,7 +72,8 @@ public Command moveToPosition(OverallPosition setpoint) { return _armCommands.moveArm(ArmPosition.Transient) .andThen(_elevatorCommands.moveElevator(setpoint.getElevatorPosition())) .unless(() -> _elevator.getCurrentPos() == setpoint.getElevatorPosition()) - .andThen(_armCommands.moveArm(setpoint.getArmPosition())); + .andThen(_armCommands.moveArm(setpoint.getArmPosition())) + .finallyDo(() -> System.out.println("MOVED TO POS")); } public Command setGamepieceMode(GamepieceMode mode) {