Rev a alliance auton (#113)

* driverOI handlePre

* changed autons to old shootNow

* adjusted auton path

* re-added DriverShootVelocityIntake in driverOI
changed auton to shoot off of subwoofer for first shot

* added timeout to auto intake and removed incorrect context ownership

* resets gyro to 180 degrees of current after auton is done

* Spotless

* stop shooter at the end of an auton and other context stuff

* comments and used constants

* removed logging and deleted commented out stuff

src/main/deploy/pathplanner/paths/3 Piece 1 Stop 1.path

@@ -0,0 +1,58 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7483342975945213,
+        "y": 6.675397934129203
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.406194437388875,
+        "y": 7.089841508386721
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.1015855040969345,
+        "y": 6.522126854438895
+      },
+      "prevControl": {
+        "x": 1.7969436448132157,
+        "y": 6.6332944101073394
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "3 Piece 1 Stop 1"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.2,
+      "rotationDegrees": 37.38608672087997,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 35.26397392088418,
+    "rotateFast": true
+  },
+  "reversed": false,
+  "folder": "RevA",
+  "previewStartingState": {
+    "rotation": 59.08161572041965,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/3 Piece 1 Stop 2.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.1015855040969345,
+        "y": 6.522126854438895
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.328968287566205,
+        "y": 6.561759988334791
+      },
+      "isLocked": false,
+      "linkedName": "3 Piece 1 Stop 1"
+    },
+    {
+      "anchor": {
+        "x": 2.809031599544681,
+        "y": 6.921961871701522
+      },
+      "prevControl": {
+        "x": 2.6349878644201734,
+        "y": 6.817492873874638
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "3 Piece 1"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 36.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "RevA",
+  "previewStartingState": {
+    "rotation": 35.26,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/3 Piece 1.path

@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.406194437388875,
-        "y": 7.089841508386721
+        "x": 2.3770102493908696,
+        "y": 6.456447732339909
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.8139011185429776,
-        "y": 7.04708475620655
+        "x": 2.809031599544681,
+        "y": 6.921961871701522
       },
       "prevControl": {
-        "x": 1.778950178270727,
-        "y": 7.121671535009737
+        "x": 2.2404261799264344,
+        "y": 6.638104544727609
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,7 +45,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -8.825474574526524,
+    "rotation": 36.0,
     "rotateFast": true
   },
   "reversed": false,

src/main/deploy/pathplanner/paths/3 Piece 2.path

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.8139011185429776,
-        "y": 7.04708475620655
+        "x": 2.809031599544681,
+        "y": 6.921961871701522
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.7965014957807695,
-        "y": 6.4770541115156295
+        "x": 2.791631976782473,
+        "y": 6.351931227010602
       },
       "isLocked": false,
       "linkedName": "3 Piece 1"

src/main/deploy/pathplanner/paths/4 Piece 3.path

@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.623633571341217,
+        "x": 2.6682193623646513,
         "y": 5.646385431659753
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.6761661106788028,
+        "x": 1.7207519017022372,
         "y": 5.204122117306238
       },
       "isLocked": false,

src/main/deploy/pathplanner/paths/Alternate 3 Piece 2.path

@@ -3,40 +3,40 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.8139011185429776,
-        "y": 7.04708475620655
+        "x": 2.809031599544681,
+        "y": 6.921961871701522
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.196897430831899,
-        "y": 7.214845624135694
+        "x": 2.1920279118336023,
+        "y": 7.089722739630666
       },
       "isLocked": false,
       "linkedName": "3 Piece 1"
     },
     {
       "anchor": {
-        "x": 2.8438902294471258,
-        "y": 5.646385431659753
+        "x": 2.9128948767156797,
+        "y": 5.502794005588011
       },
       "prevControl": {
-        "x": 2.5489874081600017,
-        "y": 5.752742852707928
+        "x": 2.6179920554285556,
+        "y": 5.609151426636186
       },
       "nextControl": {
-        "x": 2.8457928338581193,
-        "y": 5.645699252743102
+        "x": 2.9147974811266733,
+        "y": 5.5021078266713594
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.623633571341217,
+        "x": 2.6682193623646513,
         "y": 5.646385431659753
       },
       "prevControl": {
-        "x": 2.636163675105128,
+        "x": 2.6807494661285625,
         "y": 5.638427925003999
       },
       "nextControl": null,

src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java

@@ -10,6 +10,7 @@
 import com.team766.robot.common.constants.ConfigConstants;
 import com.team766.robot.common.constants.PathPlannerConstants;
 import com.team766.robot.common.mechanisms.Drive;
+import com.team766.robot.reva.Robot;
 import com.team766.robot.reva.VisionUtil.VisionSpeakerHelper;
 import com.team766.robot.reva.procedures.MoveClimbersToBottom;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -108,7 +109,7 @@ public final void run(Context context) {
         drive.setCurrentPosition(
                 shouldFlipAuton ? GeometryUtil.flipFieldPose(initialPosition) : initialPosition);
         // }
-        context.takeOwnership(drive);
+        // context.takeOwnership(drive);
         drive.resetGyro(
                 (shouldFlipAuton
                                 ? GeometryUtil.flipFieldRotation(initialPosition.getRotation())
@@ -118,5 +119,16 @@ public final void run(Context context) {
             pathItem.run(context);
             context.yield();
         }
+
+        context.takeOwnership(Robot.shooter);
+        Robot.shooter.stop();
+        context.releaseOwnership(Robot.shooter);
+
+        // TODO: For some reason, the gyro is consistenty 180 degrees from expected in teleop
+        // TODO: We should figure out why after EBR but for now we can just reset the gyro to 180 of
+        // current angle
+        context.takeOwnership(drive);
+        drive.resetGyro(180 + drive.getHeading());
+        context.releaseOwnership(drive);
     }
 }

src/main/java/com/team766/robot/reva/DriverOI.java

@@ -12,6 +12,7 @@
 import com.team766.robot.reva.mechanisms.Shooter;
 import com.team766.robot.reva.mechanisms.Shoulder;
 import com.team766.robot.reva.procedures.DriverShootNow;
+import com.team766.robot.reva.procedures.DriverShootVelocityAndIntake;
 
 public class DriverOI extends OIFragment {
 
@@ -59,8 +60,8 @@ public DriverOI(
                                                 > 0);
     }
 
-    public void handleOI(Context context) {
-
+    @Override
+    protected void handlePre() {
         // Negative because forward is negative in driver station
         leftJoystickX =
                 -createJoystickDeadzone(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD))
@@ -73,6 +74,10 @@ public void handleOI(Context context) {
         rightJoystickY =
                 -createJoystickDeadzone(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT))
                         * ControlConstants.MAX_ROTATIONAL_VELOCITY; // For steer
+    }
+
+    @Override
+    protected void handleOI(Context context) {
 
         if (leftJoystick.getButtonPressed(InputConstants.BUTTON_RESET_GYRO)) {
             drive.resetGyro();
@@ -103,13 +108,27 @@ public void handleOI(Context context) {
             context.takeOwnership(drive);
             context.takeOwnership(intake);
 
-            Robot.intake.stop();
+            intake.stop();
             drive.stopDrive();
 
             context.releaseOwnership(drive);
             context.releaseOwnership(intake);
         }
 
+        if (rightJoystick.getButtonPressed(InputConstants.BUTTON_START_SHOOTING_PROCEDURE)) {
+
+            visionContext = context.startAsync(new DriverShootVelocityAndIntake());
+
+        } else if (rightJoystick.getButtonReleased(
+                InputConstants.BUTTON_START_SHOOTING_PROCEDURE)) {
+
+            visionContext.stop();
+
+            context.takeOwnership(intake);
+            intake.stop();
+            context.releaseOwnership(intake);
+        }
+
         // Moves the robot if there are joystick inputs
         if (movingJoysticks.isTriggering()) {
             double drivingCoefficient = 1;

src/main/java/com/team766/robot/reva/VisionUtil/VisionPIDProcedure.java

@@ -23,8 +23,8 @@ public abstract class VisionPIDProcedure extends Procedure {
             new AnywhereScoringPosition(1.7201, 5600, 22.205946);
     private static AnywhereScoringPosition a3 = new AnywhereScoringPosition(1.9506, 5600, 23.516);
     private static AnywhereScoringPosition a4 = new AnywhereScoringPosition(2.072, 5600, 27.32);
-    private static AnywhereScoringPosition a5 = new AnywhereScoringPosition(2.29161, 5600, 30.109);
-    private static AnywhereScoringPosition a6 = new AnywhereScoringPosition(2.4616, 5600, 30.8987);
+    private static AnywhereScoringPosition a5 = new AnywhereScoringPosition(2.29161, 5600, 31.109);
+    private static AnywhereScoringPosition a6 = new AnywhereScoringPosition(2.4616, 5600, 31.8987);
     private static AnywhereScoringPosition a7 = new AnywhereScoringPosition(2.6942, 5600, 32.699);
     private static AnywhereScoringPosition a8 =
             new AnywhereScoringPosition(2.8657, 5600, 34.103733);

src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java

@@ -5,6 +5,7 @@
 import com.team766.framework.Mechanism;
 import com.team766.logging.LoggerExceptionUtils;
 import com.team766.robot.reva.Robot;
+import com.team766.robot.reva.constants.VisionConstants;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -37,31 +38,16 @@ public GrayScaleCamera getCamera() {
     }
 
     public void run() {
-        if (tagId == -1) {
-            Optional<Alliance> alliance = DriverStation.getAlliance();
-
-            if (alliance.isPresent()) {
-                if (alliance.get().equals(Alliance.Blue)) {
-                    tagId = 7;
-                } else {
-                    tagId = 4;
-                }
-                Robot.lights.signalCameraConnected();
-            } else {
-                LoggerExceptionUtils.logException(
-                        new AprilTagGeneralCheckedException("Couldn't find alliance correctly"));
-            }
-        }
 
         try {
             if (tagId == -1) {
                 Optional<Alliance> alliance = DriverStation.getAlliance();
 
                 if (alliance.isPresent()) {
                     if (alliance.get().equals(Alliance.Blue)) {
-                        tagId = 7;
+                        tagId = VisionConstants.MAIN_BLUE_SPEAKER_TAG;
                     } else {
-                        tagId = 4;
+                        tagId = VisionConstants.MAIN_RED_SPEAKER_TAG;
                     }
                     Robot.lights.signalCameraConnected();
                 } else {

src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java

@@ -136,7 +136,7 @@ public void rotate(double angle) {
     }
 
     public boolean isFinished() {
-        return Math.abs(getAngle() - targetAngle) < 1;
+        return Math.abs(getAngle() - targetAngle) < 2.5;
     }
 
     @Override