       if (sample == LOW) {
         // Just seen a black bit.
         ms_per_int = count * sample_interval;
+        
+        if (ms_per_int > 700) {
+          Input = 0;
+        } else {
+          
+          // Switch low to high
+          Input = map(ms_per_int, 0, 700, 700, 0);
+          //speed_motor_left = map(ms_per_int, 0, 1023, 1023, 0);
+          
+          //speed_motor_left = ms_per_int;
+          //Input = speed_motor_left;
+          //Serial.print(" : "); Serial.print(speed_motor_left);
+          //Serial.print(" - "); Serial.println(Input);
+          
+          count = 0;
+        }
+      } else {
+        // Check for stopped/too slow
+        ms_per_int = count * sample_interval;
+        if (ms_per_int > 700) {
+          count = 0;
+          Input = 0;
+        }
+      }
+      
+    } else {
+      
+      // Nah not right yet for slow
+      
+      // No change in light
+      // Check for stopped/too slow
+      ms_per_int = count * sample_interval;
+      if (ms_per_int > 700) {
         count = 0;
-        //Serial.println(ms_per_int);
-        
-        ms_per_int = constrain(ms_per_int, 100, 700);
-        
-        // Convert to byte value for processing for now
-        speed_motor_left = map(ms_per_int, 100, 700, 700, 100);
-        //speed_motor_left = ms_per_int;
-        Input = speed_motor_left;
-        //Serial.print(" : "); Serial.print(speed_motor_left);
-        //Serial.print(" - "); Serial.println(Input);
+        Input = 0;
       }
-      
     }
+    
     count++; // Up the sample count
     
-    /*
-    speed_requested = analogRead(PIN_POT) / 4;
-    if (speed_requested < 75) {
+    // Do the computation now
+    myPID.Compute();
+    
+    
+    pwm_motor_left = Output;
+    //pwm_motor_left = constrain(pwm_motor_left, 100, 180);
+    if (pwm_motor_left < 90) pwm_motor_left = 0;
+    analogWrite(PIN_MOTOR_LEFT, pwm_motor_left);
+       
+    
+    Setpoint = analogRead(PIN_POT); // NB motor can't go faster than 700 :(
+    if (Setpoint < 90) {
       // Can't go slower than this
-      speed_requested = 0; 
+      Setpoint = 0; 
     }
-    */
-    //Serial.println(speed_requested);
-    
-    //pid-related code
-    myPID.Compute();
-    
-    
-    pwm_motor_left = Output;
-    pwm_motor_left = constrain(pwm_motor_left, 100, 180);
-    //if (pwm_motor_left < 90) pwm_motor_left = 90;
-    analogWrite(PIN_MOTOR_LEFT, pwm_motor_left);
-    
-    /*
-    // Send it to the motor if changed
-    if (pwm_motor_left != speed_requested) {
-      pwm_motor_left = speed_requested;
-      
-      //pwm_motor_left = map(Output, 0, 1000, 0, 255);
-      if (pwm_motor_left < 75) pwm_motor_left = 0;
-      analogWrite(PIN_MOTOR_LEFT, pwm_motor_left);
-    }
-    */
+    
     
   }
   

int inputPin=0, outputPin=3;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, 1, 5, 1, DIRECT);
unsigned long serialTime; //this will help us know when to talk with processing

int count =0;

  if (millis()>serialTime) {
    SerialReceive();
    SerialSend();
    serialTime+=250;
  }
  

