Diff: main.cpp

Revision:

18:eb675df59c7f

Parent:

17:3dac99cf2b89

Child:

19:4869b10a962e

--- a/main.cpp	Mon Jun 30 03:37:05 2014 +0000
+++ b/main.cpp	Mon Jun 30 07:01:58 2014 +0000
@@ -6,6 +6,7 @@
 #include "motor_api.h"
 #include "pot.h"
 #include "TSISensor.h"
+#include "Stack.h"
 
 #define Debug_cam_uart
 #define L_eye
@@ -15,8 +16,8 @@
 #define task_ma_time
 
 
-#define R_target 20
-#define L_target 108
+#define R_target 64
+#define L_target 64
 
 
 
@@ -42,7 +43,11 @@
                                  // 90/30=3
 PID cam_to_M_ctrlr(10.0,118.0,0.046,0.083,0.083-0.046,0.00,0.00,10);
 
-DigitalOut task_pin(PTD1);
+#ifdef task_ma_time
+DigitalOut cam_p(PTD1); //cam       black
+DigitalOut servo_p(PTA13); //servo   coffee
+DigitalOut de_p(PTD3);   //   red
+#endif
 TSISensor tsi;
 
 //os
@@ -50,6 +55,12 @@
 
 
 
+//global resource
+
+Stack<int> points(10);
+
+
+
 
 
 
@@ -67,16 +78,23 @@
 void cam_thread(void const *args){
     
     while(true){
-     
+        #ifdef task_ma_time
+        cam_p=1;
+        #endif   
            cam.read();
         
      
             b_r_c=cam.black_centerR();
-            b_l_c=cam.black_centerL();
-            //if(b_r_c==-1)
-             //  b_r_c=pre_b_r_c;
-            
-            //pre_b_r_c=b_r_c;
+           // b_l_c=cam.black_centerL();
+           
+           points.push(b_r_c);           
+           
+           
+        #ifdef task_ma_time
+        cam_p=0;
+        #endif    
+           
+           
             Thread::wait(t_cam);
         
         }
@@ -87,7 +105,10 @@
     
     while(1){
         
-        
+          
+        #ifdef task_ma_time
+        de_p=0;
+        #endif       
         
         
     stdio_mutex.lock();    
@@ -127,7 +148,10 @@
          stdio_mutex.unlock();
      
          
-     
+           
+        #ifdef task_ma_time
+        de_p=1;
+        #endif   
        
          
          
@@ -146,18 +170,33 @@
     
 
         while(1){
+     
+        #ifdef task_ma_time
+        servo_p=1;
+        #endif
         
-        if(b_r_c!=-1)
+        int point;
+        
+        for(;points.available()!=0;){
+            points.pop(&point);
+        
+        //algorithm
+        
+        }
+             
+        //if(b_r_c!=-1)
          v_servo=cam_to_M_ctrlr.compute(b_r_c,R_target);     
-        if(b_l_c!=-1)
-         v_servo=cam_to_M_ctrlr.compute(b_l_c,L_target);     
+        //if(b_l_c!=-1)
+        // v_servo=cam_to_M_ctrlr.compute(b_l_c,L_target);     
             
-         
-         
-            
-
+        
              // v_servo=pot2.read();
-               servo.set_angle(v_servo);
+         servo.set_angle(v_servo);
+        
+        #ifdef task_ma_time
+        servo_p=0;
+        #endif 
+        
               
          Thread::wait(20);    
         }    
@@ -216,8 +255,8 @@
       Thread th_c(cam_thread);
    //   Thread thread(ctrl_thread);  
       Thread th_s(servo_thread);  
-      Thread th_m(motor_thread);     
-   //  Thread th_de(de_thread);  
+   //   Thread th_m(motor_thread);     
+    // Thread th_de(de_thread);  
      while(1){