From 6186482b590e41bf23f9441ae5fddc19efd60097 Mon Sep 17 00:00:00 2001 From: micw1417 Date: Sat, 14 Mar 2026 09:24:44 -0500 Subject: [PATCH 1/3] Auton Lets see if it works --- .../autos/LeftNeutralEatTrench.auto | 21 +- .../pathplanner/paths/NeutralZoneEat.path | 130 ----------- .../paths/NeutralZoneEatShoot.path | 212 ++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 2 - 4 files changed, 232 insertions(+), 133 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/NeutralZoneEat.path create mode 100644 src/main/deploy/pathplanner/paths/NeutralZoneEatShoot.path diff --git a/src/main/deploy/pathplanner/autos/LeftNeutralEatTrench.auto b/src/main/deploy/pathplanner/autos/LeftNeutralEatTrench.auto index 38f6e1c..18c42e6 100644 --- a/src/main/deploy/pathplanner/autos/LeftNeutralEatTrench.auto +++ b/src/main/deploy/pathplanner/autos/LeftNeutralEatTrench.auto @@ -7,7 +7,26 @@ { "type": "path", "data": { - "pathName": "NeutralZoneEat" + "pathName": "NeutralZoneEatShoot" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/paths/NeutralZoneEat.path b/src/main/deploy/pathplanner/paths/NeutralZoneEat.path deleted file mode 100644 index 5a695bf..0000000 --- a/src/main/deploy/pathplanner/paths/NeutralZoneEat.path +++ /dev/null @@ -1,130 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6250213980028523, - "y": 7.366704707560627 - }, - "prevControl": null, - "nextControl": { - "x": 4.910358124386693, - "y": 7.235232866436881 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.109243937232525, - "y": 7.2890727532097 - }, - "prevControl": { - "x": 5.293763195435091, - "y": 7.444041369472182 - }, - "nextControl": { - "x": 6.876923920144677, - "y": 7.143187879923687 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.769115549215405, - "y": 7.263195435092724 - }, - "prevControl": { - "x": 6.440439884906579, - "y": 7.250554063388538 - }, - "nextControl": { - "x": 7.441925820256776, - "y": 7.2890727532097 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.778330955777461, - "y": 7.250256776034237 - }, - "prevControl": { - "x": 7.700699001426534, - "y": 7.547845934379458 - }, - "nextControl": { - "x": 7.983342381391882, - "y": 6.464379644512293 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.843024251069899, - "y": 4.455506419400857 - }, - "prevControl": { - "x": 7.881840228245363, - "y": 6.111654778887304 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 179.64632684075352 - }, - { - "waypointRelativePos": 2, - "rotationDegrees": -97.12501634890168 - }, - { - "waypointRelativePos": 3.0, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 4.0, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 4, - "rotationDegrees": -40.60129464500447 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 270.0, - "maxAngularAcceleration": 360.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -91.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 178.8308606720925 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NeutralZoneEatShoot.path b/src/main/deploy/pathplanner/paths/NeutralZoneEatShoot.path new file mode 100644 index 0000000..03fe576 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NeutralZoneEatShoot.path @@ -0,0 +1,212 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6250213980028523, + "y": 7.366704707560627 + }, + "prevControl": null, + "nextControl": { + "x": 4.928500436632966, + "y": 7.24484093912391 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.109243937232525, + "y": 7.2890727532097 + }, + "prevControl": { + "x": 5.352969879672161, + "y": 7.3163272798495145 + }, + "nextControl": { + "x": 6.6924213677461735, + "y": 7.2680562649978135 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.769115549215405, + "y": 7.263195435092724 + }, + "prevControl": { + "x": 6.202838145377834, + "y": 7.309493962170061 + }, + "nextControl": { + "x": 7.399788857164295, + "y": 7.211631934541322 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.778330955777461, + "y": 7.250256776034237 + }, + "prevControl": { + "x": 7.778330955777461, + "y": 7.500256776034237 + }, + "nextControl": { + "x": 7.778330955777461, + "y": 6.250256776034237 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.778330955777461, + "y": 4.7670456852791885 + }, + "prevControl": { + "x": 7.778330955777461, + "y": 5.0170456852791885 + }, + "nextControl": { + "x": 7.778330955777461, + "y": 4.5170456852791885 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.769115549215405, + "y": 4.905167512690355 + }, + "prevControl": { + "x": 7.1342878556377025, + "y": 4.481593908629441 + }, + "nextControl": { + "x": 6.403943242793107, + "y": 5.328741116751268 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.269938513371728, + "y": 7.250256776034237 + }, + "prevControl": { + "x": 6.966780503409591, + "y": 7.250256776034237 + }, + "nextControl": { + "x": 5.781907035448068, + "y": 7.250256776034237 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0563654822335025, + "y": 7.288795261663903 + }, + "prevControl": { + "x": 5.306365482233503, + "y": 7.288795261663903 + }, + "nextControl": { + "x": 4.806365482233502, + "y": 7.288795261663903 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.338131979695432, + "y": 7.280862944162436 + }, + "prevControl": { + "x": 5.157654822335026, + "y": 7.290071065989848 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 179.64632684075352 + }, + { + "waypointRelativePos": 2, + "rotationDegrees": -97.12501634890168 + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 4, + "rotationDegrees": -86.93191714919085 + }, + { + "waypointRelativePos": 4, + "rotationDegrees": -89.58836317312266 + }, + { + "waypointRelativePos": 6, + "rotationDegrees": 91.0846947726597 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.9389956184698183, + "maxWaypointRelativePos": 4, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 2.3781597573306374, + "endWaypointRelativePos": 4.214357937310409, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 88.74584863935306 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 178.8308606720925 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d1c024..2bfbf60 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -304,7 +304,6 @@ private void configureDriverBindings() { // controller.x().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); // controller.a().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); - controller.povUp().whileTrue(intake.runPivotVoltageCommand(-3)); controller.povDown().whileTrue(intake.runPivotVoltageCommand(3)); //positive means down @@ -361,7 +360,6 @@ private void registerCommands() { NamedCommands.registerCommand("Intake", intake.runIntakeCommand(IntakeMode.INTAKE)); NamedCommands.registerCommand("deployIntake", intake.runPivotVoltageCommand(3).withTimeout(0.5)); - NamedCommands.registerCommand("RevShooterClose", shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE).withTimeout(0.5)); } /** From 356d5076c04405449f120694e18ce94fa7e8d3a3 Mon Sep 17 00:00:00 2001 From: micw1417 Date: Sat, 14 Mar 2026 09:53:12 -0500 Subject: [PATCH 2/3] angle setcontrol code from spectrum lets see if it works --- .../subsystems/shooter/ShooterConstants.java | 4 +- .../shooter/angler/AnglerIOTalonFX.java | 58 ++++++++++++------- 2 files changed, 40 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 7b53980..c4abbb0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -16,8 +16,8 @@ public class ShooterConstants { public enum ShooterModes { SHOOT_FAR(Units.RPM.of(3500), Units.Rotations.of(.75)), SHOOT_CLOSE(Units.RPM.of(2500), Units.Rotations.of(.1)), - IDLE(Units.RPM.of(0), Units.Degree.of(0)), - REVERSE(Units.RPM.of(-500), Units.Degree.of(0)); + IDLE(Units.RPM.of(0), Units.Rotations.of(0)), + REVERSE(Units.RPM.of(-500), Units.Rotations.of(0)); public AngularVelocity speed; public Angle angle; diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index e5e06b1..1e7cab7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.controller.PIDController; @@ -14,6 +15,7 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants; @@ -23,8 +25,12 @@ public class AnglerIOTalonFX implements AnglerIO { private final TalonFX talon = new TalonFX(ShooterAnglerConstants.id, Constants.CANIVORE_CANBUS); - public final ProfiledPIDController pid = new ProfiledPIDController(ShooterAnglerConstants.anglerP.get(), - ShooterAnglerConstants.anglerI.get(), ShooterAnglerConstants.anglerD.get(), new TrapezoidProfile.Constraints(3.0, 8.0)); + private TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(3.0, 8.0)); + private TrapezoidProfile.State anglerSetpoint = new TrapezoidProfile.State(); + private PositionTorqueCurrentFOC anglerRequest = new PositionTorqueCurrentFOC(0); + + // public final ProfiledPIDController pid = new ProfiledPIDController(ShooterAnglerConstants.anglerP.get(), + // ShooterAnglerConstants.anglerI.get(), ShooterAnglerConstants.anglerD.get(), new TrapezoidProfile.Constraints(3.0, 8.0)); public static class PivotTalonFXConfig extends CTREMechanism.Config { public PivotTalonFXConfig() { @@ -41,6 +47,7 @@ public PivotTalonFXConfig() { private StatusSignal appliedVoltage; private StatusSignal pivotPosition; private StatusSignal currentAmps; + private StatusSignal velocityRPM; private double angleSetpoint; // No clue stole from ModuleIO @@ -52,20 +59,21 @@ public AnglerIOTalonFX() { appliedVoltage = talon.getMotorVoltage(); pivotPosition = talon.getPosition(); currentAmps = talon.getSupplyCurrent(); + velocityRPM = talon.getVelocity(); config.applyTalonConfig(talon); - BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, pivotPosition); + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, pivotPosition, velocityRPM); } @Override public void updateInputs(AnglerIOInputs inputs) { - if (ShooterAnglerConstants.tunePID) { - pid.setP(ShooterAnglerConstants.anglerP.get()); - pid.setI(ShooterAnglerConstants.anglerI.get()); - pid.setD(ShooterAnglerConstants.anglerD.get()); - } + // if (ShooterAnglerConstants.tunePID) { + // pid.setP(ShooterAnglerConstants.anglerP.get()); + // pid.setI(ShooterAnglerConstants.anglerI.get()); + // pid.setD(ShooterAnglerConstants.anglerD.get()); + // } var anglerStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); @@ -80,26 +88,36 @@ public void updateInputs(AnglerIOInputs inputs) { @Override public void setPosition(double angleSetpoint) { - // PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); - // talon.setControl(mm); this.angleSetpoint = angleSetpoint; - // far is 0.75, near is 1 - // angleSetpoint = ShooterAnglerConstants.bangBangAngle.getAsDouble(); + TrapezoidProfile.State currentState = new TrapezoidProfile.State(pivotPosition.getValue() + .in(Rotation), velocityRPM.getValueAsDouble() / 60.0); + TrapezoidProfile.State goalState = new TrapezoidProfile.State(angleSetpoint, 0); + anglerSetpoint = profile.calculate(0.02, currentState, goalState); + + anglerRequest.Position = anglerSetpoint.position; + anglerRequest.Velocity = anglerSetpoint.velocity; + talon.setControl(anglerRequest); + + // // PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); + // // talon.setControl(mm); + // this.angleSetpoint = angleSetpoint; + // // far is 0.75, near is 1 + // // angleSetpoint = ShooterAnglerConstants.bangBangAngle.getAsDouble(); - double currentAngle = talon.getPosition().getValueAsDouble(); - double voltage = 0; + // double currentAngle = talon.getPosition().getValueAsDouble(); + // double voltage = 0; - // ShooterAnglerConstants.bangBangController.setTolerance(ShooterAnglerConstants.tolerance); - // ShooterAnglerConstants.bangBangController.setTolerance(ShooterAnglerConstants.tunableTolerance.getAsDouble()); + // // ShooterAnglerConstants.bangBangController.setTolerance(ShooterAnglerConstants.tolerance); + // // ShooterAnglerConstants.bangBangController.setTolerance(ShooterAnglerConstants.tunableTolerance.getAsDouble()); - voltage = pid.calculate(currentAngle, angleSetpoint); + // voltage = pid.calculate(currentAngle, angleSetpoint); - voltage = Math.max(Math.min(voltage, 12), -12); + // voltage = Math.max(Math.min(voltage, 12), -12); - // talon.setVoltage( ShooterAnglerConstants.anglerkV.get()); + // // talon.setVoltage( ShooterAnglerConstants.anglerkV.get()); - talon.setVoltage(voltage); + // talon.setVoltage(voltage); } @Override From c475275c7a25fd860735a5906b3001d038ec7e81 Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Mon, 16 Mar 2026 13:55:49 -0500 Subject: [PATCH 3/3] more auton code and climber --- .../deploy/pathplanner/autos/DummyAuto.auto | 25 +++ .../autos/LeftNeutralEatTrench.auto | 29 ++- .../pathplanner/autos/NearNeutralTrench.auto | 63 ++++++ src/main/deploy/pathplanner/paths/Dummy.path | 54 +++++ .../paths/NeutralZoneEatShoot.path | 126 +++++------- .../pathplanner/paths/NeutralZoneNear.path | 189 ++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 22 +- .../frc/robot/subsystems/climber/Climber.java | 9 +- .../subsystems/climber/ClimberConstants.java | 5 +- .../robot/subsystems/climber/ClimberIO.java | 3 +- .../climber/ClimberIOSparkFlex.java | 10 +- .../subsystems/climber/ClimberIOTalonFX.java | 13 +- 12 files changed, 444 insertions(+), 104 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/DummyAuto.auto create mode 100644 src/main/deploy/pathplanner/autos/NearNeutralTrench.auto create mode 100644 src/main/deploy/pathplanner/paths/Dummy.path create mode 100644 src/main/deploy/pathplanner/paths/NeutralZoneNear.path diff --git a/src/main/deploy/pathplanner/autos/DummyAuto.auto b/src/main/deploy/pathplanner/autos/DummyAuto.auto new file mode 100644 index 0000000..d4c9924 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/DummyAuto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Dummy" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeftNeutralEatTrench.auto b/src/main/deploy/pathplanner/autos/LeftNeutralEatTrench.auto index 18c42e6..933c608 100644 --- a/src/main/deploy/pathplanner/autos/LeftNeutralEatTrench.auto +++ b/src/main/deploy/pathplanner/autos/LeftNeutralEatTrench.auto @@ -4,6 +4,25 @@ "type": "sequential", "data": { "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.8 + } + }, + { + "type": "named", + "data": { + "name": "deployIntake" + } + } + ] + } + }, { "type": "path", "data": { @@ -14,6 +33,12 @@ "type": "race", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, { "type": "named", "data": { @@ -21,9 +46,9 @@ } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 3.0 + "name": "CarpetRun" } } ] diff --git a/src/main/deploy/pathplanner/autos/NearNeutralTrench.auto b/src/main/deploy/pathplanner/autos/NearNeutralTrench.auto new file mode 100644 index 0000000..b6feef5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/NearNeutralTrench.auto @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.8 + } + }, + { + "type": "named", + "data": { + "name": "deployIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "NeutralZoneNear" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "AutoShoot" + } + }, + { + "type": "named", + "data": { + "name": "CarpetRun" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Dummy.path b/src/main/deploy/pathplanner/paths/Dummy.path new file mode 100644 index 0000000..24917f4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Dummy.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 7.3 + }, + "prevControl": null, + "nextControl": { + "x": 4.24998201190616, + "y": 7.302998953708963 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 7.3 + }, + "prevControl": { + "x": 2.525235378031383, + "y": 7.278544935805992 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 200.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NeutralZoneEatShoot.path b/src/main/deploy/pathplanner/paths/NeutralZoneEatShoot.path index 03fe576..c551a04 100644 --- a/src/main/deploy/pathplanner/paths/NeutralZoneEatShoot.path +++ b/src/main/deploy/pathplanner/paths/NeutralZoneEatShoot.path @@ -3,45 +3,29 @@ "waypoints": [ { "anchor": { - "x": 3.6250213980028523, - "y": 7.366704707560627 + "x": 3.6, + "y": 7.55 }, "prevControl": null, "nextControl": { - "x": 4.928500436632966, - "y": 7.24484093912391 + "x": 4.909163237417582, + "y": 7.55 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.109243937232525, - "y": 7.2890727532097 + "x": 7.002011412268189, + "y": 7.55 }, "prevControl": { - "x": 5.352969879672161, - "y": 7.3163272798495145 + "x": 6.329130291578829, + "y": 7.588820064655155 }, "nextControl": { - "x": 6.6924213677461735, - "y": 7.2680562649978135 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.769115549215405, - "y": 7.263195435092724 - }, - "prevControl": { - "x": 6.202838145377834, - "y": 7.309493962170061 - }, - "nextControl": { - "x": 7.399788857164295, - "y": 7.211631934541322 + "x": 7.633738663063232, + "y": 7.513554197069516 }, "isLocked": false, "linkedName": null @@ -96,44 +80,28 @@ }, { "anchor": { - "x": 6.269938513371728, - "y": 7.250256776034237 + "x": 5.9927960057061345, + "y": 7.55 }, "prevControl": { - "x": 6.966780503409591, - "y": 7.250256776034237 + "x": 6.689637995743998, + "y": 7.55 }, "nextControl": { - "x": 5.781907035448068, - "y": 7.250256776034237 + "x": 5.504764527782474, + "y": 7.55 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.0563654822335025, - "y": 7.288795261663903 + "x": 4.2, + "y": 7.55 }, "prevControl": { - "x": 5.306365482233503, - "y": 7.288795261663903 - }, - "nextControl": { - "x": 4.806365482233502, - "y": 7.288795261663903 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.338131979695432, - "y": 7.280862944162436 - }, - "prevControl": { - "x": 5.157654822335026, - "y": 7.290071065989848 + "x": 3.3804254279715185, + "y": 7.55 }, "nextControl": null, "isLocked": false, @@ -142,35 +110,19 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1, - "rotationDegrees": 179.64632684075352 - }, - { - "waypointRelativePos": 2, - "rotationDegrees": -97.12501634890168 + "waypointRelativePos": 2.0, + "rotationDegrees": -90.0 }, { "waypointRelativePos": 3.0, "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 4, - "rotationDegrees": -86.93191714919085 - }, - { - "waypointRelativePos": 4, - "rotationDegrees": -89.58836317312266 - }, - { - "waypointRelativePos": 6, - "rotationDegrees": 91.0846947726597 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 2.9389956184698183, - "maxWaypointRelativePos": 4, + "minWaypointRelativePos": 1.9389956184698183, + "maxWaypointRelativePos": 3.0, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 1.0, @@ -183,15 +135,31 @@ ], "pointTowardsZones": [], "eventMarkers": [ + { + "name": "deployIntake", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "deployIntake" + } + } + }, { "name": "Intake", - "waypointRelativePos": 2.3781597573306374, - "endWaypointRelativePos": 4.214357937310409, - "command": null + "waypointRelativePos": 1.93377088305489, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.5, "maxAcceleration": 3.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0, @@ -200,13 +168,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 88.74584863935306 + "rotation": -90.13877205926134 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 178.8308606720925 + "rotation": -90.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NeutralZoneNear.path b/src/main/deploy/pathplanner/paths/NeutralZoneNear.path new file mode 100644 index 0000000..8a96bbe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NeutralZoneNear.path @@ -0,0 +1,189 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6767760342368048, + "y": 0.48333808844507875 + }, + "prevControl": null, + "nextControl": { + "x": 5.080622564741996, + "y": 0.48333808844507875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.088388017118404, + "y": 0.483 + }, + "prevControl": { + "x": 5.556929967469076, + "y": 0.48300000000000004 + }, + "nextControl": { + "x": 6.551055053557106, + "y": 0.483 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.687760342368046, + "y": 0.7421112696148362 + }, + "prevControl": { + "x": 7.5066191155492135, + "y": 0.4962767475035662 + }, + "nextControl": { + "x": 7.88583404580208, + "y": 1.0109255814181655 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.75, + "y": 2.0230385164051357 + }, + "prevControl": { + "x": 7.740142443347099, + "y": 1.7732329348485283 + }, + "nextControl": { + "x": 7.761222446719625, + "y": 2.307432502460396 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.75, + "y": 3.510984308131241 + }, + "prevControl": { + "x": 7.744429370302565, + "y": 3.760922236595038 + }, + "nextControl": { + "x": 7.755570629697435, + "y": 3.261046379667444 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.639728958630526, + "y": 2.6829101283880177 + }, + "prevControl": { + "x": 7.211599748734846, + "y": 3.2384417530607883 + }, + "nextControl": { + "x": 6.186875891583452, + "y": 2.2429957203994295 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.384679029957203, + "y": 2.450014265335236 + }, + "prevControl": { + "x": 6.0977153711979195, + "y": 2.450014265335236 + }, + "nextControl": { + "x": 5.134679029957205, + "y": 2.450014265335236 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 2.45 + }, + "prevControl": { + "x": 3.4709742988286334, + "y": 2.449999999999999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.05, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.858522452921293, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 6.15, + "rotationDegrees": 45.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.4701670644391385, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 0.8, + "maxAcceleration": 0.8, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 1.8019093078758972, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 42.709389957361466 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2bfbf60..7d17a89 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,8 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.AutoShootCommand; import frc.robot.commands.DriveCommands; @@ -33,6 +35,8 @@ import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.climber.ClimberIOSim; +import frc.robot.subsystems.climber.ClimberIOTalonFX; +import frc.robot.subsystems.climber.ClimberConstants.ClimberModes; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; @@ -128,7 +132,7 @@ public RobotContainer() { intake = new Intake(new RollerIOSparkFlex(), new PivotIOSparkFlex()); shooter = new Shooter(new AnglerIOSim(), new FlywheelIOTalonFX()); carpet = new Carpet(new CarpetIOSparkFlex()); - climber = new Climber(new ClimberIOSim()); + climber = new Climber(new ClimberIOTalonFX()); feeder = new Feeder(new FeederIOSparkFlex()); vision = new Vision(drive::addVisionMeasurement, new VisionIOLimelight("limelight", drive::getRotation)); // vision = @@ -203,6 +207,8 @@ public RobotContainer() { } + registerCommands(); + // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -223,7 +229,6 @@ public RobotContainer() { "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); configureDriverBindings(); - registerCommands(); SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); RobotController.setBrownoutVoltage(6); @@ -249,6 +254,7 @@ private void configureDriverBindings() { carpet.setDefaultCommand(carpet.runCarpetCommand(CarpetModes.IDLE)); feeder.setDefaultCommand(feeder.runFeederCommand(FeederModes.IDLE)); shooter.setDefaultCommand(shooter.stop()); + climber.setDefaultCommand(climber.runClimberCommand(ClimberModes.STOW)); // shooter.setDefaultCommand(); // // Lock to 0° when A button is held // controller @@ -304,7 +310,7 @@ private void configureDriverBindings() { // controller.x().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); // controller.a().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); - controller.povUp().whileTrue(intake.runPivotVoltageCommand(-3)); + controller.povUp().whileTrue(intake.runPivotVoltageCommand(-5)); controller.povDown().whileTrue(intake.runPivotVoltageCommand(3)); //positive means down // controller.povRight().onTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); @@ -315,6 +321,9 @@ private void configureDriverBindings() { controller.start().whileTrue(new UnjamCommand(feeder, shooter)); + controller.b().whileTrue(climber.runClimberCommand(ClimberModes.CLIMB)); + controller.rightBumper().whileTrue(climber.runClimberCommand(ClimberModes.CLIMB_MINS)); + // controller.x().whileTrue(shooter.tune()); @@ -352,14 +361,13 @@ private void registerCommands() { // NamedCommands.registerCommand("AutoShoot", ); - NamedCommands.registerCommand("AutoShoot", Commands.defer( - () -> {return new AutoShootCommand(intake, carpet, feeder, shooter, drive);}, Set.of(intake, carpet, feeder, shooter))); + NamedCommands.registerCommand("AutoShoot", new ShootFuelCommandAuto(feeder, shooter, () -> getTranslationToGameElement().getNorm())); NamedCommands.registerCommand("AutoAlign", DriveCommands.joystickDriveAtAngle(drive, () -> 0, () -> 0, () -> getTranslationToGameElement().getAngle())); NamedCommands.registerCommand("Intake", intake.runIntakeCommand(IntakeMode.INTAKE)); - - NamedCommands.registerCommand("deployIntake", intake.runPivotVoltageCommand(3).withTimeout(0.5)); + NamedCommands.registerCommand("CarpetRun", carpet.runCarpetCommand(CarpetModes.INTAKE)); + NamedCommands.registerCommand("deployIntake", intake.runPivotVoltageCommand(3)); NamedCommands.registerCommand("RevShooterClose", shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE).withTimeout(0.5)); } /** diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 7a5c411..5b51865 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.climber; +import static edu.wpi.first.units.Units.Rotations; + import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.Command; @@ -29,13 +31,14 @@ public void periodic() { public void runClimberEnum(ClimberModes climberMode) { this.mode = climberMode; - climberIO.setClimberPosition(mode.positionAngle.baseUnitMagnitude()); + climberIO.setVoltage(climberMode.positionAngle.in(Rotations)); + // climberIO.setClimberPosition(mode.positionAngle.baseUnitMagnitude()); } - public Command runCarpetCommand(ClimberModes climberModes) { + public Command runClimberCommand(ClimberModes climberModes) { return new RunCommand(() -> { this.runClimberEnum(climberModes); - }, this).withName("Carpet.runCarpetEnum"); + }, this).withName("Climber.runClimberEnum"); } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java index f3b35d3..e62b3a5 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java @@ -11,7 +11,8 @@ public class ClimberConstants { public enum ClimberModes { STOW(Units.Rotation.of(0.0)), - CLIMB(Units.Rotation.of(5)); + CLIMB(Units.Rotation.of(5)), + CLIMB_MINS(Units.Rotations.of(-5)); public Angle positionAngle; @@ -22,7 +23,7 @@ public enum ClimberModes { public static final boolean attached = true; - public static final int id = -1; + public static final int id = 24; PIDConstants pidConstants = new PIDConstants(1, 0, 0); public static final double p = 1; diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index d2fdc26..f391492 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -14,6 +14,5 @@ public static class ClimberIOInputs { /** Updates the set of loggable inputs. */ public default void updateInputs(ClimberIOInputs inputs) {} - /** Run the motor to the specified position. */ - public default void setClimberPosition(double positionAngle) {} + public default void setVoltage(double voltage) {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java index 98ec497..4b2e9b0 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java @@ -40,9 +40,9 @@ public void updateInputs(ClimberIOInputs inputs) { ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); } - @Override - public void setClimberPosition(double positionAngle) { - sparkFlex.getClosedLoopController().setSetpoint((positionAngle), ControlType.kPosition, - ClosedLoopSlot.kSlot0); - } + // @Override + // public void setClimberPosition(double positionAngle) { + // sparkFlex.getClosedLoopController().setSetpoint((positionAngle), ControlType.kPosition, + // ClosedLoopSlot.kSlot0); + // } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java index 9277793..2f98ae6 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java @@ -60,9 +60,14 @@ public void updateInputs(ClimberIOInputs inputs) { inputs.currentAmps = currentAmps.getValueAsDouble(); } + // @Override + // public void setClimberPosition(double positionAngle) { + // PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); + // talon.setControl(mm); + // } + @Override - public void setClimberPosition(double positionAngle) { - PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); - talon.setControl(mm); - } + public void setVoltage(double voltage) { + talon.setVoltage(voltage); + } }