Diff: main.cpp

Revision:

22:9ef4a01e5038

Parent:

21:18f2dc256df2

Child:

23:6e1e142b7baf

--- a/main.cpp	Tue Apr 11 20:57:17 2017 +0000
+++ b/main.cpp	Mon Apr 17 00:41:26 2017 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "Camera.h"
+#include "tsi_sensor.h"
 #define STRAIGHT 0.00092f
 #define FULLRIGHT 0.0013f
 #define FULLLEFT 0.0005
@@ -14,6 +15,17 @@
 #define BLIND_LENGTH 30
 #define DIFF_RATIO 0.5
 
+#if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
+  #define ELEC0 9
+  #define ELEC1 10
+#elif defined (TARGET_KL05Z)
+  #define ELEC0 9
+  #define ELEC1 8
+#else
+  #error TARGET NOT DEFINED
+#endif
+
+
 PwmOut servo(PTE20);
 PwmOut motor_left(PTA5);
 PwmOut motor_right(PTC8);
@@ -21,6 +33,7 @@
 DigitalOut DIR_R(PTA4);
 Serial pc(USBTX, USBRX);
 Camera cam(PTE23, PTE21, PTB3);
+TSIAnalogSlider tsi(ELEC0, ELEC1, 40);
 int turnCounter = 0;
 int startFinishCounter = 0;
 int threshold = DEFAULT_THRESHOLD;
@@ -28,8 +41,10 @@
 bool idle = true;
 int leftBlind = 0;
 int rightBlind = 0;
+float lastSlide;
 int numDarks = 0;
 PwmOut led(LED_GREEN);
+PwmOut redLed(LED_RED);
 
 /*
     Function: setAccel
@@ -114,6 +129,12 @@
     int maxLightGap = 55;
     int minDarkBlock = 4;
     int maxDarkBlock = 40;
+    if(tsi.readPercentage() != lastSlide){
+        idle = !idle;
+        led = 0.0;
+        wait(0.5);
+    }
+    if(numDarks <= 15) return;
     for(int i = 0; i < 128; i++){
         if(lookForDark){
             if(frame[i] < threshold){
@@ -152,7 +173,7 @@
             idle = !idle; //toggle idle
             startFinishCounter = 1;
             if(!idle){ 
-                led = 0.0;
+                led = 1.0 - led;
                 servo.pulsewidth(STRAIGHT);
                 wait(5);
             }
@@ -180,7 +201,8 @@
         if(cam.imageData[i] > high) high = cam.imageData[i];
         if(cam.imageData[i] < low) low = cam.imageData[i];
     }
-    threshold = low + (high - low) * 0.35;        //(high + 2 * low) / 3;
+//    threshold = low + (high - low) * 0.35;        //(high + 2 * low) / 3;
+    threshold = (low + high) / 2;
 }
 
 int main() {    
@@ -191,12 +213,15 @@
     DIR_L = 0;
     servo.period(0.020f);
     led = 1.0;
+    redLed = 1.0;
+    idle = true;
+    lastSlide = tsi.readPercentage();
     while(1){
         wait_ms(5);
         cam.capture();
         //display(cam.imageData);
         turnWheels(cam.imageData);
         setAccel(wheelPos);
-        if(numDarks > 15) detectStartFinish(cam.imageData);
+        detectStartFinish(cam.imageData);
     }
 }
\ No newline at end of file