Diff: main.cpp

Revision:

20:30799cbda86b

Parent:

19:4869b10a962e

--- a/main.cpp	Wed Jul 02 03:23:07 2014 +0000
+++ b/main.cpp	Wed Jul 02 06:44:48 2014 +0000
@@ -10,7 +10,7 @@
 
 #define Debug_cam_uart
 //#define L_eye
-#define R_eye
+//#define R_eye
 #define motor_on 
 #define Pcontroller
 //#define task_ma_time
@@ -160,7 +160,6 @@
          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());
          
          stdio_mutex.unlock();
      
@@ -195,39 +194,42 @@
        int point;
        int sum_e=0;
        int n_point=0; 
-       n_point=points.available() ;
-       
-        for(int i=0;i<n_point;i++){
-             points.pop(&point);
+       int compute_p=0;
+         printf("stak size: %d\r\n",n_point);
+        for(int i=0; points.pop(&point)==0;i++){
+            
+
+            printf("pop :%d\r\n",point);
         //algorithm
-           
-             sum_e+=point-black_center;  
-     
+           if(point!=-1){
+             sum_e+=point;  
+             n_point++;     
+           }
         }
-             printf("error :%d compute: %f\r\n",sum_e/n_point, cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target));
+             printf("error :%d compute: %f\r\n",(sum_e/(n_point/2)), cam_to_M_ctrlr.compute(R_target+sum_e/n_point,R_target));
         
         
-        cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target);  
+       // v_servo=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) );
+         // servo.set_angle(         cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target) );
          
-        // pc.printf("dfdf");    
+        
         //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);     
             
         
-             // v_servo=pot2.read();
-       
+              v_servo=pot2.read();
+                servo.set_angle(v_servo);
         
         #ifdef task_ma_time
         servo_p=0;
         #endif 
         
               
-         Thread::wait(20);
+         Thread::wait(1000);
         
              
         }    
@@ -273,6 +275,11 @@
     servo.set_angle(0.055);
     pc.baud(115200);
    
+   
+   
+   
+   
+   
     
    
      //while(1);
@@ -289,8 +296,8 @@
   */
     
     
-    
-      Thread th_c(cam_thread);
+       
+   //   Thread th_c(cam_thread);
    //   Thread thread(ctrl_thread);  
       Thread th_s(servo_thread);  
    //   Thread th_m(motor_thread);     
@@ -303,18 +310,22 @@
              idle_p=1;
           #endif
            
-           
-           
-          #ifdef task_ma_time
-             idle_p=0;
-          #endif
+     // pc.printf("idle\r\n");        
            
+     for(int i=0;i<6;i++){
+      // printf("push :%d\r\n",10+i*10);  
+     
+        points.push(0+i*10);
+       
+     }
+       printf("idle\r\n");
+            Thread::wait(1000);
            
-     //idle
-  //   stdio_mutex.lock();
-    //    printf("L: %d  mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
-    // stdio_mutex.unlock();     
-          Thread::wait(1000);
+         
+                
+       #ifdef task_ma_time
+             idle_p=0;
+          #endif   
     
      }