diff --git a/.gitignore b/.gitignore index 2c7dcf3..c5c9340 100644 --- a/.gitignore +++ b/.gitignore @@ -55,4 +55,4 @@ Thumbs.db .vscode/ # sim files -.ctre_sim/ +ctre_sim/ diff --git a/docs/intake/velocity-compensated-intake.md b/docs/intake/velocity-compensated-intake.md new file mode 100644 index 0000000..42c825d --- /dev/null +++ b/docs/intake/velocity-compensated-intake.md @@ -0,0 +1,233 @@ +# Proposal: Velocity-Compensated Intake Roller Speed + +## The Problem + +When our robot drives toward a game piece, the intake rollers sometimes push it away instead of picking it up. This gets worse the faster we drive. + +Here's why: if the robot approaches a piece at 2 m/s but the rollers only spin at a surface speed of 2 m/s, there's zero net "grabbing" force. The rollers need to spin **faster** than the robot is moving to reliably pull pieces in. + +## What Other Teams Do + +A widely referenced guideline (from Team 254, documented on [FRCDesign.org](https://www.frcdesign.org/learning-course/stage2/2C/intake-golden-rules/)) is: + +> Intake roller surface speed should be at least **2x** the robot's ground speed. + +Team 1678 [designed their Rapid React intake](https://www.frcdesign.org/mechanism-examples/intake/linkage/1678_2022_intake/) at 2.5x. The idea is simple: if you might drive at 3 m/s while intaking, your rollers should spin at 6+ m/s surface speed. + +## Where We Stand Today + +### Assumptions + +The estimates below depend on the following values: + +1. Auto commands `DRIVE_SPEED_MPS = 2` (`LeftNeutralZoneAuto1`, line 22). We assume the robot actually drives at approximately 2.0 m/s, which is good enough for the rough calculations in this section. + +2. Roller surface speed can be estimated from physical parameters: + - Gear ratio: 18-tooth motor gear / 24-tooth roller gear = 0.75 reduction + - Roller OD: 1.25" base + 2 × ~1/8" silicone sleeve ≈ 1.5" (0.0381 m) + - Motor: NEO Vortex (published free speed 6784 RPM). Under load we estimate ~5400 RPM. + - Max roller surface speed: `5400 × 0.75 × π × 0.0381 / 60 ≈ 8.1 m/s` + +### Current Auto Behavior + +The following two constants are used to calculate the intake motor output used in `LeftNeutralZoneAuto1.java`: + +The max percent output for the intake motor is specified on line 318 in `Constants.java`: + +```java +INTAKE_SPEED = 0.50; +``` + +The intake power requested during auto is specified on line 35 of `LeftNeutralZoneAuto1.java`: +```java +INTAKE_POWER = -0.5; // negative means intake direction (positive would eject) +``` + +On lines 50 and 103 in `LeftNeutralZoneAuto1.java`, the auto routine calls `IntakeSubsystem.setIntakePower(double power)` with `power` argument equal to `INTAKE_POWER`. This sets the motor output to the following: +```java +INTAKE_POWER * INTAKE_SPEED = (-0.5) * 0.5 = -0.25 +``` + +So actual motor output during auto is **25%**, giving a roller surface speed of roughly `8.1 × 0.25 ≈` **2.0 m/s**. Our auto drives at roughly **2.0 m/s** (see assumptions above), so that's approximately a **1:1 ratio** -- barely matching robot speed and well below the recommended 2x. This likely explains why intake is unreliable while moving. + +## A Possible Approach + +One option would be to make the intake aware of how fast the robot is driving and automatically set roller speed to maintain the 2x ratio. The operator model would be simplified to: + +- **Stick forward** (past deadband) = intake on. Speed is automatic. +- **Stick backward** (past deadband) = eject at a fixed reverse speed. +- **Stick centered** = intake off. + +The operator doesn't need to modulate roller speed -- the "right" speed is determined by the robot's velocity, not feel. + +### The Core Math + +When the intake is on, the roller output is: + +```java +velocityBasedOutput = (robotForwardMps * INTAKE_SPEED_MULTIPLIER) / ROLLER_MAX_SURFACE_SPEED; +output = MathUtil.clamp(velocityBasedOutput, INTAKE_MIN_SPEED, 1.0); +``` + +Where: +- `robotForwardMps` comes from `SwerveSubsystem.getChassisSpeeds().vxMetersPerSecond` +- `INTAKE_SPEED_MULTIPLIER` is the target multiplier (e.g. 2.0, the minimum recommended by other teams) -- tunable +- `ROLLER_MAX_SURFACE_SPEED` is how fast our rollers spin at 100% output (see below) +- `INTAKE_MIN_SPEED` is the floor -- the slowest the rollers will ever spin when the intake is on (e.g. 0.30). This ensures the rollers still grab pieces when the robot is stationary or moving slowly, since the velocity formula would give near-zero at low speeds. + +The `Math.max` handles the transition naturally: at low robot speeds `INTAKE_MIN_SPEED` applies; once the robot is fast enough that the 2x formula exceeds the minimum, velocity compensation takes over. + +### Constants + +**Tuning constants** (adjust based on testing): +- `INTAKE_MIN_SPEED` -- roller output floor when intake is on (e.g. 0.30) +- `INTAKE_SPEED_MULTIPLIER` -- target roller-to-ground-speed ratio (e.g. 2.0) +- `INTAKE_EJECT_SPEED` -- fixed reverse speed for ejecting + +**Physical parameters** (tweak if the hardware changes): + +```java +public static final double ROLLER_MOTOR_MAX_RPM = 5400; // measure this (see below) +public static final int ROLLER_GEAR_MOTOR_TEETH = 18; // gear on motor shaft +public static final int ROLLER_GEAR_ROLLER_TEETH = 24; // gear on roller shaft +public static final double ROLLER_BASE_DIAMETER = Units.inchesToMeters(1.25); +public static final double ROLLER_SLEEVE_THICKNESS = Units.inchesToMeters(0.125); +``` + +**Computed from the above** (not hardcoded): + +```java +// Gear ratio: how many roller turns per motor turn (18:24 = 0.75) +public static final double ROLLER_GEAR_RATIO = + (double) ROLLER_GEAR_MOTOR_TEETH / ROLLER_GEAR_ROLLER_TEETH; + +// Roller outer diameter including the silicone sleeve on each side +public static final double ROLLER_OUTER_DIAMETER = + ROLLER_BASE_DIAMETER + 2 * ROLLER_SLEEVE_THICKNESS; + +// How far the roller surface travels in one full rotation +public static final double ROLLER_CIRCUMFERENCE = + Math.PI * ROLLER_OUTER_DIAMETER; + +// Roller RPM = motor RPM scaled by gear ratio +public static final double ROLLER_MAX_RPM = + ROLLER_MOTOR_MAX_RPM * ROLLER_GEAR_RATIO; + +// Surface speed (m/s) = roller RPM * circumference, converted from per-minute to per-second +public static final double ROLLER_MAX_SURFACE_SPEED = + ROLLER_MAX_RPM * ROLLER_CIRCUMFERENCE / 60.0; +``` + +Each physical parameter is its own constant so we can verify or adjust them independently. For example, if the silicone sleeve turns out to be thinner than 1/8", just change that one value and everything recalculates. + +### Measuring ROLLER_MOTOR_MAX_RPM + +The `5400` above is a placeholder. To get the real value: + +1. Deploy code, open SmartDashboard +2. Lower the intake arm +3. Hold the intake stick at full power (the rollers don't touch the ground, so just run them) +4. Read the motor RPM from SmartDashboard (the SparkMax encoder reports this) +5. Update the constant with the observed value + +This gives a real-world number that accounts for friction, belt/gear losses, etc. + +## What Would Change in the Code + +Three files would be touched. Auto routines would **not** need any changes. + +### Constants.java + +Add the physical parameter and tuning constants shown above. + +### IntakeSubsystem.java + +**Constructor** -- accept a `DoubleSupplier` so the subsystem can read the robot's forward speed each cycle: + +```java +private final java.util.function.DoubleSupplier forwardSpeedSupplier; +private double requestedIntakePower = 0.0; + +public IntakeSubsystem(java.util.function.DoubleSupplier forwardSpeedSupplier) { + this.forwardSpeedSupplier = forwardSpeedSupplier; + // ... rest of existing constructor unchanged ... +} +``` + +**`setIntakePower()`** -- instead of setting the motor directly, just store what the operator requested. The actual motor command happens in `periodic()`: + +```java +public void setIntakePower(double power) { + // store the request power to be used by periodic() + requestedIntakePower = Math.max(-1.0, Math.min(1.0, power)); + intakeOn = Math.abs(requestedIntakePower) > 0.0; +} +``` + +**`periodic()`** -- at the end, after the existing arm PID logic, add roller motor control. This is where the compensation actually happens: + +```java +// Roller velocity compensation +double output = 0.0; + +if (requestedIntakePower < 0) { + // Intaking: apply velocity compensation + double forwardMps = Math.max(0, forwardSpeedSupplier.getAsDouble()); + + // Output calculated from the robot's current velocity + double velocityBasedOutput = + (forwardMps * IntakeConstants.INTAKE_SPEED_MULTIPLIER) / IntakeConstants.ROLLER_MAX_SURFACE_SPEED; + + // Clamp between INTAKE_MIN_SPEED (so we grab pieces even when stationary) + // and 1.0 (motor can't exceed 100%). Negate because intake direction is negative. + output = -MathUtil.clamp(velocityBasedOutput, IntakeConstants.INTAKE_MIN_SPEED, 1.0); + +} else if (requestedIntakePower > 0) { + // Ejecting: fixed reverse speed + output = IntakeConstants.INTAKE_EJECT_SPEED; +} + +intakeMotor.set(output); +``` + +The key idea: `periodic()` runs every 20ms, so the motor output continuously tracks the robot's speed even though `setIntakePower()` might only be called once (as in auto's `runOnce`). + +### RobotContainer.java + +One-line change to wire the swerve speed into the intake: + +```java +// Before: +private final IntakeSubsystem m_intake = new IntakeSubsystem(); + +// After: +private final IntakeSubsystem m_intake = new IntakeSubsystem( + () -> m_drive.getChassisSpeeds().vxMetersPerSecond); +``` + +This works because `m_drive` is declared before `m_intake`, and the lambda is only called later at runtime. + +## What This Would Look Like With Real Numbers + +Assuming measured max surface speed ~8.1 m/s and `INTAKE_MIN_SPEED = 0.30`: + +Motor output is the result of `max(INTAKE_MIN_SPEED, (robotSpeed * INTAKE_SPEED_MULTIPLIER) / ROLLER_MAX_SURFACE_SPEED)`. Roller-to-robot ratio is roller surface speed / robot speed (the 2x target). + +| Scenario | Robot Speed | Motor Output | Roller Surface Speed | Roller:Robot Ratio | Notes | +|---|---|---|---|---|---| +| Stationary | 0 m/s | 30% | 2.4 m/s | n/a | Min speed applies | +| Slow drive | 1 m/s | 30% | 2.4 m/s | 2.4x | Min speed still higher than 2x formula | +| Auto | 2 m/s | 49% | 4.0 m/s | 2.0x | 2x formula takes over | +| Fast teleop | 3 m/s | 74% | 6.0 m/s | 2.0x | | +| Full speed | 5 m/s | 100% | 8.1 m/s | 1.62x | Mechanical limit | + +## Also Worth Noting + +There's a bug in `toggleIntake()` where the boolean assignments are flipped -- `intakeOn` gets set to `false` when turning on and `true` when turning off. Worth fixing regardless of whether we do velocity compensation. + +## Open Questions for the Team + +- Does the 2x ratio feel right as a starting point, or should we try something different? +- What should `INTAKE_MIN_SPEED` be? 0.30? Higher? +- Are there concerns about current draw from running the rollers harder at high speed? +- Does simplifying the stick to on/off/eject make sense, or do we want to keep variable speed control? diff --git a/src/main/deploy/pathplanner/paths/Neutral.path b/src/main/deploy/pathplanner/paths/Neutral.path new file mode 100644 index 0000000..6033ecd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.734, + "y": 7.416 + }, + "prevControl": null, + "nextControl": { + "x": 7.724072048611112, + "y": 6.433763961226853 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.734, + "y": 4.550436342592593 + }, + "prevControl": { + "x": 7.781087601273149, + "y": 5.190860749421297 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.96353558963717 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -89.64693045105892 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral2.path b/src/main/deploy/pathplanner/paths/Neutral2.path new file mode 100644 index 0000000..56205c1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.734, + "y": 4.550436342592593 + }, + "prevControl": null, + "nextControl": { + "x": 6.758923828125001, + "y": 4.543087962962963 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.389864583333334, + "y": 4.550436342592593 + }, + "prevControl": { + "x": 7.1843425202546305, + "y": 4.545974826388889 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.96353558963717 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -91.28065898863989 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral3.path b/src/main/deploy/pathplanner/paths/Neutral3.path new file mode 100644 index 0000000..f7820a6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.39, + "y": 4.550436342592593 + }, + "prevControl": null, + "nextControl": { + "x": 6.148155381944445, + "y": 5.488601345486111 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.622352575231482, + "y": 7.352006076388888 + }, + "prevControl": { + "x": 5.813935329861112, + "y": 6.452879340277778 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 89.85355743195251 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 88.65080525169873 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral4.path b/src/main/deploy/pathplanner/paths/Neutral4.path new file mode 100644 index 0000000..d2d7dc6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral4.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.622, + "y": 7.352 + }, + "prevControl": null, + "nextControl": { + "x": 4.729130787037037, + "y": 7.329042390046296 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4963744936342596, + "y": 7.246963614004629 + }, + "prevControl": { + "x": 4.508482566550927, + "y": 7.295712239583334 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 28.352937229598172 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..d83c0a8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.9840517216435192, + "y": 7.19913353587963 + }, + "prevControl": null, + "nextControl": { + "x": 1.9840517216435192, + "y": 5.727979004662265 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9840517216435192, + "y": 2.0811839554398146 + }, + "prevControl": { + "x": 1.9840517216435192, + "y": 0.9191430558661833 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Trench.path b/src/main/deploy/pathplanner/paths/Trench.path new file mode 100644 index 0000000..e40574e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Trench.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5615257523148154, + "y": 7.306603587962964 + }, + "prevControl": null, + "nextControl": { + "x": 4.336320529513889, + "y": 7.318282262731482 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.733961950231482, + "y": 7.306603587962964 + }, + "prevControl": { + "x": 6.733961950231482, + "y": 7.306603587962964 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.3798895366251577 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.6234506722048545 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Trench2.path b/src/main/deploy/pathplanner/paths/Trench2.path new file mode 100644 index 0000000..9b04eed --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Trench2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.733961950231482, + "y": 7.306603587962964 + }, + "prevControl": null, + "nextControl": { + "x": 7.699385199652777, + "y": 6.840703197337962 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.733961950231482, + "y": 7.306603587962964 + }, + "prevControl": { + "x": 7.721233506944445, + "y": 6.896668981481482 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.00827495365614 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.6234506722048545 + }, + "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 145ffd7..1de7b1b 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, + "robotWidth": 0.838, + "robotLength": 0.838, "holonomicMode": true, "pathFolders": [ "Offset Depot" @@ -11,23 +11,23 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, - "robotMOI": 6.883, + "robotMass": 49.895, + "robotMOI": 3.9111, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, + "driveWheelRadius": 0.0508, + "driveGearing": 6.75, "maxDriveSpeed": 5.45, - "driveMotorType": "krakenX60", - "driveCurrentLimit": 60.0, + "driveMotorType": "vortex", + "driveCurrentLimit": 40.0, "wheelCOF": 1.2, - "flModuleX": 0.273, - "flModuleY": 0.273, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, + "flModuleX": 0.276, + "flModuleY": 0.276, + "frModuleX": 0.276, + "frModuleY": -0.276, + "blModuleX": -0.276, + "blModuleY": 0.276, + "brModuleX": -0.276, + "brModuleY": -0.276, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/Auto/CenterLemonAuto.java b/src/main/java/frc/robot/Auto/CenterLemonAuto.java index f9b2712..6fc41fd 100644 --- a/src/main/java/frc/robot/Auto/CenterLemonAuto.java +++ b/src/main/java/frc/robot/Auto/CenterLemonAuto.java @@ -4,12 +4,10 @@ package frc.robot.Auto; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.ShooterConstants; -import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; @@ -42,6 +40,7 @@ public CenterLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSub shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ) ); diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java new file mode 100644 index 0000000..7724d4e --- /dev/null +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -0,0 +1,199 @@ +// 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.Auto; + +import java.util.concurrent.atomic.AtomicReference; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class CenterToDepotAuto extends SequentialCommandGroup { + private static final double DRIVE_SPEED_MPS = 2.0; + private static final double DRIVE_SPEED_MPS_2 = 1.0; + private static final double DRIVE_HEADING_P = 3.0; + private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; + private static final double TURN_P = 4.0; + private static final double TURN_TOLERANCE_DEG = 3.0; + private static final double TURN_TIMEOUT_SEC = 2.5; + + private static final double BACKWARD_METERS_1 = 1.7; + private static final double FORWARD_METERS = 1.8; + private static final double FORWARD_METERS_1 = 1.8; + private static final double FORWARD_METERS_2 = 1.9; + private static final double FORWARD_METERS_3 = 1.0; + + + private static final double INTAKE_POWER = -1.0; + + public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); + + addCommands( + // Drive forwards 2m. + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees right. + turnRelativeDegrees(drive, -90.0), + + Commands.runOnce(intake::lowerIntake, intake), + + // Drive forward 1.9m + driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 90 degrees left (intake on). + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + turnRelativeDegrees(drive, 90.0), + + // Drive forward 1.8m (intake still on). + driveDistanceMeters(drive, FORWARD_METERS, DRIVE_SPEED_MPS_2), + + // Drive backward 1.7m + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 110 degrees right (intake still on). + turnRelativeDegrees(drive, -123), + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.MED), shooter), + + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(1.0), + + + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + }, shooter) + ), + + Commands.waitSeconds(2), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() + ) + ); + } + + private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( + SwerveSubsystem drive, + double distanceMeters, + double speedMps) { + double clampedSpeedMps = MathUtil.clamp(Math.abs(speedMps), 0.0, SwerveConstants.maxSpeed); + double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); + double distanceAbsMeters = Math.abs(distanceMeters); + + AtomicReference startPositions = new AtomicReference<>(); + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> { + startPositions.set(drive.getPositions()); + startYawRad[0] = drive.getYaw().getRadians(); + }, drive), + Commands.runEnd( + () -> { + double errorRad = MathUtil.angleModulus(startYawRad[0] - drive.getYaw().getRadians()); + double maxOmegaRadPerSec = + Math.min(DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC, SwerveConstants.maxAngularVelocity); + double omegaRadPerSec = + MathUtil.clamp(errorRad * DRIVE_HEADING_P, -maxOmegaRadPerSec, maxOmegaRadPerSec); + drive.drive(commandedSpeedMps, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until( + () -> getAverageWheelDeltaMeters(startPositions.get(), drive.getPositions()) >= distanceAbsMeters) + .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) + ); + } + + private static double getAverageWheelDeltaMeters( + SwerveModulePosition[] startPositions, + SwerveModulePosition[] currentPositions) { + if (startPositions == null || currentPositions == null) { + return 0.0; + } + + int count = Math.min(startPositions.length, currentPositions.length); + if (count <= 0) { + return 0.0; + } + + double sum = 0.0; + for (int i = 0; i < count; i++) { + sum += Math.abs(currentPositions[i].distanceMeters - startPositions[i].distanceMeters); + } + return sum / count; + } + + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( + SwerveSubsystem drive, + double deltaDegrees) { + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.runEnd( + () -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadPerSec = + MathUtil.clamp( + errorRad * TURN_P, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); + }) + .withTimeout(TURN_TIMEOUT_SEC) + + ); + } +} diff --git a/src/main/java/frc/robot/Auto/DepotShootingAuto.java b/src/main/java/frc/robot/Auto/DepotShootingAuto.java new file mode 100644 index 0000000..83205cb --- /dev/null +++ b/src/main/java/frc/robot/Auto/DepotShootingAuto.java @@ -0,0 +1,197 @@ +// 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.Auto; + +import java.util.concurrent.atomic.AtomicReference; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class DepotShootingAuto extends SequentialCommandGroup { + private static final double DRIVE_SPEED_MPS = 3.0; + private static final double DRIVE_SPEED_MPS_2 = 1.0; + private static final double DRIVE_HEADING_P = 3.0; + private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; + private static final double TURN_P = 4.0; + private static final double TURN_TOLERANCE_DEG = 3.0; + private static final double TURN_TIMEOUT_SEC = 2.5; + + private static final double BACKWARD_METERS_1 = 1.0; + private static final double FORWARD_METERS = 1.8; + private static final double FORWARD_METERS_1 = 2.0; + private static final double FORWARD_METERS_2 = 1.9; + + private static final double INTAKE_POWER = -1.0; + + public DepotShootingAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); + + addCommands( + Commands.runOnce(intake::lowerIntake, intake), + + // Drive forwards 2m. + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees right. + turnRelativeDegrees(drive, -90.0), + + // Drive forward 1.9m + driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 90 degrees left (intake on). + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + turnRelativeDegrees(drive, 90.0), + + // Drive forward 1.8m (intake still on). + driveDistanceMeters(drive, FORWARD_METERS, DRIVE_SPEED_MPS_2), + + // Drive backward 1m + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS_2), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 110 degrees right (intake still on). + turnRelativeDegrees(drive, -129), + + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(1.0), + + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + }, shooter) + ), + + Commands.waitSeconds(2), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() + ) + ); + } + + private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( + SwerveSubsystem drive, + double distanceMeters, + double speedMps) { + double clampedSpeedMps = MathUtil.clamp(Math.abs(speedMps), 0.0, SwerveConstants.maxSpeed); + double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); + double distanceAbsMeters = Math.abs(distanceMeters); + + AtomicReference startPositions = new AtomicReference<>(); + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> { + startPositions.set(drive.getPositions()); + startYawRad[0] = drive.getYaw().getRadians(); + }, drive), + Commands.runEnd( + () -> { + double errorRad = MathUtil.angleModulus(startYawRad[0] - drive.getYaw().getRadians()); + double maxOmegaRadPerSec = + Math.min(DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC, SwerveConstants.maxAngularVelocity); + double omegaRadPerSec = + MathUtil.clamp(errorRad * DRIVE_HEADING_P, -maxOmegaRadPerSec, maxOmegaRadPerSec); + drive.drive(commandedSpeedMps, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until( + () -> getAverageWheelDeltaMeters(startPositions.get(), drive.getPositions()) >= distanceAbsMeters) + .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) + ); + } + + private static double getAverageWheelDeltaMeters( + SwerveModulePosition[] startPositions, + SwerveModulePosition[] currentPositions) { + if (startPositions == null || currentPositions == null) { + return 0.0; + } + + int count = Math.min(startPositions.length, currentPositions.length); + if (count <= 0) { + return 0.0; + } + + double sum = 0.0; + for (int i = 0; i < count; i++) { + sum += Math.abs(currentPositions[i].distanceMeters - startPositions[i].distanceMeters); + } + return sum / count; + } + + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( + SwerveSubsystem drive, + double deltaDegrees) { + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.runEnd( + () -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadPerSec = + MathUtil.clamp( + errorRad * TURN_P, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); + }) + .withTimeout(TURN_TIMEOUT_SEC) + + ); + } +} diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/Auto/DriveTestAuto.java deleted file mode 100644 index f563d08..0000000 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ /dev/null @@ -1,33 +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.Auto; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Subsystems.SwerveSubsystem; - -/* -public class DriveTestAuto extends SequentialCommandGroup { - public DriveTestAuto (SwerveSubsystem drive) { - addCommands( - new InstantCommand(() -> drive.drive(0.5,0,0, false), drive), - Commands.waitSeconds(2), - new InstantCommand(() -> drive.drive(0,0,0, false), drive) - ); - } -} -*/ - - -public class DriveTestAuto extends SequentialCommandGroup { - public DriveTestAuto (SwerveSubsystem drive){ - addCommands( - drive.startAutoAt(1.165, 6.000, 0.000), - drive.autoDrive("DriveTestPath") - ); - } -} - diff --git a/src/main/java/frc/robot/Auto/EightLemonAuto.java b/src/main/java/frc/robot/Auto/EightLemonAuto.java deleted file mode 100644 index 5f99ce3..0000000 --- a/src/main/java/frc/robot/Auto/EightLemonAuto.java +++ /dev/null @@ -1,20 +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.Auto; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Subsystems.ShooterSubsystem; -import frc.robot.Subsystems.IntakeSubsystem; -import frc.robot.Subsystems.SwerveSubsystem; - - -public class EightLemonAuto extends SequentialCommandGroup { - public EightLemonAuto (SwerveSubsystem drive, ShooterSubsystem shooter, IntakeSubsystem intake){ - addCommands( - drive.startAutoAt(3.53, 7.13, -130.45), - drive.autoDrive("8FuelPath") - ); - } -} diff --git a/src/main/java/frc/robot/Auto/LeftLemonAuto.java b/src/main/java/frc/robot/Auto/LeftLemonAuto.java index a23d4e4..e7580a1 100644 --- a/src/main/java/frc/robot/Auto/LeftLemonAuto.java +++ b/src/main/java/frc/robot/Auto/LeftLemonAuto.java @@ -54,6 +54,7 @@ public LeftLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsy shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ) ); diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java new file mode 100644 index 0000000..2248606 --- /dev/null +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -0,0 +1,199 @@ +// 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.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; + +import java.util.concurrent.atomic.AtomicReference; + + +public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { + private static final double DRIVE_SPEED_MPS = 2.0; + private static final double DRIVE_HEADING_P = 3.0; + private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; + private static final double TURN_P = 4.0; + private static final double TURN_TOLERANCE_DEG = 3.0; + private static final double TURN_TIMEOUT_SEC = 2.5; + + private static final double BACKWARD_METERS_1 = 3.6; + private static final double BACKWARD_METERS_2 = 4.5; + private static final double BACKWARD_METERS_3 = 2.6; + + private static final double FORWARD_METERS_1 = 2.5; + //private static final double FORWARD_METERS_2 = 1.0; + //private static final double FORWARD_METERS_3 = 3.2; + + private static final double INTAKE_POWER = -0.75; + + public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); + + addCommands( + Commands.runOnce(intake::lowerIntake, intake), + + // Drive backwards 3.6m. + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees left. + turnRelativeDegrees(drive, 90.0), + + // Drive forward 3m while starting intake (intake stays on for the rest of auto). + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Drive forward 3m (intake still on). + driveDistanceMeters(drive, -BACKWARD_METERS_3, DRIVE_SPEED_MPS), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees left + turnRelativeDegrees(drive, 90.0), + + // Drive backward (back to the trench) + driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 20 degrees left + turnRelativeDegrees(drive, 20.0), + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(1.0), + + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + }, shooter) + ), + + Commands.waitSeconds(1), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() + ) + ); + } + + private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( + SwerveSubsystem drive, + double distanceMeters, + double speedMps) { + double clampedSpeedMps = MathUtil.clamp(Math.abs(speedMps), 0.0, SwerveConstants.maxSpeed); + double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); + double distanceAbsMeters = Math.abs(distanceMeters); + + AtomicReference startPositions = new AtomicReference<>(); + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> { + startPositions.set(drive.getPositions()); + startYawRad[0] = drive.getYaw().getRadians(); + }, drive), + Commands.runEnd( + () -> { + double errorRad = MathUtil.angleModulus(startYawRad[0] - drive.getYaw().getRadians()); + double maxOmegaRadPerSec = + Math.min(DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC, SwerveConstants.maxAngularVelocity); + double omegaRadPerSec = + MathUtil.clamp(errorRad * DRIVE_HEADING_P, -maxOmegaRadPerSec, maxOmegaRadPerSec); + drive.drive(commandedSpeedMps, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until( + () -> getAverageWheelDeltaMeters(startPositions.get(), drive.getPositions()) >= distanceAbsMeters) + .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) + ); + } + + private static double getAverageWheelDeltaMeters( + SwerveModulePosition[] startPositions, + SwerveModulePosition[] currentPositions) { + if (startPositions == null || currentPositions == null) { + return 0.0; + } + + int count = Math.min(startPositions.length, currentPositions.length); + if (count <= 0) { + return 0.0; + } + + double sum = 0.0; + for (int i = 0; i < count; i++) { + sum += Math.abs(currentPositions[i].distanceMeters - startPositions[i].distanceMeters); + } + return sum / count; + } + + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( + SwerveSubsystem drive, + double deltaDegrees) { + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.runEnd( + () -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadPerSec = + MathUtil.clamp( + errorRad * TURN_P, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); + }) + .withTimeout(TURN_TIMEOUT_SEC) + + ); + } +} diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java new file mode 100644 index 0000000..eed4d48 --- /dev/null +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java @@ -0,0 +1,268 @@ +// 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.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; + +import java.util.concurrent.atomic.AtomicReference; + + +public class LeftNeutralZoneAuto2 extends SequentialCommandGroup { + private static final double DRIVE_SPEED_MPS = 2.0; + private static final double DRIVE_HEADING_P = 3.0; + private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; + private static final double TURN_P = 4.0; + private static final double TURN_TOLERANCE_DEG = 3.0; + private static final double TURN_TIMEOUT_SEC = 2.5; + + private static final double BACKWARD_METERS_1 = 3.6; + private static final double BACKWARD_METERS_2 = 4.3; + private static final double FORWARD_METERS_1 = 3.0; + //private static final double FORWARD_METERS_2 = 1.0; + //private static final double FORWARD_METERS_3 = 3.2; + + private static final double INTAKE_POWER = -0.75; + private static final double FEED_DURATION_SEC = 1.5; + + public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); + + addCommands( + Commands.runOnce(intake::lowerIntake, intake), + + // Drive backwards 3.6m. + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees left. + turnRelativeDegrees(drive, 90.0), + + // Drive forward 3m while starting intake (intake stays on for the rest of auto). + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Drive backwards 3m (intake still on). + driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees left + turnRelativeDegrees(drive, 90.0), + + // Drive backward (back to the trench) + driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 20 degrees left + turnRelativeDegrees(drive, 20.0), + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(1.0), + + + /*Run the intake, kicker, indexer, hopper, and intake arm together for a fixed time, + then stop the feeding mechanisms so another driving path can be added.*/ + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + }, shooter) + ), + + Commands.waitSeconds(1), + + // While feeding, continuously move the intake arm up/down for the fixed feed time. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() + ).withTimeout(FEED_DURATION_SEC), + + + + + //Go to the neutral zone a second time. + // Turn 20 degrees right + turnRelativeDegrees(drive, -20.0), + + // Drive FORWARD (back to the trench) + driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 90 degrees right + turnRelativeDegrees(drive, -90.0), + + // Drive forwards 3m (intake still on). + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + // Drive backwards 3m (intake still on). + driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees left + turnRelativeDegrees(drive, 90.0), + + // Drive backward (back to the trench) + driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 20 degrees left + turnRelativeDegrees(drive, 20.0), + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + }, shooter) + ), + + Commands.waitSeconds(1), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() + + ) + ); + } + + private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( + SwerveSubsystem drive, + double distanceMeters, + double speedMps) { + double clampedSpeedMps = MathUtil.clamp(Math.abs(speedMps), 0.0, SwerveConstants.maxSpeed); + double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); + double distanceAbsMeters = Math.abs(distanceMeters); + + AtomicReference startPositions = new AtomicReference<>(); + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> { + startPositions.set(drive.getPositions()); + startYawRad[0] = drive.getYaw().getRadians(); + }, drive), + Commands.runEnd( + () -> { + double errorRad = MathUtil.angleModulus(startYawRad[0] - drive.getYaw().getRadians()); + double maxOmegaRadPerSec = + Math.min(DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC, SwerveConstants.maxAngularVelocity); + double omegaRadPerSec = + MathUtil.clamp(errorRad * DRIVE_HEADING_P, -maxOmegaRadPerSec, maxOmegaRadPerSec); + drive.drive(commandedSpeedMps, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until( + () -> getAverageWheelDeltaMeters(startPositions.get(), drive.getPositions()) >= distanceAbsMeters) + .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) + ); + } + + private static double getAverageWheelDeltaMeters( + SwerveModulePosition[] startPositions, + SwerveModulePosition[] currentPositions) { + if (startPositions == null || currentPositions == null) { + return 0.0; + } + + int count = Math.min(startPositions.length, currentPositions.length); + if (count <= 0) { + return 0.0; + } + + double sum = 0.0; + for (int i = 0; i < count; i++) { + sum += Math.abs(currentPositions[i].distanceMeters - startPositions[i].distanceMeters); + } + return sum / count; + } + + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( + SwerveSubsystem drive, + double deltaDegrees) { + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.runEnd( + () -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadPerSec = + MathUtil.clamp( + errorRad * TURN_P, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); + }) + .withTimeout(TURN_TIMEOUT_SEC) + + ); + } +} diff --git a/src/main/java/frc/robot/Auto/RightLemonAuto.java b/src/main/java/frc/robot/Auto/RightLemonAuto.java index d361b89..ff8aa29 100644 --- a/src/main/java/frc/robot/Auto/RightLemonAuto.java +++ b/src/main/java/frc/robot/Auto/RightLemonAuto.java @@ -54,6 +54,7 @@ public RightLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubs shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ) ); diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java new file mode 100644 index 0000000..d574404 --- /dev/null +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -0,0 +1,194 @@ +// 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.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; + +import java.util.concurrent.atomic.AtomicReference; + + +public class RightNeutralZoneAuto1 extends SequentialCommandGroup { + private static final double DRIVE_SPEED_MPS = 3.0; + private static final double DRIVE_HEADING_P = 3.0; + private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; + private static final double TURN_P = 4.0; + private static final double TURN_TOLERANCE_DEG = 3.0; + private static final double TURN_TIMEOUT_SEC = 1.0; + + private static final double BACKWARD_METERS_1 = 3.6; + private static final double BACKWARD_METERS_2 = 4.3; + private static final double FORWARD_METERS_1 = 3.0; + + private static final double INTAKE_POWER = -0.75; + + public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); + + addCommands( + Commands.runOnce(intake::lowerIntake, intake), + + // Drive backwards 3.6m. + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees right. + turnRelativeDegrees(drive, -90.0), + + // Drive forward 2m while starting intake (intake stays on for the rest of auto). + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Drive back 2m (intake still on). + driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees left (intake still on). + turnRelativeDegrees(drive, 90.0), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Drive forward (back to the trench) + driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 13 degrees right & bring shooter hood to HIGH + Commands.parallel( + turnRelativeDegrees(drive,-13.0), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter) + ), + + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM), + + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + }, shooter) + ), + + Commands.waitSeconds(2), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() + ) + ); + } + + private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( + SwerveSubsystem drive, + double distanceMeters, + double speedMps) { + double clampedSpeedMps = MathUtil.clamp(Math.abs(speedMps), 0.0, SwerveConstants.maxSpeed); + double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); + double distanceAbsMeters = Math.abs(distanceMeters); + + AtomicReference startPositions = new AtomicReference<>(); + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> { + startPositions.set(drive.getPositions()); + startYawRad[0] = drive.getYaw().getRadians(); + }, drive), + Commands.runEnd( + () -> { + double errorRad = MathUtil.angleModulus(startYawRad[0] - drive.getYaw().getRadians()); + double maxOmegaRadPerSec = + Math.min(DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC, SwerveConstants.maxAngularVelocity); + double omegaRadPerSec = + MathUtil.clamp(errorRad * DRIVE_HEADING_P, -maxOmegaRadPerSec, maxOmegaRadPerSec); + drive.drive(commandedSpeedMps, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until( + () -> getAverageWheelDeltaMeters(startPositions.get(), drive.getPositions()) >= distanceAbsMeters) + .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) + ); + } + + private static double getAverageWheelDeltaMeters( + SwerveModulePosition[] startPositions, + SwerveModulePosition[] currentPositions) { + if (startPositions == null || currentPositions == null) { + return 0.0; + } + + int count = Math.min(startPositions.length, currentPositions.length); + if (count <= 0) { + return 0.0; + } + + double sum = 0.0; + for (int i = 0; i < count; i++) { + sum += Math.abs(currentPositions[i].distanceMeters - startPositions[i].distanceMeters); + } + return sum / count; + } + + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( + SwerveSubsystem drive, + double deltaDegrees) { + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.runEnd( + () -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadPerSec = + MathUtil.clamp( + errorRad * TURN_P, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); + }) + .withTimeout(TURN_TIMEOUT_SEC) + + ); + } +} diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java new file mode 100644 index 0000000..69a04e7 --- /dev/null +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java @@ -0,0 +1,239 @@ +// 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.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; + +import java.util.concurrent.atomic.AtomicReference; + + +public class RightNeutralZoneAuto2 extends SequentialCommandGroup { + private static final double DRIVE_SPEED_MPS = 2.5; + private static final double DRIVE_HEADING_P = 3.0; + private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; + private static final double TURN_P = 4.0; + private static final double TURN_TOLERANCE_DEG = 3.0; + private static final double TURN_TIMEOUT_SEC = 1.0; + + private static final double BACKWARD_METERS_1 = 3.6; + private static final double BACKWARD_METERS_2 = 4.3; + private static final double BACKWARD_METERS_3 = 3.1; + private static final double FORWARD_METERS_1 = 3.0; + private static final double FORWARD_METERS_2 = 4.0; + + private static final double INTAKE_POWER = -0.75; + private static final double FEED_DURATION_SEC = 10.0; + + public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); + + addCommands( + //Commands.waitSeconds(5.0), //GINGER WAS HERE + + Commands.runOnce(intake::lowerIntake, intake), + + // Drive backwards 3.6m. + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees right. + turnRelativeDegrees(drive, -90.0), + + // Drive forward 3m while starting intake (intake stays on for the rest of auto). + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Drive back 3.2m (intake still on). + driveDistanceMeters(drive, -BACKWARD_METERS_3, DRIVE_SPEED_MPS), + + // Turn 90 degrees left (intake still on). + turnRelativeDegrees(drive, 90.0), + + // Drive forward (back to the trench) + driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 13 degrees right & bring shooter hood to HIGH + Commands.parallel( + turnRelativeDegrees(drive,-13.0), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter) + ), + + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(1.0), + + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + }, shooter) + ), + + Commands.waitSeconds(2), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() + ).withTimeout(FEED_DURATION_SEC), + + Commands.parallel( + Commands.runOnce(() -> { + intake.lowerIntake(); + intake.setIntakePower(0.0); + }, intake), + Commands.runOnce(() -> { + shooter.setKickerSpeed(0.0); + shooter.setIndexerSpeed(0.0); + shooter.setShooterSpeed(0.0); + shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW); + }, shooter) + ), + + //Turn 13 degrees left + turnRelativeDegrees(drive, 13), + + driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + turnRelativeDegrees(drive,-90), + + Commands.runOnce(()-> intake.setIntakePower(INTAKE_POWER), intake), + driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), + + driveDistanceMeters(drive, -FORWARD_METERS_2, DRIVE_SPEED_MPS), + + turnRelativeDegrees(drive, 90.0), + + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + + // Turn 13 degrees right & bring shooter hood to HIGH + Commands.parallel( + turnRelativeDegrees(drive,-13.0), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter) + ) + ); + } + + private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( + SwerveSubsystem drive, + double distanceMeters, + double speedMps) { + double clampedSpeedMps = MathUtil.clamp(Math.abs(speedMps), 0.0, SwerveConstants.maxSpeed); + double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); + double distanceAbsMeters = Math.abs(distanceMeters); + + AtomicReference startPositions = new AtomicReference<>(); + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> { + startPositions.set(drive.getPositions()); + startYawRad[0] = drive.getYaw().getRadians(); + }, drive), + Commands.runEnd( + () -> { + double errorRad = MathUtil.angleModulus(startYawRad[0] - drive.getYaw().getRadians()); + double maxOmegaRadPerSec = + Math.min(DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC, SwerveConstants.maxAngularVelocity); + double omegaRadPerSec = + MathUtil.clamp(errorRad * DRIVE_HEADING_P, -maxOmegaRadPerSec, maxOmegaRadPerSec); + drive.drive(commandedSpeedMps, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until( + () -> getAverageWheelDeltaMeters(startPositions.get(), drive.getPositions()) >= distanceAbsMeters) + .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) + ); + } + + private static double getAverageWheelDeltaMeters( + SwerveModulePosition[] startPositions, + SwerveModulePosition[] currentPositions) { + if (startPositions == null || currentPositions == null) { + return 0.0; + } + + int count = Math.min(startPositions.length, currentPositions.length); + if (count <= 0) { + return 0.0; + } + + double sum = 0.0; + for (int i = 0; i < count; i++) { + sum += Math.abs(currentPositions[i].distanceMeters - startPositions[i].distanceMeters); + } + return sum / count; + } + + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( + SwerveSubsystem drive, + double deltaDegrees) { + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.runEnd( + () -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadPerSec = + MathUtil.clamp( + errorRad * TURN_P, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); + }) + .withTimeout(TURN_TIMEOUT_SEC) + + ); + } +} diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java new file mode 100644 index 0000000..f5b6108 --- /dev/null +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -0,0 +1,197 @@ +package frc.robot.Command; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; + +public class SimpleAutoAlign extends Command { + + private final SwerveSubsystem swerveSubsystem; + + //Target for how far away the robot should be from the hub + private static final double TARGET_DISTANCE_METERS = 2.1; + + //Camera geometry + public static final double CAMERA_HEIGHT_METERS = 0.5; + public static final double APRIL_TAG_HEIGHT_METERS = 1.0; + public static final double CAMERA_TILT_DEG = 0.0; + + //Distance PID tuning + public static final double DISTANCE_KP = 2.5; + public static final double DISTANCE_KI = 0; + public static final double DISTANCE_KD = 0; + + //Rotation PID tuning + public static final double ROTATION_KP = 0.02; + public static final double ROTATION_KI = 0; + public static final double ROTATION_KD = 0; + + //Tolerance and things + public static final double DISTANCE_TOLERANCE_METERS = 0.08; + public static final double ROTATION_TOLERANCE_DEG = 1.5; + public static final double MAX_FORWARD_SPEED_MPS = 0; + public static final double MAX_ROTATION_SPEED_RAD_PER_SEC = 2.5; + public static final double MIN_DISTANCE_CALC_ANGLE_DEG = 1.0; + public static final int SETTLE_CYCLES_REQUIRED = 10; + public static final double UNLOCK_DISTANCE_ERROR_METERS = 0.15; + public static final double UNLOCK_ROTATION_ERROR_DEG = 3.0; + public static final double DISTANCE_FILTER_ALPHA = 0.25; + public static final double MAX_DISTANCE_ACCEL_MPS_PER_SEC = SwerveConstants.maxSpeed * 4.0; + + //PID Controller and alignment + private final PIDController distanceController = new PIDController(DISTANCE_KP, DISTANCE_KI, DISTANCE_KD); + private final PIDController rotationController = new PIDController(ROTATION_KP, ROTATION_KI, ROTATION_KD); + private int settledCycles = 0; + private boolean alignmentLocked = false; + private double filteredDistanceMeters = Double.NaN; + private final SlewRateLimiter distanceSpeedLimiter = new SlewRateLimiter(MAX_DISTANCE_ACCEL_MPS_PER_SEC); + + + + public SimpleAutoAlign(SwerveSubsystem swerveSubsystem){ + this.swerveSubsystem = swerveSubsystem; + addRequirements(swerveSubsystem); + } + + private boolean canSeeTag() { //checks if limelight can see tag + double tv = NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tv").getDouble(0.0); + return tv > 0; + } + + private int getTagId() { //gets the tag id from network tables + return (int) NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tid").getDouble(0.0); + } + + private boolean isSupportedTag(int tagId) { //checks if we want to align to that tag + return tagId == 10 || tagId == 11 ||tagId == 26 || tagId == 27 || tagId == 8 || tagId == 24; + } + + private double getDesiredAlignmentAngle(int tagId) { //for each tag decides what angle the robot should be at in comparison to the tag + if (tagId == 11 || tagId == 27) { + return 20.0; + } + if (tagId == 8 || tagId == 24) { + return -20.0; + } + return 0.0; + } + + private double getVerticalOffsetToTarget(){ + return NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("ty").getDouble(0.0); + } + private double getOffsetToTarget() { + return NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tx").getDouble(0.0); + } + + private double getDistanceToTargetMeters(double tyDegrees){ //uses trig to find distance + double angleToTargetDegrees = CAMERA_TILT_DEG + tyDegrees; + if (Math.abs(angleToTargetDegrees)= SETTLE_CYCLES_REQUIRED; + if (alignmentLocked){ + distanceSpeedLimiter.reset(0.0); + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0, 0, 0), false); + return; + } + double driveSpeed = 0.0; + + /*if (distanceIsValid && Math.abs (rotationError) <= UNLOCK_ROTATION_ERROR_DEG &&! withinDistanceTolerance){ + driveSpeed = distanceController.calculate(0.0, distanceError); + } + */ + + //driveSpeed = MathUtil.clamp(driveSpeed, -MAX_FORWARD_SPEED_MPS, MAX_FORWARD_SPEED_MPS); + //driveSpeed = distanceSpeedLimiter.calculate(driveSpeed); + rotationSpeed = MathUtil.clamp(rotationSpeed, -MAX_ROTATION_SPEED_RAD_PER_SEC, MAX_ROTATION_SPEED_RAD_PER_SEC); + + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0, driveSpeed, rotationSpeed), false); + + } + + @Override + public void end(boolean interrupted) { + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0,0,0), false); + } + + @Override + public boolean isFinished() { + return false; + } + + + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 416a1ed..d2add26 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -97,6 +97,7 @@ public static final class SwerveConstants{ /* Swerve Profiling Values */ public static final double maxSpeed = 5; // meters per second + public static final double PathPlannerMaxSpeed = DCMotor.getNeoVortex(1).withReduction(driveGearRatio).freeSpeedRadPerSec*(wheelDiameter/2); public static final double maxAngularVelocity = maxSpeed/driveBaseRadius; //radians per second how fast the robot spin /* Neutral Modes */ @@ -126,13 +127,10 @@ public record ModuleData( ){} public static ModuleData[] moduleData = { - new ModuleData(6, 5, 7, 31.46, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left - // Module 1 is currently the only module oscillating; flip its angle motor invert so its - // steering closed-loop sign matches the encoder direction. - // Module 1: also invert drive so +X command drives forward like the others. - new ModuleData(9, 8, 10, 49.57, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right - new ModuleData(12, 11, 13, 33.13, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right - new ModuleData(15, 14, 16, 8.52, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left + new ModuleData(6, 5, 7, 34.18, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left + new ModuleData(9, 31, 10, 44.03, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right + new ModuleData(12, 11, 13, 28.21, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right + new ModuleData(15, 14, 16, 8.87, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left }; } @@ -142,7 +140,7 @@ public static final class AutoConstants { private static boolean dashboardInitialized = false; public static final ModuleConfig MODULE_CONFIG = new ModuleConfig(SwerveConstants.wheelDiameter/2, - SwerveConstants.maxSpeed, + SwerveConstants.PathPlannerMaxSpeed, 1.2, DCMotor.getNeoVortex(1).withReduction(SwerveConstants.driveGearRatio), SwerveConstants.driveContinuousCurrentLimit, @@ -151,15 +149,20 @@ public static final class AutoConstants { public static final RobotConfig ROBOT_CONFIG = new RobotConfig (52, 6.8, MODULE_CONFIG, SwerveConstants.FRONT_LEFT, SwerveConstants.FRONT_RIGHT, SwerveConstants.BACK_LEFT, SwerveConstants.BACK_RIGHT); - public static final PPHolonomicDriveController SWERV_DRIVE_CONTROLLER = new PPHolonomicDriveController(new PIDConstants(5.0,0.00001,0.0), + public static final PPHolonomicDriveController SWERVE_DRIVE_CONTROLLER = new PPHolonomicDriveController(new PIDConstants(5.0,0.00001,0.0), new PIDConstants(5.0, 0.005, 0.001) ); public enum AutoMode{ None, LeftLemonAuto, RightLemonAuto, - ShootEightAuto, - CenterLemonAuto + LeftNeutralZoneAuto1, + LeftNeutralZoneAuto2, + RightNeutralZoneAuto1, + RightNeutralZoneAuto2, + CenterLemonAuto, + CenterToDepotAuto, + DepotShootingAuto } private static SendableChooser sideChooser = new SendableChooser(); @@ -175,10 +178,15 @@ public static void initDashboard() { autoModeChooser.setDefaultOption("LeftLemonAuto", AutoMode.LeftLemonAuto); autoModeChooser.addOption("None", AutoMode.None); - autoModeChooser.addOption("ShootEightAuto", AutoMode.ShootEightAuto); autoModeChooser.addOption("RightLemonAuto", AutoMode.RightLemonAuto); autoModeChooser.addOption("LeftLemonAuto", AutoMode.LeftLemonAuto); + autoModeChooser.addOption("RightNeutralZoneAuto1", AutoMode.RightNeutralZoneAuto1); + autoModeChooser.addOption("RightNeutralZoneAuto2", AutoMode.RightNeutralZoneAuto2); + autoModeChooser.addOption("LeftNeutralZoneAuto1", AutoMode.LeftNeutralZoneAuto1); + autoModeChooser.addOption("LeftNeutralZoneAuto2", AutoMode.LeftNeutralZoneAuto2); autoModeChooser.addOption("CenterLemonAuto", AutoMode.CenterLemonAuto); + autoModeChooser.addOption("CenterToDepotAuto", AutoMode.CenterToDepotAuto); + autoModeChooser.addOption("DepotShootingAuto", AutoMode.DepotShootingAuto); SmartDashboard.putData("Auto Starting Location", sideChooser); SmartDashboard.putData("Auto Mode", autoModeChooser); @@ -250,8 +258,9 @@ public static final class VisionConstants { public static final double MAX_VISION_ANGULAR_RATE_DEG_PER_SEC = 720.0; /** Standard deviations for vision measurements: (x meters, y meters, theta radians). */ - public static final double VISION_STD_DEV_X_METERS = 0.7; - public static final double VISION_STD_DEV_Y_METERS = 0.7; + //Rotation (Radians) should be handled by pigion so deviation is high + public static final double VISION_STD_DEV_X_METERS = 0.4; + public static final double VISION_STD_DEV_Y_METERS = 0.4; public static final double VISION_STD_DEV_THETA_RADIANS = 99999.0; public static String getLimelightStreamUrl(String limelightName) { @@ -273,11 +282,13 @@ public static final class ShooterConstants { public static final int KICKER_ID = 21; public static final int HOOD_ID = 20; public static final int INDEXER_ID = 23; + public static final int HOPPER_ID = 43; //placeholder // Percent output caps ([-1..1]). Higher = faster spin-up but more current draw. public static final double SHOOTER_SPEED = 0.6; public static final double KICKER_SPEED = 0.6; public static final double INDEXER_SPEED = 0.4; //placeholder + public static final double HOPPER_SPEED = 0.5; //placeholder // Shooter readiness (SparkMax encoder velocity is RPM). Tune on the real robot. public static final double SHOOTER_READY_RPM = 3000.0; @@ -291,7 +302,7 @@ public static final class ShooterConstants { // Max travel is 3 rotations = 1080 degrees. public static final double HOOD_MIN_ROTATIONS = 0.0; public static final double HOOD_MED_ROTATIONS = 20.0; - public static final double HOOD_MAX_ROTATIONS = 36.0; + public static final double HOOD_MAX_ROTATIONS = 27.0; // Preset positions. public static final double HOOD_ANGLE_LOW = HOOD_MIN_ROTATIONS; @@ -306,13 +317,15 @@ public static final class IntakeConstants { // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. public static int INTAKE_ID = 19; - public static double INTAKE_SPEED = 90; //percent output scaling for intake motor + // SparkMax.set(...) expects [-1.0, 1.0] percent output. + public static double INTAKE_SPEED = 0.80; // max percent output for intake motor public static int INTAKE_ARM_ID = 18; + public static int INTAKE_ARM_2_ID = 24; public static int GEAR_RATIO = 25; //Intake arm position units are degrees - public static final double INTAKE_ARM_MIN_DEG = 20.0; + public static final double INTAKE_ARM_MIN_DEG = 25.0; public static final double INTAKE_ARM_MAX_DEG = 90.0; //Preset positions @@ -320,15 +333,23 @@ public static final class IntakeConstants { public static final double INTAKE_ARM_RAISED_POSITION = INTAKE_ARM_MAX_DEG; //PID constants for intake arm (degrees). - public static final double INTAKE_ARM_kP = 6.0; - public static final double INTAKE_ARM_kI = 1.5; - public static final double INTAKE_ARM_kD = 0.15; + public static final double INTAKE_ARM_kP = 9.90; + public static final double INTAKE_ARM_kI = 1.60; + public static final double INTAKE_ARM_kD = 0.16; public static final double INTAKE_ARM_TOLERANCE_DEG = 2.0; + //Feedforward constants for intake arm + public static final double INTAKE_ARM_kS = 0.0; + public static final double INTAKE_ARM_kG = 0.0; + public static final double INTAKE_ARM_kV = 0.0; + public static final double INTAKE_ARM_kA = 0.0; + //Percent output cap (0..1) for gentler motion //duty-cycle / percent output for SparkMax.set(...), which expects a value in [-1.0, 1.0] public static final double INTAKE_ARM_MAX_OUTPUT = 0.20; public static final double INTAKE_ARM_MIN_OUTPUT = -0.10; + // Additional clamp while lowering so the arm descends more softly. + public static final double INTAKE_ARM_LOWERING_MIN_OUTPUT = -0.06; } public static final class CANdleConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b4ec70d..8166f38 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,30 +5,44 @@ package frc.robot; import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.Constants.AutoConstants; +import frc.robot.Tuning.TuningHelper; public class Robot extends TimedRobot { private Command m_autonomousCommand; + private TuningHelper tuning = new TuningHelper(); + private final RobotContainer m_robotContainer; private final RobotSimulation m_robotSimulation; + Thread visionThread; + public Robot() { m_robotContainer = new RobotContainer(); m_robotSimulation = new RobotSimulation(m_robotContainer); + + visionThread = new Thread(() -> { + UsbCamera visionCam = CameraServer.startAutomaticCapture(); + visionCam.setResolution(640, 480); + }); + visionThread.start(); } @Override public void robotInit() { - CameraServer.startAutomaticCapture(); + AutoConstants.initDashboard(); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + tuning.TuningPeriodic(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7c0b076..e04a773 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,19 +21,24 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Auto.LeftLemonAuto; +import frc.robot.Auto.LeftNeutralZoneAuto1; +import frc.robot.Auto.LeftNeutralZoneAuto2; +import frc.robot.Auto.RightNeutralZoneAuto1; +import frc.robot.Auto.RightNeutralZoneAuto2; import frc.robot.Auto.RightLemonAuto; -import frc.robot.Auto.ShootEightAuto; import frc.robot.Auto.CenterLemonAuto; +import frc.robot.Auto.CenterToDepotAuto; +import frc.robot.Auto.DepotShootingAuto; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.VisionConstants; import frc.robot.Constants.ShooterConstants; -import frc.robot.Command.AltAutoAlign; -import frc.robot.Command.AutoAlign; +import frc.robot.Command.SimpleAutoAlign; import frc.robot.Command.TeleopSwerve; import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.DriverStation; public class RobotContainer { @@ -45,31 +50,29 @@ public class RobotContainer { private final CommandXboxController helmsController = new CommandXboxController(1); // Left Stick Y = Forward/backward motion - private final int translationAxis = XboxController.Axis.kLeftY.value; + private final int TRANSLATION_AXIS = XboxController.Axis.kLeftY.value; // Left Stick X = Side-to-side motion - private final int strafeAxis = XboxController.Axis.kLeftX.value; + private final int STRAFE_AXIS = XboxController.Axis.kLeftX.value; // Right Stick X = Rotation/turning motion - private final int rotationAxis = XboxController.Axis.kRightX.value; + private final int ROTATION_AXIS = XboxController.Axis.kRightX.value; // Left Bumper = Toggle robot-oriented mode (default is field-oriented) private final Trigger robotCentric = new Trigger(driveController.leftBumper()); // SwerveSubsystem instance for the drive subsystem private final SwerveSubsystem m_drive = new SwerveSubsystem(); - // IntakeSubsystem for intake private final IntakeSubsystem m_intake = new IntakeSubsystem(); - //ShooterSubsystem for shooter private final ShooterSubsystem m_shooter = new ShooterSubsystem(); private boolean lastHelmsRightBumperPressed = false; private double helmsRightBumperPressTimestampSec = 0.0; + //Limelight private final java.util.Map limelightCameras = new java.util.HashMap<>(); private UsbCamera driverCamera; public RobotContainer() { - AutoConstants.initDashboard(); startLimelightStreams(); startDriverCameraStream(); configureBindings(); @@ -127,8 +130,8 @@ private void configureBindings() { driveController.button(Button.kBack.value).onTrue(new InstantCommand(()->m_drive.resyncModuleEncoders(), m_drive)); //Start Button (menu) = save current module offsets (DISABLED ONLY, wheels must be straight) driveController.button(Button.kStart.value).onTrue(new InstantCommand(()->m_drive.saveModuleOffsets(), m_drive)); - - + //xLock + driveController.button(Button.kB.value).whileTrue(Commands.run(() -> m_drive.xLock(), m_drive)); // SHOOTER CONTROLLER m_shooter.setDefaultCommand( @@ -156,36 +159,35 @@ private void configureBindings() { double kickerSpeed = 0.0; double indexerSpeed = 0.0; + double hopperSpeed = 0.0; if (leftBumperPressed) { kickerSpeed = -ShooterConstants.KICKER_SPEED; indexerSpeed = -ShooterConstants.INDEXER_SPEED; + hopperSpeed = -ShooterConstants.HOPPER_SPEED; } else if (rightBumperPressed) { kickerSpeed = ShooterConstants.KICKER_SPEED; indexerSpeed = indexerEnabled ? ShooterConstants.INDEXER_SPEED : 0.0; + hopperSpeed = indexerEnabled ? ShooterConstants.HOPPER_SPEED : 0.0; } m_shooter.setKickerSpeed(kickerSpeed); m_shooter.setIndexerSpeed(indexerSpeed); + m_shooter.setHopperSpeed(hopperSpeed); lastHelmsRightBumperPressed = rightBumperPressed; }, m_shooter)); - + // Hood controls (helms controller). // Y = hood up (2 inches / max travel), B = hood down. helmsController.y().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), m_shooter)); helmsController.b().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.MED), m_shooter)); helmsController.a().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); - - // Left Trigger = Auto-align to left scoring position - driveController.axisGreaterThan(Axis.kLeftTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, true)); - // Right Trigger = Auto-align to right scoring position - driveController.axisGreaterThan(Axis.kRightTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, false)); - // Right Bumper = Alt-Auto-Align - driveController.button(Button.kRightBumper.value).whileTrue(new AltAutoAlign(m_drive, m_shooter)); + //Right Bumper = Simple Auto Align + driveController.button(Button.kRightBumper.value).whileTrue(new SimpleAutoAlign(m_drive)); // Default command runs continuously when no other command requires the subsystem. // It automatically pauses when commands like AutoAlign take control, then resumes @@ -195,11 +197,11 @@ private void configureBindings() { // SwerveSubsystem - The drive subsystem to control m_drive, // translationSupplier - Forward/backward speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.7, + () -> -getSpeedMultiplier() * driveController.getRawAxis(TRANSLATION_AXIS) * 1.0, // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.7, + () -> -getSpeedMultiplier() * driveController.getRawAxis(STRAFE_AXIS) * 1.0, // rotationSupplier - Rotation speed - () -> -driveController.getRawAxis(rotationAxis) * 0.5, + () -> -driveController.getRawAxis(ROTATION_AXIS) * 0.5, // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) () -> robotCentric.getAsBoolean(), // isAutoAlignSupplier - Auto-align active flag @@ -230,13 +232,20 @@ private double getSpeedMultiplier(){ public Command getAutonomousCommand() { AutoConstants.AutoMode selected = AutoConstants.getSelectedAutoMode(); + SmartDashboard.putString("Auto/Selected", selected.name()); + DriverStation.reportWarning("Auto selected: " + selected.name(), false); return switch (selected) { case None -> Commands.none(); case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); - case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); + case RightNeutralZoneAuto1 -> new RightNeutralZoneAuto1 (m_drive, m_intake, m_shooter); + case RightNeutralZoneAuto2 -> new RightNeutralZoneAuto2 (m_drive, m_intake, m_shooter); + case LeftNeutralZoneAuto2 -> new LeftNeutralZoneAuto2 (m_drive, m_intake, m_shooter); + case LeftNeutralZoneAuto1 -> new LeftNeutralZoneAuto1(m_drive, m_intake, m_shooter); case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); + case CenterToDepotAuto -> new CenterToDepotAuto(m_drive, m_intake, m_shooter); + case DepotShootingAuto -> new DepotShootingAuto(m_drive, m_intake, m_shooter); default -> Commands.none(); diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 412243c..351a044 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -4,6 +4,7 @@ package frc.robot.Subsystems; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -21,14 +22,27 @@ public class IntakeSubsystem extends SubsystemBase { private final SparkMax intakeMotor = new SparkMax(IntakeConstants.INTAKE_ID, MotorType.kBrushless); private final SparkMax intakeArmMotor = new SparkMax(IntakeConstants.INTAKE_ARM_ID, MotorType.kBrushless); + private final SparkMax intakeArmMotor2 = new SparkMax(IntakeConstants.INTAKE_ARM_2_ID, MotorType.kBrushless); private final RelativeEncoder intakeArmEncoder = intakeArmMotor.getEncoder(); + private final RelativeEncoder intakeArm2Encoder = intakeArmMotor2.getEncoder(); private final PIDController intakeArmController = new PIDController( IntakeConstants.INTAKE_ARM_kP, IntakeConstants.INTAKE_ARM_kI, IntakeConstants.INTAKE_ARM_kD); + private final PIDController intakeArm2Controller = new PIDController( + IntakeConstants.INTAKE_ARM_kP, + IntakeConstants.INTAKE_ARM_kI, + IntakeConstants.INTAKE_ARM_kD); + + private final ArmFeedforward intakeArmFeedforward = new ArmFeedforward( + IntakeConstants.INTAKE_ARM_kS, + IntakeConstants.INTAKE_ARM_kG, + IntakeConstants.INTAKE_ARM_kV, + IntakeConstants.INTAKE_ARM_kA); + private double intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_RAISED_POSITION; private boolean intakeArmActive = false; @@ -60,6 +74,14 @@ public IntakeSubsystem() { intakeArmController.setTolerance(IntakeConstants.INTAKE_ARM_TOLERANCE_DEG); intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_RAISED_POSITION; intakeArmActive = false; + + intakeArmMotor2.configure(intakeArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + //On enable, assume the arm starts raised at 90 degrees + intakeArm2Encoder.setPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); + intakeArm2Controller.setTolerance(IntakeConstants.INTAKE_ARM_TOLERANCE_DEG); + intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_RAISED_POSITION; + intakeArmActive = false; + } public void toggleIntake() { @@ -97,6 +119,7 @@ public void setIntakeArmAngle(IntakeArmAngle angle){ IntakeConstants.INTAKE_ARM_MIN_DEG, Math.min(IntakeConstants.INTAKE_ARM_MAX_DEG, intakeArmTargetDeg)); intakeArmController.reset(); + intakeArm2Controller.reset(); intakeArmActive = true; } @@ -121,26 +144,55 @@ public double getArmPositionDeg() { return intakeArmEncoder.getPosition(); } + public double getArmPositionDeg2() { + return intakeArm2Encoder.getPosition(); + } + @Override public void periodic() { double currentDeg = getArmPositionDeg(); + double currentDeg2 = getArmPositionDeg2(); SmartDashboard.putNumber("IntakeArm/TargetDeg", intakeArmTargetDeg); SmartDashboard.putNumber("IntakeArm/PostionDeg", currentDeg); + SmartDashboard.putNumber("IntakeArm/PostionDeg2", currentDeg2); SmartDashboard.putBoolean("IntakeArm/Active", intakeArmActive); + double rawffOutput = intakeArmFeedforward.calculate(Math.toRadians(currentDeg), 0); + double rawffOutput2 = intakeArmFeedforward.calculate(Math.toRadians(currentDeg2), 0); + //divide ff output(in volts) by battery volts for percent output that motor.set expects + double ffOutput = rawffOutput/12; + double ffOutput2 = rawffOutput2/12; + if (intakeArmActive){ - double output = intakeArmController.calculate(currentDeg, intakeArmTargetDeg); + double pidOutput = intakeArmController.calculate(currentDeg, intakeArmTargetDeg); + double pidOutput2 = intakeArm2Controller.calculate(currentDeg2, intakeArmTargetDeg); + + double output = pidOutput + ffOutput; + double output2 = pidOutput2 + ffOutput2; + + //NEED TESTING + // When commanded down, limit downward power further so the arm settles more gently. + if (!intakeUp) { + output = Math.max(IntakeConstants.INTAKE_ARM_LOWERING_MIN_OUTPUT, output); + output2 = Math.max(IntakeConstants.INTAKE_ARM_LOWERING_MIN_OUTPUT, output2); + } + output = Math.max(IntakeConstants.INTAKE_ARM_MIN_OUTPUT, Math.min(IntakeConstants.INTAKE_ARM_MAX_OUTPUT, output)); + output2 = Math.max(IntakeConstants.INTAKE_ARM_MIN_OUTPUT, Math.min(IntakeConstants.INTAKE_ARM_MAX_OUTPUT, output2)); + - if (intakeArmController.atSetpoint()){ - intakeArmMotor.set(0.0); + if (intakeArmController.atSetpoint() && intakeArm2Controller.atSetpoint()){ + intakeArmMotor.set(ffOutput); + intakeArmMotor2.set(ffOutput2); intakeArmActive = false; } else { intakeArmMotor.set(output); + intakeArmMotor2.set(output2); } } else{ - intakeArmMotor.set(0.0); + intakeArmMotor.set(ffOutput); + intakeArmMotor2.set(ffOutput2); } } } diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 4f78c76..ffe678f 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -23,10 +23,12 @@ public class ShooterSubsystem extends SubsystemBase { private final SparkMax kickerMotor = new SparkMax(ShooterConstants.KICKER_ID, MotorType.kBrushless); private final SparkMax hoodMotor = new SparkMax(ShooterConstants.HOOD_ID, MotorType.kBrushless); private final SparkMax indexerMotor = new SparkMax(ShooterConstants.INDEXER_ID, MotorType.kBrushless); + private final SparkMax hopperMotor = new SparkMax(ShooterConstants.HOPPER_ID, MotorType.kBrushless); private double shooterCmd = 0.0; private double kickerCmd = 0.0; private double indexerCmd = 0.0; + private double hopperCmd = 0.0; private final PIDController hoodController = new PIDController( ShooterConstants.HOOD_KP, @@ -65,12 +67,17 @@ public ShooterSubsystem() { indexConfig.inverted(true); indexConfig.idleMode(IdleMode.kBrake); + SparkMaxConfig hopperConfig = new SparkMaxConfig(); + hopperConfig.inverted(true); + hopperConfig.idleMode(IdleMode.kCoast); + shooterMotor.configure(shootConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); kickerMotor.configure(feedConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); hoodMotor.configure(hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); indexerMotor.configure(indexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + hopperMotor.configure(hopperConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); hoodController.setTolerance(ShooterConstants.HOOD_TOLERANCE); } @@ -117,6 +124,11 @@ public void setKickerSpeed(double speed) { kickerMotor.set(speed); } + public void setHopperSpeed(double speed) { + hopperCmd = speed; + hopperMotor.set(speed); + } + public void setHoodAngle(HoodAngle angle) { switch (angle) { case LOW: @@ -158,7 +170,7 @@ public void AutoToggleShoot (boolean AutoShootOn) { public void AutoToggleKickIndex (boolean AutoIndexKickOn) { setKickerSpeed(AutoIndexKickOn ? 0 : ShooterConstants.KICKER_SPEED); - setIndexerSpeed(AutoIndexKickOn ? 0 : ShooterConstants.SHOOTER_SPEED); + setIndexerSpeed(AutoIndexKickOn ? 0 : ShooterConstants.INDEXER_SPEED); } @Override diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 27244d3..cc7cd1c 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -7,6 +7,9 @@ import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.FollowPathCommand; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.VecBuilder; @@ -104,11 +107,12 @@ public void simulationReset() { private void configurePathPlanner(){ - AutoBuilder.configure(this::getPose, + AutoBuilder.configure( + this::getPose, this::resetOdometry, this::getChassisSpeeds, - (speeds, feedforwards)->driveFromChassisSpeeds(speeds, false), - AutoConstants.SWERV_DRIVE_CONTROLLER, + (speeds, feedforwards)-> driveFromChassisSpeeds(speeds, false), + AutoConstants.SWERVE_DRIVE_CONTROLLER, AutoConstants.ROBOT_CONFIG, FieldConstants::isRedAlliance, this); @@ -120,7 +124,17 @@ public Command autoDrive(String filename){ if (AutoConstants.isRightSideAuto()){ path = path.mirrorPath(); } - return AutoBuilder.followPath(path); + return new FollowPathCommand( + path, + this::getPose, + this::getChassisSpeeds, + (speeds, feedforwards) -> driveFromChassisSpeeds(speeds, isVisionEnabled()), + new PPHolonomicDriveController( + new PIDConstants(SwerveConstants.driveKP, SwerveConstants.driveKI, SwerveConstants.driveKD), + new PIDConstants(SwerveConstants.driveKP, SwerveConstants.driveKI, SwerveConstants.driveKD)), + Constants.AutoConstants.ROBOT_CONFIG, + Constants.FieldConstants::isRedAlliance, + this); } catch(Exception e){ DriverStation.reportError("PATHPLANNER ERROR" + e.getMessage(), e.getStackTrace()); @@ -147,17 +161,27 @@ private boolean isVisionEnabled() { } private void updateOdometryWithVision (String limelightName){ - boolean doRejectUpdate = false; - LimelightHelpers.SetRobotOrientation(limelightName, odometry.getEstimatedPosition().getRotation().getDegrees(),0,0,0,0,0); - LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); - if (mt1 == null){ + boolean doRejectUpdate = false; + + //pass raw yaw + rate instead of estimate here for accuracy + LimelightHelpers.SetRobotOrientation(limelightName, getYaw().getDegrees(),pigeon.getAngularVelocityZWorld().getValueAsDouble(),0,0,0,0); + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); + + if (mt2 == null){ return; } if(Math.abs(pigeon.getAngularVelocityZWorld().getValueAsDouble()) > VisionConstants.MAX_VISION_ANGULAR_RATE_DEG_PER_SEC) { doRejectUpdate = true; } - if(mt1.tagCount == 0) + + //reject poses that aren't inside the field + boolean isInField = mt2.pose.getX() > 0 && mt2.pose.getX() < FieldConstants.FIELD_LENGTH + && mt2.pose.getY() > 0 && mt2.pose.getY() < FieldConstants.FIELD_WIDTH; + //reject poses if LL is too far away from tag + boolean isCloseEnough = mt2.avgTagDist < 10; //10 is high, check what we think distance should be + + if(mt2.tagCount <= 0 || isInField == false || isCloseEnough == false) { doRejectUpdate = true; } @@ -169,13 +193,14 @@ private void updateOdometryWithVision (String limelightName){ VisionConstants.VISION_STD_DEV_Y_METERS, VisionConstants.VISION_STD_DEV_THETA_RADIANS)); // need to measure odometry.addVisionMeasurement( - mt1.pose, - mt1.timestampSeconds); + mt2.pose, + mt2.timestampSeconds); } - SmartDashboard.putNumber("Vision/" + limelightName + "/TagCount", mt1.tagCount); - SmartDashboard.putNumber("Vision/" + limelightName + "/AvgTagDist", mt1.avgTagDist); - SmartDashboard.putNumber("Vision/" + limelightName + "/LatencyMs", mt1.latency); + SmartDashboard.putBoolean("Is Vision Good", !doRejectUpdate); + SmartDashboard.putNumber("Vision/" + limelightName + "/TagCount", mt2.tagCount); + SmartDashboard.putNumber("Vision/" + limelightName + "/AvgTagDist", mt2.avgTagDist); + SmartDashboard.putNumber("Vision/" + limelightName + "/LatencyMs", mt2.latency); } @@ -203,6 +228,32 @@ public void driveFromChassisSpeeds(ChassisSpeeds driveSpeeds, boolean isOpenLoop } } + private void Lockwheels () { + for(int i = 0; i < mSwerveMods. length; i++){ + SwerveModule module = mSwerveMods [i]; + Rotation2d angle = SwerveConstants.swerveKinematics.getModules() [i].getAngle(); + SwerveModuleState xState = new SwerveModuleState(0, angle); + module. setDesiredState(xState, false) ; + } + } + + public Command xLockCommand ( ) { + return run(this::xLock); + } + + public void xLock(){ + Rotation2d[] xLockAngles = new Rotation2d [4]; + xLockAngles[0] = Rotation2d.fromDegrees(225); + xLockAngles[1] = Rotation2d.fromDegrees(135); + xLockAngles[2] = Rotation2d.fromDegrees(225); + xLockAngles[3] = Rotation2d.fromDegrees(135); + + for (SwerveModule mod:mSwerveMods){ + mod.setDesiredState(new SwerveModuleState(0,mod.getState().angle), true); + mod.pointInDirection(xLockAngles[mod.moduleNumber].getDegrees()); + } + } + public ChassisSpeeds getChassisSpeeds(){ return SwerveConstants.swerveKinematics.toChassisSpeeds(getStates()); } @@ -299,6 +350,7 @@ public void periodic() { } } } + //sends robot pose to network tables field.setRobotPose(getPose()); SmartDashboard.putNumber("Pigeon Yaw", pigeon.getYaw().getValueAsDouble()); diff --git a/src/main/java/frc/robot/Tuning/TuningHelper.java b/src/main/java/frc/robot/Tuning/TuningHelper.java new file mode 100644 index 0000000..45a0f09 --- /dev/null +++ b/src/main/java/frc/robot/Tuning/TuningHelper.java @@ -0,0 +1,57 @@ +package frc.robot.Tuning; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class TuningHelper { + + /* PID Tuning Variables */ + public static double tuningkP; + public static double tuningkI; + public static double tuningkD; + + /* Feedforward Tuning Variables */ + public static double tuningkS; + public static double tuningkG; + public static double tuningkV; + public static double tuningkA; + + /* Motor Speed Tuning */ + public static double tuningSpeed; + + /* Setpoint Tuning */ + public static double tuningSetpoint; + + /* tuningDeadband */ + public static double tuningDeadband; + + public TuningHelper() { + SmartDashboard.putNumber("Tuning/kP", tuningkP); + SmartDashboard.putNumber("Tuning/kI", tuningkI); + SmartDashboard.putNumber("Tuning/kD", tuningkD); + + SmartDashboard.putNumber("Tuning/kS", tuningkS); + SmartDashboard.putNumber("Tuning/kG", tuningkG); + SmartDashboard.putNumber("Tuning/kV", tuningkV); + SmartDashboard.putNumber("Tuning/kA", tuningkA); + + SmartDashboard.putNumber("Tuning/Speed", tuningSpeed); + SmartDashboard.putNumber("Tuning/Setpoint", tuningSetpoint); + SmartDashboard.putNumber("Tuning/Deadband", tuningDeadband); + } + + public void TuningPeriodic() { + tuningkP = SmartDashboard.getNumber("Tuning/kP", tuningkP); + tuningkI = SmartDashboard.getNumber("Tuning/kI", tuningkI); + tuningkD = SmartDashboard.getNumber("Tuning/kD", tuningkD); + + tuningkS = SmartDashboard.getNumber("Tuning/kS", tuningkS); + tuningkG = SmartDashboard.getNumber("Tuning/kG", tuningkG); + tuningkV = SmartDashboard.getNumber("Tuning/kV", tuningkV); + tuningkA = SmartDashboard.getNumber("Tuning/kA", tuningkA); + + tuningSpeed = SmartDashboard.getNumber("Tuning/Speed", tuningSpeed); + tuningSetpoint = SmartDashboard.getNumber("Tuning/Setpoint", tuningSetpoint); + tuningDeadband = SmartDashboard.getNumber("Tuning/Deadband", tuningDeadband); + } + +} diff --git a/src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java b/src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java deleted file mode 100644 index 34a761f..0000000 --- a/src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java +++ /dev/null @@ -1,91 +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.UnusedAuto; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.SwerveConstants; -import frc.robot.Subsystems.SwerveSubsystem; - -public class CenterToDepotAuto extends SequentialCommandGroup { - public CenterToDepotAuto (SwerveSubsystem drive) { - final double[] startYawRad = new double[1]; - addCommands( - drive.startAutoAt(4.61, 4.03, 90.0), - new InstantCommand(()->drive.drive(0,0.5,0, false), drive), - Commands.waitSeconds(2), - new InstantCommand(()->drive.drive(0,0,0, false),drive), - - Commands.waitSeconds(1), - - //SHOOT - - new InstantCommand(()-> drive.drive(0.9,0,0,false),drive), - Commands.waitSeconds(2), - new InstantCommand(()->drive.drive(0,0,0,false), drive), - - //Turn ~90 degrees in place (robot-centric) - Commands.runOnce(()->startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(()->{ - double targetYawRad = startYawRad[0] + (Math.PI / 2.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0,0,omegaRadiansPerSecond, false); - }, drive).until(()->{ - double targetYawRad = startYawRad[0] + (Math.PI / 2.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }), - new InstantCommand(() -> drive.drive(0,0,0, false), drive), - - // Move forward ~1m (0.5 m/s for 2s) after turning (to the depot) - new InstantCommand(() -> drive.drive(0.7,0,0, false), drive), - Commands.waitSeconds(2), - new InstantCommand(() -> drive.drive(0,0,0, false), drive), - - Commands.waitSeconds(2), - - //INTAKE - - - // Back up ~0.5m, then turn 180 degrees - new InstantCommand(() -> drive.drive(-0.5,0,0, false), drive), - Commands.waitSeconds(1), - new InstantCommand(() -> drive.drive(0,0,0, false), drive), - Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(()->{ - double targetYawRad = startYawRad[0] + Math.PI; - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0,0,omegaRadiansPerSecond, false); - }, drive).until(()->{ - double targetYawRad = startYawRad[0] + Math.PI; - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }), - - new InstantCommand(() -> drive.drive(0.4, 0, 0, false), drive), - Commands.waitSeconds(2), - new InstantCommand(() -> drive.drive(0,0,0, false), drive), - - //Turn 40 degrees left (counterclockwise) - Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(40.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0,0, omegaRadiansPerSecond, false); - }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(40.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }), - new InstantCommand(() -> drive.drive(0,0,0, false), drive) - ); - } -} - diff --git a/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java b/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java index 105578a..89a1e05 100644 --- a/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java +++ b/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java @@ -4,29 +4,18 @@ package frc.robot.UnusedAuto; - import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; -/* public class DriveTestAuto extends SequentialCommandGroup { - public DriveTestAuto (SwerveSubsystem drive) { - addCommands( - new InstantCommand(() -> drive.drive(0.5,0,0, false), drive), - Commands.waitSeconds(2), - new InstantCommand(() -> drive.drive(0,0,0, false), drive) - ); - } -} -*/ - + public DriveTestAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); - -public class DriveTestAuto extends SequentialCommandGroup { - public DriveTestAuto (SwerveSubsystem drive){ addCommands( - drive.startAutoAt(1.165, 6.000, 0.000), - drive.autoDrive("DriveTestPath") + drive.startAutoAt(1.984, 7.199, -90), + drive.autoDrive("Path") ); } } diff --git a/src/main/java/frc/robot/UnusedAuto/LeftNeutralZoneAuto2.java b/src/main/java/frc/robot/UnusedAuto/LeftNeutralZoneAuto2.java new file mode 100644 index 0000000..fed70d8 --- /dev/null +++ b/src/main/java/frc/robot/UnusedAuto/LeftNeutralZoneAuto2.java @@ -0,0 +1,154 @@ +// 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.UnusedAuto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; + + +public class LeftNeutralZoneAuto2 extends SequentialCommandGroup { + + private static final double INTAKE_POWER = -1.0; + + public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + final double[] startYawRad = new double[1]; + + addCommands( + Commands.runOnce(intake::lowerIntake, intake), + + //Drive backwards 3.6m + new InstantCommand(() -> drive.drive(-3.6, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + //Turn 90 degrees left + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Drive forward 3m + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + new InstantCommand(() -> drive.drive(3, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees right + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Drive forward 1.3m + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + new InstantCommand(() -> drive.drive(1.3, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees right + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Drive forward 3m + new InstantCommand(() -> drive.drive(3, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + //Stop intake + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0,false), drive), + + // Turn 90 degrees right + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Drive backwards + new InstantCommand(() -> drive.drive(-1, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 10 degrees left + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(10.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(10.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Bring hood up to HIGH angle + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + //Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(2.0), + + Commands.sequence( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) + + + ); + } +} diff --git a/src/main/java/frc/robot/Auto/ShootEightAuto.java b/src/main/java/frc/robot/UnusedAuto/ShootEightAuto.java similarity index 93% rename from src/main/java/frc/robot/Auto/ShootEightAuto.java rename to src/main/java/frc/robot/UnusedAuto/ShootEightAuto.java index 3ec5bf5..f13b2ca 100644 --- a/src/main/java/frc/robot/Auto/ShootEightAuto.java +++ b/src/main/java/frc/robot/UnusedAuto/ShootEightAuto.java @@ -2,7 +2,7 @@ // 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.Auto; +package frc.robot.UnusedAuto; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -16,7 +16,7 @@ public class ShootEightAuto extends SequentialCommandGroup { public ShootEightAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { final double[] startYawRad = new double[1]; addCommands( - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), Commands.runOnce(() -> { shooter.runIndexer(false); shooter.runKicker(false); @@ -36,6 +36,7 @@ public ShootEightAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubs shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ) );