Diff: main.cpp

Revision:

7:ad893fc41b95

Parent:

6:b0e160c51013

Child:

8:7c5e6b1e7aa5

--- a/main.cpp	Wed Nov 09 17:54:38 2016 +0000
+++ b/main.cpp	Mon Nov 14 11:11:13 2016 +0000
@@ -23,6 +23,9 @@
 float speed = 0.3;
 int frame_counter = 0;
  
+ int startstop = 0;
+bool seen = false;
+ 
 int main() {
     TFC_Init();
     TFC_InitServos(0.00052,0.00122,0.02);
@@ -76,6 +79,7 @@
                     if(xb.cBuffer->available() >= 1) {
                         char a = xb.cBuffer->read();
                         speed = a/256.0f;
+                        TFC_SetMotorPWM(speed,speed);  
                         pc.putc('E');
                         pc.printf("s = %u %f",a, speed);
                         pc.putc(0);
@@ -123,7 +127,7 @@
                 */
                 
                 diff = prev - curr_left;
-                if(abs(diff) >= 10 && prev != -1) {
+                if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
                     left = i;
                     break;    
                 }
@@ -142,7 +146,7 @@
                 */
                 
                 int diff = prev - curr_right;
-                if(abs(diff) >= 10 && prev != -1) {
+                if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
                     right = i;
                     break;    
                 }
@@ -150,19 +154,35 @@
                 prev = curr_right;
             }
 
-            //if((frame_counter % 10) == 0) {
+            if((frame_counter % 3) == 0) {
             pc.putc('H');
             for(i = 0; i < 128; i++) {
                 pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);    
             }
-            //}
-            //frame_counter++;
+            }
+            frame_counter++;
             
             measured_value = (64 - ((left+right)/2))/64.f;
             
-            pc.putc('E');
-            pc.printf("left=%u right=%u", left, right);
-            pc.putc(0);            
+            if(right - left < 60) {
+                pc.putc('E');
+                pc.printf("START STOP!! &d",startstop);
+                pc.putc(0);            
+                
+                if(seen) {
+                    seen = false;
+                } else {
+                    startstop++;
+                    seen = true;    
+                }    
+            
+            if(startstop >= 5) {
+                TFC_SetMotorPWM(0.0,0.0);
+                TFC_HBRIDGE_DISABLE;
+                startstop = 0;      
+            }
+            
+            } 
             
     t.start();
     dt=t.read();
@@ -219,7 +239,7 @@
 
             //Reset image ready flag
             TFC_LineScanImageReady=0;
-            wait(0.05f);
+            wait(0.02f);
         }
     }
 }
\ No newline at end of file