Diff: main.cpp

Revision:

21:4e8a4f66aaef

Parent:

20:f541b6b063fa

Child:

22:34a0c436ac45

--- a/main.cpp	Wed Jul 09 12:48:46 2014 +0000
+++ b/main.cpp	Thu Jul 10 03:25:59 2014 +0000
@@ -94,9 +94,9 @@
             b_l_c=cam.black_centerL();
       
       //right
-           //printf("push :%d\r\n",b_r_c);     
+           //printf("push : R: %d   L: %d \r\n",b_r_c,b_l_c);     
            pointsR.push(b_r_c);           
-           pointsR.push(b_l_c); 
+           pointsL.push(b_l_c); 
       
            
            
@@ -182,6 +182,7 @@
         servo_p=1;
         #endif
         
+      //  servo.set_angle(0.055);
        int point;
        int sum_error_R=0;
        int sum_error_L=0;
@@ -192,24 +193,34 @@
        n_pointL=pointsL.available() ;
        int correct_pointL_number=0;
        
+       pc.printf("R: ");
+       
         for(int i=0;i<n_pointR;i++){
              pointsR.pop(&point);
+             pc.printf("%d ",point);
         //algorithm
             if(point>0){
                 sum_error_R+=point;   //because R's black is on the right side of line
                 correct_pointR_number++;
             }
         }
+        
+pc.printf("L:");
         for(int i=0;i<n_pointL;i++){
+                pointsL.pop(&point);
+                pc.printf("%d ",point);
             if(point>0){
                 sum_error_L+=point;   //because R's black is on the right side of line
                 correct_pointL_number++;
              }
        }
-        
+        pc.printf("\r\n");
         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;
         
+      //  pc.printf("L: %d  R: %d\r\n",error_L_ave,error_R_ave);
+        
+        
         //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) );
@@ -218,9 +229,11 @@
         //in the correct think, one line should not appear
         //right line
         else if(error_L_ave==0 && error_R_ave!=0){  
+            servo.set_angle(cam_to_M_ctrlr.compute(error_R_ave,30));
         }
         //left line
         else if(error_R_ave==0 && error_L_ave!=0){
+            servo.set_angle(cam_to_M_ctrlr.compute(error_L_ave,88));
         }
         //no line
         else if(error_L_ave!=0 && error_R_ave!=0){}
@@ -310,9 +323,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){