Eliminate potential vision thread race condition & use persistent pau…

…sable thread

src/main/java/frc/Robot.java

@@ -65,14 +65,14 @@ public void robotInit() {
 		Command autoShooter = new Command() {
 			@Override
 			public void initialize() {
-				// Start the vision thread
-				vision.start();
+				// Resume the vision thread.
+				vision.notifyStart();
 			}
 
 			@Override
 			public void end(boolean interrupted) {
-				// Kill the vision thread
-				vision.requestStop();
+				// Pause the vision thread.
+				vision.notifyStop();
 			}
 		}.alongWith(shooterLeft.runLo(), shooterRight.runLo());
 

src/main/java/frc/system/Vision.java

@@ -1,7 +1,5 @@
 package frc.system;
 
-import java.util.concurrent.atomic.AtomicBoolean;
-
 import org.opencv.core.Core;
 import org.opencv.core.Mat;
 import org.opencv.core.Rect;
@@ -15,7 +13,9 @@
 
 public class Vision extends Thread {
     final Target[] targets;
-    final AtomicBoolean stop = new AtomicBoolean(false);
+
+    // Whether the vision executor should be active.
+    boolean active = false;
 
     final CvSink source = CameraServer.getVideo();
     final CvSource stream = CameraServer.putVideo("Rectangle", 640, 480);
@@ -48,14 +48,36 @@ public Vision(Target targets[]) {
         setDaemon(true);
 
         CameraServer.startAutomaticCapture();
+
+        start();
     }
 
     @Override
     public void run() {
-        // Mats are very memory expensive. Lets reuse this Mat.
         Mat mat = new Mat();
 
-        while (!Thread.interrupted() && !stop.compareAndSet(true, false)) {
+        // Main loop of the thread.
+        while (true) {
+            // Acquire the lock on the vision state.
+            synchronized (this) {
+                while (true) {
+                    // Check if the thread should continue.
+                    if (active) {
+                        // If so, then execute another iteration of the vision loop.
+                        break;
+                    }
+
+                    try {
+                        // Otherwise, wait for a ping from `start`, relinquishing the lock to allow the
+                        // active state to change.
+                        // Then, continue, and check the active state again.
+                        wait();
+                    } catch (InterruptedException e) {
+                        return;
+                    }
+                }
+            }
+
             // Tell the CvSink to grab a frame from the camera and put it
             // in the source mat.  If there is an error notify the output.
             if (source.grabFrame(mat) == 0) {
@@ -75,7 +97,7 @@ public void run() {
                     (int)(t.x2 * c),
                     (int)(t.y2 * r)
                 );
-                
+
                 // Slice the mat to the target rectangle
                 Mat submat = mat.submat(rect);
                 // Find the average pixel value
@@ -88,13 +110,23 @@ public void run() {
 
                 Imgproc.rectangle(mat, rect, new Scalar(0, detected ? 255 : 0, detected ? 0 : 255), 5);
             }
-        
+
             // Give the output stream a new image to display
             stream.putFrame(mat);
         }
     }
 
-    public void requestStop() {
-        stop.set(true);
+    public synchronized void notifyStart() {
+        // Set the active flag.
+        active = true;
+        // Notify the thread. When this function exits, the lock on the object will be
+        // relinquished, resuming the thread.
+        notify();
+    }
+
+    public synchronized void notifyStop() {
+        // Set the active flag. The thread will view this flag and enter the waiting
+        // state.
+        active = false;
     }
 }