Diff: main.cpp

Revision:

20:f541b6b063fa

Parent:

19:4869b10a962e

Child:

21:4e8a4f66aaef

--- a/main.cpp	Wed Jul 02 03:23:07 2014 +0000
+++ b/main.cpp	Wed Jul 09 12:48:46 2014 +0000
@@ -9,19 +9,18 @@
 #include "Stack.h"
 
 #define Debug_cam_uart
-//#define L_eye
+#define L_eye
 #define R_eye
 #define motor_on 
 #define Pcontroller
 //#define task_ma_time
 
 
-#define R_target 64
-#define L_target 64
+#define center 64
 
 
 
-#define t_cam 6
+#define t_cam 4
 
 #define black_center 64
 
@@ -58,7 +57,8 @@
 
 //global resource
 
-Stack<int> points(10);
+Stack<int> pointsR(10);
+Stack<int> pointsL(10);
 
 
 
@@ -66,8 +66,6 @@
 
 
 
-static int pre_b_r_c;
-
 static double v_motor;
 static double v_servo;
 
@@ -93,12 +91,12 @@
                 
       
             b_r_c=cam.black_centerR();
-           // b_l_c=cam.black_centerL();
+            b_l_c=cam.black_centerL();
       
       //right
-          // printf("push :%d\r\n",b_r_c);     
-           points.push(b_r_c);           
-           
+           //printf("push :%d\r\n",b_r_c);     
+           pointsR.push(b_r_c);           
+           pointsR.push(b_l_c); 
       
            
            
@@ -130,28 +128,20 @@
     #ifdef Debug_cam_uart 
       #ifdef L_eye  
          pc.printf("L: ");
-        for(int i=128;i>=0;i--){
-             if(i==64)
+        for(int i=127;i>=0;i--){
+             if(i==64||i==0||i==127)
                pc.printf("X");
-             else if(i<10)
-               pc.printf("-");
-             else if(i>117)
-               pc.printf("-");
              else          
                pc.printf("%c", cam.sign_line_imageL[i]);         
          }
          
-         pc.printf("           ||             ");
+         pc.printf(" || ");
       #endif
       #ifdef R_eye
         pc.printf("R: ");
-        for(int i=128;i>=0;i--){
-             if(i==64)
+        for(int i=127;i>=0;i--){
+             if(i==64||i==0||i==127)
                pc.printf("X");
-             else if(i<10)
-               pc.printf("-");
-             else if(i>117)
-               pc.printf("-");
              else          
                pc.printf("%c", cam.sign_line_imageR[i]);         
          }
@@ -160,7 +150,7 @@
          pc.printf("\r\n Rcenter :%d Lcenter : %d servo: %f \r\n",cam.black_centerR(),cam.black_centerL(),v_servo);
     
          
-         pc.printf("stack n: %d",points.available());
+        // pc.printf("stack n: %d",points.available());
          
          stdio_mutex.unlock();
      
@@ -193,24 +183,52 @@
         #endif
         
        int point;
-       int sum_e=0;
-       int n_point=0; 
-       n_point=points.available() ;
+       int sum_error_R=0;
+       int sum_error_L=0;
+       int n_pointR=0;
+       int n_pointL=0;
+       n_pointR=pointsR.available() ;
+       int correct_pointR_number=0;
+       n_pointL=pointsL.available() ;
+       int correct_pointL_number=0;
        
-        for(int i=0;i<n_point;i++){
-             points.pop(&point);
+        for(int i=0;i<n_pointR;i++){
+             pointsR.pop(&point);
         //algorithm
-           
-             sum_e+=point-black_center;  
-     
+            if(point>0){
+                sum_error_R+=point;   //because R's black is on the right side of line
+                correct_pointR_number++;
+            }
         }
-             printf("error :%d compute: %f\r\n",sum_e/n_point, cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target));
+        for(int i=0;i<n_pointL;i++){
+            if(point>0){
+                sum_error_L+=point;   //because R's black is on the right side of line
+                correct_pointL_number++;
+             }
+       }
+        
+        int error_R_ave=(correct_pointR_number==0)?0:sum_error_R/correct_pointR_number;
+        int error_L_ave=(correct_pointL_number==0)?0:sum_error_L/correct_pointL_number;
+        
+        //two line
+        if(error_L_ave!=0 && error_R_ave!=0){
+            servo.set_angle(cam_to_M_ctrlr.compute((error_L_ave+error_R_ave)/2,center) );
+        }
+        
+        //in the correct think, one line should not appear
+        //right line
+        else if(error_L_ave==0 && error_R_ave!=0){  
+        }
+        //left line
+        else if(error_R_ave==0 && error_L_ave!=0){
+        }
+        //no line
+        else if(error_L_ave!=0 && error_R_ave!=0){}
         
         
-        cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target);  
-       // pc.printf("expect :%d \r\n",black_center+sum_e/n_point );
-       // pc.printf("angle :%f", cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target)  );
-          servo.set_angle(         cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target) );
+       // cam_to_M_ctrlr.compute(black_center+sum_error_R/n_pointR,center);  
+     
+       // servo.set_angle(         cam_to_M_ctrlr.compute(black_center+sum_e/n_pointR,center) );
          
         // pc.printf("dfdf");    
         //if(b_r_c!=-1)
@@ -292,9 +310,9 @@
     
       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_s(servo_thread);  
+     // Thread th_m(motor_thread);     
+     Thread th_de(de_thread);
      
      //Thread dddd(pin2_thread);  
      while(1){
@@ -314,7 +332,7 @@
   //   stdio_mutex.lock();
     //    printf("L: %d  mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
     // stdio_mutex.unlock();     
-          Thread::wait(1000);
+//          Thread::wait(1000);
     
      }