From f848d95e55103f5978d1afdc66023ab4d53096ba Mon Sep 17 00:00:00 2001 From: Jack Cammarata Date: Sat, 2 Mar 2024 21:26:34 -0500 Subject: [PATCH] gym stuff --- .pathplanner/settings.json | 2 +- .../pathplanner/autos/Amp Side Shoot 2.auto | 154 ++++++++++++++++++ .../pathplanner/autos/Center Shoot 2.auto | 154 ++++++++++++++++++ .../deploy/pathplanner/autos/New Auto.auto | 31 ---- .../deploy/pathplanner/autos/TestAuto.auto | 25 --- .../paths/Amp Side Shoot 2 Get Note.path | 52 ++++++ ...Test.path => Center Shoot 2 Get Note.path} | 33 ++-- .../deploy/pathplanner/paths/New Path.path | 65 -------- .../Return Amp Side Shoot 2 Get Note.path | 76 +++++++++ .../paths/Return Center Shoot 2 Get Note.path | 76 +++++++++ src/main/java/frc/robot/Constants.java | 5 +- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 38 ++++- .../robot/commands/intake/SetIntakeState.java | 2 +- .../robot/commands/shooter/WaitForNote.java | 30 ++++ src/main/java/frc/robot/subsystems/Arm.java | 5 +- .../java/frc/robot/subsystems/Shooter.java | 3 +- .../subsystems/cameras/AprilTagCamera.java | 2 +- .../cameras/NoteDetectorCamera.java | 7 +- .../robot/subsystems/drive/SwerveDrive.java | 52 ++++-- 20 files changed, 643 insertions(+), 170 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Amp Side Shoot 2.auto create mode 100644 src/main/deploy/pathplanner/autos/Center Shoot 2.auto delete mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/TestAuto.auto create mode 100644 src/main/deploy/pathplanner/paths/Amp Side Shoot 2 Get Note.path rename src/main/deploy/pathplanner/paths/{Test.path => Center Shoot 2 Get Note.path} (54%) delete mode 100644 src/main/deploy/pathplanner/paths/New Path.path create mode 100644 src/main/deploy/pathplanner/paths/Return Amp Side Shoot 2 Get Note.path create mode 100644 src/main/deploy/pathplanner/paths/Return Center Shoot 2 Get Note.path create mode 100644 src/main/java/frc/robot/commands/shooter/WaitForNote.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 0905f9b..a6a7c65 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,5 +1,5 @@ { - "robotWidth": 0.864, + "robotWidth": 0.86, "robotLength": 0.86, "holonomicMode": true, "pathFolders": [], diff --git a/src/main/deploy/pathplanner/autos/Amp Side Shoot 2.auto b/src/main/deploy/pathplanner/autos/Amp Side Shoot 2.auto new file mode 100644 index 0000000..278a34d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Amp Side Shoot 2.auto @@ -0,0 +1,154 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.24, + "y": 6.66 + }, + "rotation": 50.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "DeployIntake" + } + }, + { + "type": "named", + "data": { + "name": "LiftArmToDeployDemonHorns" + } + }, + { + "type": "named", + "data": { + "name": "RampShooterForManualShot" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SetShootFromSubwoofer" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "named", + "data": { + "name": "SetArmAndShooterForIntake" + } + }, + { + "type": "named", + "data": { + "name": "StartIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Amp Side Shoot 2 Get Note" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "HasNote" + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "path", + "data": { + "pathName": "Return Amp Side Shoot 2 Get Note" + } + }, + { + "type": "named", + "data": { + "name": "SetShootFromSubwoofer" + } + }, + { + "type": "named", + "data": { + "name": "RampShooterForManualShot" + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Shoot 2.auto b/src/main/deploy/pathplanner/autos/Center Shoot 2.auto new file mode 100644 index 0000000..89d51da --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Shoot 2.auto @@ -0,0 +1,154 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.42, + "y": 5.527514849608872 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RampShooterForManualShot" + } + }, + { + "type": "named", + "data": { + "name": "DeployIntake" + } + }, + { + "type": "named", + "data": { + "name": "LiftArmToDeployDemonHorns" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SetShootFromSubwoofer" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "named", + "data": { + "name": "SetArmAndShooterForIntake" + } + }, + { + "type": "named", + "data": { + "name": "StartIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Center Shoot 2 Get Note" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "HasNote" + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "path", + "data": { + "pathName": "Return Center Shoot 2 Get Note" + } + }, + { + "type": "named", + "data": { + "name": "SetShootFromSubwoofer" + } + }, + { + "type": "named", + "data": { + "name": "RampShooterForManualShot" + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto deleted file mode 100644 index bb00b28..0000000 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 2.0, - "y": 7.0 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "New Path" - } - }, - { - "type": "named", - "data": { - "name": "print" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TestAuto.auto b/src/main/deploy/pathplanner/autos/TestAuto.auto deleted file mode 100644 index 80c0c54..0000000 --- a/src/main/deploy/pathplanner/autos/TestAuto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 2, - "y": 7.0 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Test" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Amp Side Shoot 2 Get Note.path b/src/main/deploy/pathplanner/paths/Amp Side Shoot 2 Get Note.path new file mode 100644 index 0000000..4720067 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Amp Side Shoot 2 Get Note.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.24, + "y": 6.66 + }, + "prevControl": null, + "nextControl": { + "x": 2.2040432097982583, + "y": 6.733731013824367 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5905008158090483, + "y": 6.792285196553275 + }, + "prevControl": { + "x": 1.5950797094176201, + "y": 6.722020177278586 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 29.898901838614496, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 32.87927139602872, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Test.path b/src/main/deploy/pathplanner/paths/Center Shoot 2 Get Note.path similarity index 54% rename from src/main/deploy/pathplanner/paths/Test.path rename to src/main/deploy/pathplanner/paths/Center Shoot 2 Get Note.path index 29bebe7..78ff2b2 100644 --- a/src/main/deploy/pathplanner/paths/Test.path +++ b/src/main/deploy/pathplanner/paths/Center Shoot 2 Get Note.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 1.36086297850199, + "y": 5.574358195791997 }, "prevControl": null, "nextControl": { - "x": 3.1526250858218794, - "y": 7.0 + "x": 2.36086297850199, + "y": 5.574358195791997 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.932668124965351, - "y": 7.0 + "x": 2.5, + "y": 5.574358195791997 }, "prevControl": { - "x": 3.5184545625922556, - "y": 7.0 + "x": 1.5, + "y": 5.574358195791997 }, "nextControl": null, "isLocked": false, @@ -32,18 +32,21 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 3.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0 }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": 0, "rotateFast": false }, "reversed": false, "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false + "previewStartingState": { + "rotation": 0, + "velocity": 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 deleted file mode 100644 index df1ea71..0000000 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ /dev/null @@ -1,65 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.3868377009221913, - "y": 6.991369417831561 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.745294740232846, - "y": 7.0 - }, - "prevControl": { - "x": 3.3517051912848466, - "y": 7.049923600560469 - }, - "nextControl": { - "x": 6.056291615146893, - "y": 6.953035178565569 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.834402538990526, - "y": 5.644623215066687 - }, - "prevControl": { - "x": 5.822691702444745, - "y": 7.190453639109847 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Return Amp Side Shoot 2 Get Note.path b/src/main/deploy/pathplanner/paths/Return Amp Side Shoot 2 Get Note.path new file mode 100644 index 0000000..fa11ac0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Return Amp Side Shoot 2 Get Note.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.59, + "y": 6.79 + }, + "prevControl": null, + "nextControl": { + "x": 1.9812722961537204, + "y": 6.67517683109546 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.24, + "y": 6.66 + }, + "prevControl": { + "x": 2.110356517432006, + "y": 6.75715268691593 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "PrepareShooter", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SetShootFromSubwoofer" + } + }, + { + "type": "named", + "data": { + "name": "RampShooterForManualShot" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 44.97316285798226, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 32.87927139602872, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Return Center Shoot 2 Get Note.path b/src/main/deploy/pathplanner/paths/Return Center Shoot 2 Get Note.path new file mode 100644 index 0000000..53498c2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Return Center Shoot 2 Get Note.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.4, + "y": 5.574358195791997 + }, + "prevControl": null, + "nextControl": { + "x": 1.8644289499705946, + "y": 5.58606903233778 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.36, + "y": 5.574358195791997 + }, + "prevControl": { + "x": 2.157199863615132, + "y": 5.597779868883561 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "PrepareShooter", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SetShootFromSubwoofer" + } + }, + { + "type": "named", + "data": { + "name": "RampShooterForManualShot" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ 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 a8e080a..2a522de 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -42,9 +42,8 @@ public class Constants { public static final double DRIVE_Y_I = 0; public static final double DRIVE_Y_D = 0; - // TODO: cammera offsets - public static final Transform3d FRONT_APRILTAG_CAMERA_OFFSET = new Transform3d(0, 0, 0.7112, new Rotation3d(0, Math.toRadians(-20), 0)); - public static final Transform3d REAR_APRILTAG_CAMERA_OFFSET = new Transform3d(0, 0, 0, new Rotation3d()); + public static final Transform3d FRONT_APRILTAG_CAMERA_OFFSET = new Transform3d(Units.inchesToMeters(7.6882), Units.inchesToMeters(-13.5080), Units.inchesToMeters(12.1545), new Rotation3d(0, Math.toRadians(60), Math.toDegrees(0))); + public static final Transform3d REAR_APRILTAG_CAMERA_OFFSET = new Transform3d(Units.inchesToMeters(-6.6882), Units.inchesToMeters(-12.9915), Units.inchesToMeters(8.5545), new Rotation3d(0, Math.toRadians(60), Math.toRadians(180))); public static final Transform3d INTAKE_NOTE_CAMERA_OFFSET = new Transform3d(Units.inchesToMeters(7.8971), Units.inchesToMeters(-12.99815), Units.inchesToMeters(18.3493), new Rotation3d(0, Math.toRadians(-20), Math.toRadians(20))); public static final double INTAKE_NOTE_CAMERA_HFOV = 53; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f2e8ed1..023c537 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -94,6 +94,7 @@ public void disabledPeriodic() { RobotContainer.intake.setSetpoint(RobotContainer.intake.getPosition()); RobotContainer.arm.setSetpoint(RobotContainer.arm.getPosition()); RobotContainer.shooter.setSetpoint(RobotContainer.shooter.getPosition()); + RobotContainer.shooter.stopShooting(); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bf17710..ccbd87d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.SerialPort.Port; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -35,6 +36,7 @@ import frc.robot.commands.shooter.RampShooter; import frc.robot.commands.shooter.SetShooterState; import frc.robot.commands.shooter.ShootSpeaker; +import frc.robot.commands.shooter.WaitForNote; import frc.robot.hid.OperatorStick; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Intake; @@ -142,8 +144,8 @@ private void configureXboxControllerBindings() { Trigger resetHeading = driverController.start().debounce(1); resetHeading.onTrue(new InstantCommand(() -> { RobotContainer.swerveDrive.resetNavx(); - var pose = RobotContainer.swerveDrive.getPose(); - RobotContainer.swerveDrive.setPose(new Pose2d(pose.getX(), pose.getY(), Rotation2d.fromDegrees(0))); + var isRedAlliance = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; + RobotContainer.swerveDrive.setPose(new Pose2d(8, 4, isRedAlliance ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0))); }, RobotContainer.swerveDrive)); // go slow is an exception - doesn't really need to "require" the swerve drive @@ -160,6 +162,9 @@ private void configureXboxControllerBindings() { Trigger autoIntakeNote = driverController.leftBumper(); autoIntakeNote.whileTrue(new AutoIntake()); + + Trigger autoShootNote = driverController.rightBumper(); + autoShootNote.whileTrue(new ShootSpeaker()); } /** Binds commands to the operator stick. */ @@ -168,7 +173,7 @@ private void configureOperatorStickBindings() { Trigger scoreSpeaker = new Trigger(operatorStick::scoreSpeaker); // scoreSpeaker.onTrue(new ShootSpeaker()); scoreSpeaker.onTrue( - new RampShooter(() -> 16) + new RampShooter(() -> 15) .andThen( new InstantCommand(() -> RobotContainer.shooter.feed(), RobotContainer.shooter) ) @@ -204,17 +209,24 @@ private void configureOperatorStickBindings() { Trigger retractIntake = new Trigger(() -> operatorStick.retractIntake() && RobotContainer.arm.isArmClearingIntake()); retractIntake.onTrue(new SetIntakeState(IntakeState.kUp)); - Trigger intakeNoteSetpoint = new Trigger(() -> operatorStick.intakeNoteSetpoint() && RobotContainer.intake.isIntakeDown()); - intakeNoteSetpoint.onTrue(new SetShooterState(ShooterState.kIntake).alongWith(new SetArmState(ArmState.kIntake))); + Trigger intakeNoteSetpoint = new Trigger(() -> operatorStick.intakeNoteSetpoint()); + intakeNoteSetpoint.onTrue( + new SetIntakeState(IntakeState.kDown) + .andThen(new SetShooterState(ShooterState.kIntake).alongWith(new SetArmState(ArmState.kIntake))) + ); Trigger ampSetpoint = new Trigger(operatorStick::ampSetpoint); ampSetpoint.onTrue(new SetArmState(ArmState.kAmp).alongWith(new SetShooterState(ShooterState.kAmp))); + // TODO: test subwoofer shot for auto on this button Trigger shootSetpoint = new Trigger(() -> operatorStick.shootSetpoint() && RobotContainer.intake.isIntakeDown()); shootSetpoint.onTrue(new SetArmState(ArmState.kShoot).alongWith(new SetShooterState(ShooterState.kShoot))); Trigger stowSetpoint = new Trigger(operatorStick::stowSetpoint); - stowSetpoint.onTrue(new SetArmState(ArmState.kStow).alongWith(new SetShooterState(ShooterState.kIntake))); + stowSetpoint.onTrue( + (new SetArmState(ArmState.kStow).alongWith(new SetShooterState(ShooterState.kIntake))) + .andThen(new SetIntakeState(IntakeState.kUp)) + ); Trigger scoreAmp = new Trigger(operatorStick::scoreAmp); scoreAmp.onTrue( @@ -269,6 +281,20 @@ private void registerNamedCommands() { NamedCommands.registerCommand("AutoPickupNote", new AutoPickupNote().withTimeout(5)); NamedCommands.registerCommand("SetArmForScore", new SetArmState(ArmState.kShoot)); NamedCommands.registerCommand("LaunchSpeaker", new ShootSpeaker()); + + NamedCommands.registerCommand("SetShootFromSubwoofer", new SetArmState(ArmState.kShootFromSubwoofer).alongWith(new SetShooterState(ShooterState.kShootFromSubwoofer))); + NamedCommands.registerCommand("RampShooterForManualShot", new RampShooter(() -> 15)); + NamedCommands.registerCommand("Feed", new InstantCommand(() -> shooter.feed(), shooter)); + NamedCommands.registerCommand("Stop", new InstantCommand(() -> { + shooter.stopShooting(); + shooter.stopIndexer(); + intake.stop(); + }, shooter, intake)); + NamedCommands.registerCommand("HasNote", new WaitForNote().withTimeout(3)); + NamedCommands.registerCommand("StartIntake", new InstantCommand(() -> { + intake.intake(); + shooter.index(); + }, intake, shooter)); } private void configurePathPlannerLogging() { diff --git a/src/main/java/frc/robot/commands/intake/SetIntakeState.java b/src/main/java/frc/robot/commands/intake/SetIntakeState.java index 6e91ad4..d51453a 100644 --- a/src/main/java/frc/robot/commands/intake/SetIntakeState.java +++ b/src/main/java/frc/robot/commands/intake/SetIntakeState.java @@ -34,6 +34,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return Math.abs(RobotContainer.intake.getPosition() - intakeState.rotations) <= 0.025; + return Math.abs(RobotContainer.intake.getPosition() - intakeState.rotations) <= 0.04; } } diff --git a/src/main/java/frc/robot/commands/shooter/WaitForNote.java b/src/main/java/frc/robot/commands/shooter/WaitForNote.java new file mode 100644 index 0000000..7c0f295 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/WaitForNote.java @@ -0,0 +1,30 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class WaitForNote extends Command { + private final Timer timer = new Timer(); + public WaitForNote() { + addRequirements(RobotContainer.shooter, RobotContainer.intake, RobotContainer.arm); + } + + @Override + public void end(boolean inter) { + timer.stop(); + timer.reset(); + } + + @Override + public boolean isFinished() { + var hasNote = RobotContainer.shooter.hasNote(); + if (hasNote) { + timer.start(); + } else { + timer.reset(); + } + + return hasNote && timer.hasElapsed(0.06); + } +} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index ffdef16..4d3b60b 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -73,9 +73,10 @@ public boolean isArmClearingIntake() { public enum ArmState { kStow(0.618), - kDeployDemonHorns(0.595), - kIntake(0.83), + kDeployDemonHorns(0.605), + kIntake(0.81), kShoot(0.75), + kShootFromSubwoofer(0.81), kAmp(0.52); public final double rotations; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index c3bb451..da5a142 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -378,8 +378,9 @@ public boolean hasNote() { } public enum ShooterState { - kIntake(0.37), + kIntake(0.395), kShoot(0.4), + kShootFromSubwoofer(0.41), kAmp(0.425); public final double rotations; diff --git a/src/main/java/frc/robot/subsystems/cameras/AprilTagCamera.java b/src/main/java/frc/robot/subsystems/cameras/AprilTagCamera.java index 1bcfbfe..4c7d1df 100644 --- a/src/main/java/frc/robot/subsystems/cameras/AprilTagCamera.java +++ b/src/main/java/frc/robot/subsystems/cameras/AprilTagCamera.java @@ -30,7 +30,7 @@ public class AprilTagCamera extends SubsystemBase { public AprilTagCamera(String cameraName, Transform3d cameraOffset) { camera = new PhotonCamera(cameraName); - poseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, + poseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.AVERAGE_BEST_TARGETS, camera, cameraOffset); } diff --git a/src/main/java/frc/robot/subsystems/cameras/NoteDetectorCamera.java b/src/main/java/frc/robot/subsystems/cameras/NoteDetectorCamera.java index 534f76f..dbe54ca 100644 --- a/src/main/java/frc/robot/subsystems/cameras/NoteDetectorCamera.java +++ b/src/main/java/frc/robot/subsystems/cameras/NoteDetectorCamera.java @@ -52,7 +52,7 @@ public double distanceToNote(Note note) { public Translation2d estimateNotePose(PhotonTrackedTarget target) { Pose2d robotPose = RobotContainer.swerveDrive.getPose(); // phi is the angle on the xy plane from the x axis - double phi = -Math.toRadians(target.getYaw()); + double phi = -Math.toRadians(target.getYaw() + 20); // theta is the angle from the z axis double theta = (Math.PI / 2) - cameraOffset.getRotation().getY() - Math.toRadians(target.getPitch()); @@ -68,9 +68,8 @@ public Translation2d estimateNotePose(PhotonTrackedTarget target) { // find x and y by multiplying the x and y components of the unit vector by t Translation2d noteCameraRelative = new Translation2d(targetVector.getX() * t, targetVector.getY() * t); - Translation2d noteRobotRelative = noteCameraRelative.plus(cameraOffset.getTranslation().toTranslation2d()); - Translation2d noteFieldRelative = noteRobotRelative.rotateBy(robotPose.getRotation()) - .plus(robotPose.getTranslation()); + Translation2d noteRobotRelative = noteCameraRelative.plus(cameraOffset.getTranslation().toTranslation2d().rotateBy(robotPose.getRotation())); + Translation2d noteFieldRelative = noteRobotRelative.rotateBy(robotPose.getRotation()).plus(robotPose.getTranslation()); return noteFieldRelative; } diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index 53f68a4..752e1da 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -139,7 +139,7 @@ public class SwerveDrive extends SubsystemBase { new SwerveModulePosition(0, new Rotation2d(rearRight.getAngle())) }, new Pose2d(0, 0, Rotation2d.fromDegrees(0)), VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.3)); + VecBuilder.fill(6, 6, 100000)); /** * Constructs Swerve Drive @@ -152,7 +152,7 @@ public SwerveDrive() { anglePid.setTolerance(1); xPid.setTolerance(0.1); yPid.setTolerance(0.1); - } + } /** * Returns the angle of the robot as a Rotation2d as read by the navx. @@ -199,7 +199,7 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ } else { var swerveModuleStates = kinematics .toSwerveModuleStates( - fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, getPose().getRotation()) + fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, getAngle()) : new ChassisSpeeds(xSpeed, ySpeed, rot)); frontLeft.setDesiredState(swerveModuleStates[0]); @@ -222,7 +222,7 @@ public void setModuleStates(ChassisSpeeds chassisSpeeds) { vX = chassisSpeeds.vxMetersPerSecond; vY = chassisSpeeds.vyMetersPerSecond; setModuleStates(kinematics.toSwerveModuleStates( - ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, poseEstimator.getEstimatedPosition().getRotation()))); + ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, getAngle()))); } public void setPose(Pose2d pose) { @@ -238,7 +238,7 @@ public void rotateToAngleInPlace(double setAngle) { } public void holdAngleWhileDriving(double x, double y, Rotation2d setAngle, boolean fieldOriented) { - var rotateOutput = MathUtil.clamp(anglePid.calculate(poseEstimator.getEstimatedPosition().getRotation().getDegrees(), normalizeAngle(setAngle.getDegrees())), -1, 1) * kMaxAngularSpeedRadiansPerSecond; + var rotateOutput = MathUtil.clamp(anglePid.calculate(getAngle().getDegrees(), normalizeAngle(setAngle.getDegrees())), -1, 1) * kMaxAngularSpeedRadiansPerSecond; this.drive(x, y, rotateOutput, fieldOriented); } @@ -250,7 +250,7 @@ public void goToPose(Pose2d target, boolean fieldOriented) { Pose2d pose = getPose(); double xSpeed = MathUtil.clamp(xPid.calculate(pose.getX(), target.getX()), -1, 1) * kMaxSpeedMetersPerSecond; double ySpeed = MathUtil.clamp(yPid.calculate(pose.getY(), target.getY()), -1, 1) * kMaxSpeedMetersPerSecond; - double vTheta = MathUtil.clamp(anglePid.calculate(pose.getRotation().getDegrees(), normalizeAngle(target.getRotation().getDegrees())), -1, 1) * kMaxAngularSpeedRadiansPerSecond; + double vTheta = MathUtil.clamp(anglePid.calculate(getAngle().getDegrees(), normalizeAngle(target.getRotation().getDegrees())), -1, 1) * kMaxAngularSpeedRadiansPerSecond; this.drive(xSpeed, ySpeed, vTheta, fieldOriented); } @@ -276,25 +276,42 @@ public void updateOdometry() { poseEstimator.updateWithTime(Timer.getFPGATimestamp(), getAngle(), modulePositions); // vision - updateOdometryUsingFrontCamera(); - updateOdometryUsingRearCamera(); + //updateOdometryUsingFrontCamera(); + //updateOdometryUsingRearCamera(); } private void updateOdometryUsingFrontCamera() { Optional estimatedFrontPose = RobotContainer.frontAprilTagCamera.getEstimatedGlobalPose(); if (estimatedFrontPose.isPresent()) { - var stdDevs = VecBuilder.fill(0.9, 0.9, 0.3); - var ambiguity = 0.2; - updateOdometryUsingVisionMeasurement(estimatedFrontPose.get(), stdDevs, ambiguity); + double stdDevXY = 0, distance = 0; + for (var target : estimatedFrontPose.get().targetsUsed) { + distance = target.getBestCameraToTarget().getTranslation().getNorm(); + stdDevXY = distance / 2.0; + } + + var stdDevs = VecBuilder.fill(stdDevXY, stdDevXY, 100000); + var ambiguity = 0.5; + + if (distance < 3) { + updateOdometryUsingVisionMeasurement(estimatedFrontPose.get(), stdDevs, ambiguity); + } } } private void updateOdometryUsingRearCamera() { Optional estimatedRearPose = RobotContainer.rearAprilTagCamera.getEstimatedGlobalPose(); if (estimatedRearPose.isPresent()) { - var stdDevs = VecBuilder.fill(0.9, 0.9, 0.3); - var ambiguity = 0.2; - updateOdometryUsingVisionMeasurement(estimatedRearPose.get(), stdDevs, ambiguity); + double stdDevXY = 0, distance = 0; + for (var target : estimatedRearPose.get().targetsUsed) { + distance = target.getBestCameraToTarget().getTranslation().getNorm(); + stdDevXY = distance / 4.0; + } + + var stdDevs = VecBuilder.fill(stdDevXY, stdDevXY, 100000); + var ambiguity = 1; + if (distance < 4) { + updateOdometryUsingVisionMeasurement(estimatedRearPose.get(), stdDevs, ambiguity); + } } } @@ -304,7 +321,7 @@ private void updateOdometryUsingVisionMeasurement(EstimatedRobotPose ePose, Matr boolean goodMeasurements = true; for (PhotonTrackedTarget t : targetsUsed) { - if (t.getPoseAmbiguity() < ambiguity) { + if (t.getPoseAmbiguity() > ambiguity) { goodMeasurements = false; break; } @@ -395,6 +412,11 @@ public void periodic() { SmartDashboard.putNumber("Navx-Roll", roll); SmartDashboard.putNumber("Navx-Pitch", pitch); + SmartDashboard.putNumber("Pose-X", pose.getX()); + SmartDashboard.putNumber("Pose-Y", pose.getY()); + SmartDashboard.putNumber("Pose-Degrees", pose.getRotation().getDegrees()); + + if (RobotState.isEnabled()) { var chassis = getChassisSpeeds(); var speed = Math.hypot(chassis.vxMetersPerSecond, chassis.vyMetersPerSecond);