diff --git a/src/main/deploy/pathplanner/autos/Depot Center Score.auto b/src/main/deploy/pathplanner/autos/1 Pass Depot to Mid.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Depot Center Score.auto rename to src/main/deploy/pathplanner/autos/1 Pass Depot to Mid.auto diff --git a/src/main/deploy/pathplanner/autos/1 Pass Outpost to Mid.auto b/src/main/deploy/pathplanner/autos/1 Pass Outpost to Mid.auto new file mode 100644 index 0000000..5dfeacf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 Pass Outpost to Mid.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "deployIntake" + } + }, + { + "type": "named", + "data": { + "name": "enableIntake" + } + }, + { + "type": "named", + "data": { + "name": "startFlywheel" + } + }, + { + "type": "path", + "data": { + "pathName": "Pass 1 - Outpost to Mid" + } + }, + { + "type": "named", + "data": { + "name": "requestShooting20S" + } + }, + { + "type": "named", + "data": { + "name": "stopFlywheel" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Pass Outpost to Mid.auto b/src/main/deploy/pathplanner/autos/2 Pass Outpost to Mid.auto new file mode 100644 index 0000000..14425a4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Pass Outpost to Mid.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "deployIntake" + } + }, + { + "type": "named", + "data": { + "name": "enableIntake" + } + }, + { + "type": "named", + "data": { + "name": "startFlywheel" + } + }, + { + "type": "path", + "data": { + "pathName": "Pass 1 - Outpost to Mid" + } + }, + { + "type": "named", + "data": { + "name": "requestShooting5S" + } + }, + { + "type": "path", + "data": { + "pathName": "Pass 2 - Outpost to Mid" + } + }, + { + "type": "named", + "data": { + "name": "requestShooting20S" + } + }, + { + "type": "named", + "data": { + "name": "stopFlywheel" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Outpost Center Score.auto b/src/main/deploy/pathplanner/autos/Outpost Center Score.auto index 0c1833e..ae9cb19 100644 --- a/src/main/deploy/pathplanner/autos/Outpost Center Score.auto +++ b/src/main/deploy/pathplanner/autos/Outpost Center Score.auto @@ -13,13 +13,13 @@ { "type": "path", "data": { - "pathName": "Outpost Center Path + Return" + "pathName": "Pass 1 - Outpost to Mid" } }, { "type": "named", "data": { - "name": "requestShooting15S" + "name": "requestShooting10S" } } ] diff --git a/src/main/deploy/pathplanner/paths/Outpost Center Path + Return.path b/src/main/deploy/pathplanner/paths/Pass 1 - Outpost to Mid.path similarity index 55% rename from src/main/deploy/pathplanner/paths/Outpost Center Path + Return.path rename to src/main/deploy/pathplanner/paths/Pass 1 - Outpost to Mid.path index c2bddf9..76a69e7 100644 --- a/src/main/deploy/pathplanner/paths/Outpost Center Path + Return.path +++ b/src/main/deploy/pathplanner/paths/Pass 1 - Outpost to Mid.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.320596026490066, + "x": 3.9938990066225166, "y": 0.666390728476822 }, "prevControl": null, "nextControl": { - "x": 5.320596026490067, + "x": 4.9938990066225175, "y": 0.666390728476822 }, "isLocked": false, @@ -32,44 +32,44 @@ }, { "anchor": { - "x": 7.391548013245033, - "y": 2.9823096026490052 + "x": 7.093890728476822, + "y": 3.301746688741722 }, "prevControl": { - "x": 8.052817700960428, - "y": 2.9877298459909336 + "x": 7.755160416192217, + "y": 3.3071669320836503 }, "nextControl": { - "x": 6.5058360927152314, - "y": 2.9750496688741723 + "x": 6.208178807947021, + "y": 3.294486754966889 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.498576158940397, - "y": 1.3415645695364242 + "x": 6.476796357615894, + "y": 1.101986754966888 }, "prevControl": { - "x": 6.9269122516556285, - "y": 2.2127566225165562 + "x": 6.905132450331125, + "y": 1.9731788079470203 }, "nextControl": { - "x": 6.214810484806516, - "y": 0.7644140458742933 + "x": 6.193030683482013, + "y": 0.5248362313047571 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.320596026490066, + "x": 3.7035016556291387, "y": 0.666390728476822 }, "prevControl": { - "x": 5.796368851244969, - "y": 0.8582092889088078 + "x": 5.206307947019867, + "y": 0.7172102649006615 }, "nextControl": null, "isLocked": false, @@ -77,22 +77,34 @@ } ], "rotationTargets": [ + { + "waypointRelativePos": 0.3099273607748189, + "rotationDegrees": 0.0 + }, { "waypointRelativePos": 0.9539951573849952, - "rotationDegrees": 117.40360625563348 + "rotationDegrees": 98.80231741603863 + }, + { + "waypointRelativePos": 1.8983050847457779, + "rotationDegrees": 106.53481995772754 }, { "waypointRelativePos": 2.6963635575589464, - "rotationDegrees": 89.95862437106403 + "rotationDegrees": 87.79015138447025 + }, + { + "waypointRelativePos": 3.675544794188865, + "rotationDegrees": 2.2136983625309083 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 1.2071651090342688, - "maxWaypointRelativePos": 1.8885319314641746, + "minWaypointRelativePos": 1.1646643109540615, + "maxWaypointRelativePos": 2.131448763250887, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.2, "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -102,48 +114,7 @@ } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "startFlywheel", - "waypointRelativePos": 0.3279151943462853, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "startFlywheel" - } - } - }, - { - "name": "enableIntake", - "waypointRelativePos": 0.38445229681977794, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "enableIntake" - } - } - }, - { - "name": "disableIntake", - "waypointRelativePos": 2.137102473498207, - "endWaypointRelativePos": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "disableIntake" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Pass 2 - Outpost to Mid.path b/src/main/deploy/pathplanner/paths/Pass 2 - Outpost to Mid.path new file mode 100644 index 0000000..4caa88c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Pass 2 - Outpost to Mid.path @@ -0,0 +1,137 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.805140728476821, + "y": 0.666390728476822 + }, + "prevControl": null, + "nextControl": { + "x": 4.805140728476822, + "y": 0.666390728476822 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.1573592715231795, + "y": 1.0802069536423846 + }, + "prevControl": { + "x": 5.9890401792156975, + "y": 0.635363638258319 + }, + "nextControl": { + "x": 6.258998344370861, + "y": 1.3488245033112594 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.794362582781457, + "y": 2.270836092715232 + }, + "prevControl": { + "x": 5.935691178086882, + "y": 2.047536912132661 + }, + "nextControl": { + "x": 5.431365894039736, + "y": 2.8443708609271505 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.1573592715231795, + "y": 1.2181456953642373 + }, + "prevControl": { + "x": 6.309817880794703, + "y": 1.4577235099337735 + }, + "nextControl": { + "x": 5.81207457608011, + "y": 0.6755554596679854 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.805140728476821, + "y": 0.666390728476822 + }, + "prevControl": { + "x": 5.409586092715232, + "y": 0.7099503311258286 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4503631961259079, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.2493946731235022, + "rotationDegrees": 106.73738567187311 + }, + { + "waypointRelativePos": 1.961259079903153, + "rotationDegrees": 106.53481995772754 + }, + { + "waypointRelativePos": 2.6246973365617308, + "rotationDegrees": 87.79015138447025 + }, + { + "waypointRelativePos": 3.767554479418897, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.1646643109540615, + "maxWaypointRelativePos": 2.131448763250887, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "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.24014922069523068 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 07feafe..a9c2711 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -16,6 +16,7 @@ import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -39,7 +40,6 @@ public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; - private static final boolean IS_PRACTICE = true; private static final String LOG_DIRECTORY = "/home/lvuser/logs"; private static final long MIN_FREE_SPACE = diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d25049..2c92ac2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -88,7 +88,14 @@ public RobotContainer() { NamedCommands.registerCommand("disableIntake", new SetIntakeState(intake, false)); NamedCommands.registerCommand("startFlywheel", new SetFlywheelState(shooter, true)); NamedCommands.registerCommand("stopFlywheel", new SetFlywheelState(shooter, false)); + NamedCommands.registerCommand("requestShooting5S", new RequestShootingForSeconds(shooter, true, 5.0)); + NamedCommands.registerCommand("requestShooting6S", new RequestShootingForSeconds(shooter, true, 6.0)); + NamedCommands.registerCommand("requestShooting7S", new RequestShootingForSeconds(shooter, true, 7.0)); + NamedCommands.registerCommand("requestShooting8S", new RequestShootingForSeconds(shooter, true, 8.0)); + NamedCommands.registerCommand("requestShooting9S", new RequestShootingForSeconds(shooter, true, 9.0)); NamedCommands.registerCommand("requestShooting10S", new RequestShootingForSeconds(shooter, true, 10.0)); + NamedCommands.registerCommand("requestShooting11S", new RequestShootingForSeconds(shooter, true, 11.0)); + NamedCommands.registerCommand("requestShooting12S", new RequestShootingForSeconds(shooter, true, 12.0)); NamedCommands.registerCommand("requestShooting15S", new RequestShootingForSeconds(shooter, true, 15.0)); NamedCommands.registerCommand("requestShooting20S", new RequestShootingForSeconds(shooter, true, 20.0)); NamedCommands.registerCommand("stopShooting", new RequestShootingForSeconds(shooter, false, 0.0)); diff --git a/src/main/java/frc/robot/commands/RequestShootingForSeconds.java b/src/main/java/frc/robot/commands/RequestShootingForSeconds.java index 436710c..c9eaf87 100644 --- a/src/main/java/frc/robot/commands/RequestShootingForSeconds.java +++ b/src/main/java/frc/robot/commands/RequestShootingForSeconds.java @@ -41,6 +41,8 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { this.shooter.setActuateHoodAndLaunch(false); - this.shooter.setDriverSpinUpFlywheel(false); + if (interrupted) { + this.shooter.setDriverSpinUpFlywheel(false); + } } } diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java index 29fec7d..b4d8ecd 100644 --- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java @@ -26,7 +26,9 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; @@ -65,6 +67,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); private double robotOmegaDegPerSec = 0.0; + private Field2d field = new Field2d(); /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( @@ -268,6 +271,9 @@ public void periodic() { this.robotOmegaDegPerSec = this.getState().Speeds.omegaRadiansPerSecond * 180.0 / Math.PI; + + this.field.setRobotPose(getPose()); + SmartDashboard.putData(field); } private void startSimThread() { diff --git a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java index 4faa1f2..9c55703 100644 --- a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java +++ b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java @@ -39,21 +39,21 @@ public static List getCameras() { ) ); - // registerCamera( - // new Camera( - // "right", - // new Transform3d( - // Inches.of(-1.5), - // Inches.of(-13.75), - // Inches.of(8.625), - // new Rotation3d( - // Degrees.of(0), - // Degrees.of(-65), // negative pitch = tilted upward - // Degrees.of(-90) - // ) - // ) - // ) - // ); + registerCamera( + new Camera( + "right", + new Transform3d( + Inches.of(-1.5), + Inches.of(-13.75), + Inches.of(8.625), + new Rotation3d( + Degrees.of(0), + Degrees.of(-65), // negative pitch = tilted upward + Degrees.of(-90) + ) + ) + ) + ); registerCamera( new Camera(