trap auton code

src/main/java/com/team766/ViSIONbase/ScoringPosition.java

@@ -2,6 +2,7 @@
 
 public class ScoringPosition {
     public final double speed, angle, x_position, y_position, swerve_angle;
+    public final int tagId;
 
     /*
      * @param speed the speed to set the shooter, in motor shaft rps.
@@ -11,11 +12,17 @@ public class ScoringPosition {
      * @param swerve_angle the angle the robot should be facing relative to the AprilTag (where 0 degrees is flush)
      */
     public ScoringPosition(
-            double speed, double angle, double x_position, double y_position, double swerve_angle) {
+            double speed,
+            double angle,
+            double x_position,
+            double y_position,
+            double swerve_angle,
+            int tagId) {
         this.speed = speed;
         this.angle = angle;
         this.x_position = x_position;
         this.y_position = y_position;
         this.swerve_angle = swerve_angle;
+        this.tagId = tagId;
     }
 }

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

@@ -1,8 +1,32 @@
 package com.team766.robot.reva.VisionUtil;
 
 import com.team766.ViSIONbase.ScoringPosition;
+import java.util.ArrayList;
 
 public class ScoringPositions {
-    public static final ScoringPosition MAKER_SPACE_1L = new ScoringPosition(0, 0, 2.88, -0.92, 0);
-    public static final ScoringPosition MAKER_SPACE_1R = new ScoringPosition(0, 0, 2.9, 0.80, 0);
+    public static final ScoringPosition RED_ALLIANCE_STAGE_LEFT =
+            new ScoringPosition(0, 0, 0, 0, 0, 11);
+    public static final ScoringPosition RED_ALLIANCE_STAGE_RIGHT =
+            new ScoringPosition(0, 0, 0, 0, 0, 12);
+    public static final ScoringPosition RED_ALLIANCE_CENTERSTAGE =
+            new ScoringPosition(0, 0, 0, 0, 0, 13);
+
+    public static final ScoringPosition BLUE_ALLIANCE_CENTERSTAGE =
+            new ScoringPosition(0, 0, 0, 0, 0, 14);
+    public static final ScoringPosition BLUE_ALLIANCE_STAGE_LEFT =
+            new ScoringPosition(0, 0, 0, 0, 0, 15);
+    public static final ScoringPosition BLUE_ALLIANCE_STAGE_RIGHT =
+            new ScoringPosition(0, 0, 0, 0, 0, 16);
+
+    public static final ArrayList<ScoringPosition> trapScoringPositions =
+            new ArrayList<ScoringPosition>() {
+                {
+                    add(RED_ALLIANCE_STAGE_LEFT);
+                    add(RED_ALLIANCE_STAGE_RIGHT);
+                    add(RED_ALLIANCE_CENTERSTAGE);
+                    add(BLUE_ALLIANCE_CENTERSTAGE);
+                    add(BLUE_ALLIANCE_STAGE_LEFT);
+                    add(BLUE_ALLIANCE_STAGE_RIGHT);
+                }
+            };
 }

src/main/java/com/team766/robot/reva/procedures/DriveToAndScoreAt.java

@@ -16,21 +16,26 @@ public class DriveToAndScoreAt extends VisionPIDProcedure {
     private double lastY;
 
     private double timeLastSeen = -1;
+    private int tagId;
 
     public DriveToAndScoreAt(ScoringPosition score) {
         this.score = score;
+        tagId = score.tagId;
     }
 
     // button needs to be held down
     public void run(final Context context) {
         context.takeOwnership(Robot.drive);
         context.takeOwnership(Robot.intake);
         context.takeOwnership(Robot.shooter);
+        context.takeOwnership(Robot.shoulder);
 
         yPID.setSetpoint(score.y_position);
         xPID.setSetpoint(score.x_position);
 
-        while (Math.abs(xPID.getOutput()) + Math.abs(yPID.getOutput()) != 0) {
+        Robot.shoulder.rotate(score.angle);
+
+        while (Math.abs(xPID.getOutput()) + Math.abs(yPID.getOutput()) > 0.05) {
             context.yield();
 
             Transform3d robotToTag;
@@ -49,7 +54,7 @@ public void run(final Context context) {
 
                 // TODO: Turn this into PID?
                 // If it is more that four degrees off...
-                if (Math.abs(robotToTag.getRotation().getZ()) > 4) {
+                if (Math.abs(robotToTag.getRotation().getZ()) > 3) {
                     if (robotToTag.getRotation().getZ() < 0) {
                         turnConstant = -0.02;
                     } else {
@@ -77,6 +82,20 @@ public void run(final Context context) {
 
         }
         Robot.shooter.shoot(score.speed);
+
+        context.waitFor(Robot.shoulder::isFinished);
+        context.waitFor(Robot.shooter::isCloseToExpectedSpeed);
+
+        Robot.intake.runIntake();
+
+        context.waitForSeconds(1.5);
+
+        Robot.intake.stop();
+
+        context.releaseOwnership(Robot.drive);
+        context.releaseOwnership(Robot.shooter);
+        context.releaseOwnership(Robot.intake);
+        context.releaseOwnership(Robot.shoulder);
     }
 
     /**
@@ -92,16 +111,6 @@ public void run(final Context context) {
     private Transform3d getTransform3dOfRobotToTag() throws AprilTagGeneralCheckedException {
         GrayScaleCamera toUse = Robot.forwardApriltagCamera.getCamera();
 
-        Transform3d robotToTag = toUse.getBestTargetTransform3d(toUse.getBestTrackedTarget());
-
-        int tagId = toUse.getTagIdOfBestTarget();
-
-        // this is the tag we will be using for testing in the time being. later we will need to set
-        // based on alliance color
-        if (tagId == 5) {
-            return robotToTag;
-        }
-
-        throw new AprilTagGeneralCheckedException("Could not find tag with the correct tagId");
+        return GrayScaleCamera.getBestTargetTransform3d(toUse.getTrackedTargetWithID(tagId));
     }
 }

src/main/java/com/team766/robot/reva/procedures/ShootTrap.java

@@ -0,0 +1,39 @@
+package com.team766.robot.reva.procedures;
+
+import com.team766.ViSIONbase.AprilTagGeneralCheckedException;
+import com.team766.ViSIONbase.GrayScaleCamera;
+import com.team766.framework.Context;
+import com.team766.robot.reva.Robot;
+import com.team766.robot.reva.VisionUtil.ScoringPositions;
+import com.team766.robot.reva.VisionUtil.VisionPIDProcedure;
+
+public class ShootTrap extends VisionPIDProcedure {
+
+    int tagId;
+
+    public void run(Context context) {
+
+        while (true) {
+            context.yield();
+            try {
+                tagId = getTagId();
+                break;
+            } catch (AprilTagGeneralCheckedException e) {
+                continue;
+            }
+        }
+
+        for (int i = 0; i < ScoringPositions.trapScoringPositions.size(); i++) {
+            if (ScoringPositions.trapScoringPositions.get(i).tagId == tagId) {
+                new DriveToAndScoreAt(ScoringPositions.trapScoringPositions.get(i)).run(context);
+                break;
+            }
+        }
+    }
+
+    private int getTagId() throws AprilTagGeneralCheckedException {
+        GrayScaleCamera toUse = Robot.forwardApriltagCamera.getCamera();
+
+        return toUse.getTagIdOfBestTarget();
+    }
+}

src/main/java/com/team766/robot/swerveandshoot/OI.java

@@ -63,8 +63,8 @@ public void run(final Context context) {
              */
             if (joystick0.getButtonPressed(1)) {
                 // Robot.speakerShooter.goToAndScore(SpeakerShooterPowerCalculator.makerSpace1R);
-                visionProcedure =
-                        context.startAsync(new DriveToAndScoreAt(ScoringPositions.makerSpace1R));
+                // visionProcedure =
+                // context.startAsync(new DriveToAndScoreAt(ScoringPositions.makerSpace1R));
             }
 
             if (joystick0.getButtonReleased(1)) {
@@ -77,8 +77,8 @@ public void run(final Context context) {
              */
             if (joystick0.getButtonPressed(2)) {
                 // Robot.speakerShooter.goToAndScore(SpeakerShooterPowerCalculator.makerSpace1R);
-                visionProcedure =
-                        context.startAsync(new DriveToAndScoreAt(ScoringPositions.makerSpace1L));
+                // visionProcedure =
+                // context.startAsync(new DriveToAndScoreAt(ScoringPositions.makerSpace1L));
             }
 
             if (joystick0.getButtonReleased(2)) {

src/main/java/com/team766/robot/swerveandshoot/ScoringPositions.java

@@ -1,8 +1,6 @@
 package com.team766.robot.swerveandshoot;
 
-import com.team766.ViSIONbase.ScoringPosition;
-
 public class ScoringPositions {
-    public static ScoringPosition makerSpace1L = new ScoringPosition(0, 0, 2.88, -0.92, 0);
-    public static ScoringPosition makerSpace1R = new ScoringPosition(0, 0, 2.9, 0.80, 0);
+    // public static ScoringPosition makerSpace1L = new ScoringPosition(0, 0, 2.88, -0.92, 0);
+    // public static ScoringPosition makerSpace1R = new ScoringPosition(0, 0, 2.9, 0.80, 0);
 }