Diff: main.cpp

Revision:

19:4869b10a962e

Parent:

18:eb675df59c7f

Child:

20:f541b6b063fa

--- a/main.cpp	Mon Jun 30 07:01:58 2014 +0000
+++ b/main.cpp	Wed Jul 02 03:23:07 2014 +0000
@@ -9,11 +9,11 @@
 #include "Stack.h"
 
 #define Debug_cam_uart
-#define L_eye
-//#define R_eye
+//#define L_eye
+#define R_eye
 #define motor_on 
 #define Pcontroller
-#define task_ma_time
+//#define task_ma_time
 
 
 #define R_target 64
@@ -21,9 +21,9 @@
 
 
 
-#define t_cam 2
+#define t_cam 6
 
-
+#define black_center 64
 
 
  Serial pc(USBTX, USBRX); // tx, rx
@@ -44,9 +44,10 @@
 PID cam_to_M_ctrlr(10.0,118.0,0.046,0.083,0.083-0.046,0.00,0.00,10);
 
 #ifdef task_ma_time
-DigitalOut cam_p(PTD1); //cam       black
-DigitalOut servo_p(PTA13); //servo   coffee
-DigitalOut de_p(PTD3);   //   red
+DigitalOut cam_p(PTE5); //cam       black
+DigitalOut servo_p(PTE4); //servo   coffee
+DigitalOut idle_p (PTE3);
+//DigitalOut de_p(PTD3);   //   red
 #endif
 TSISensor tsi;
 
@@ -64,8 +65,7 @@
 
 
 
-static int b_r_c=64;
-static int b_l_c=64;
+
 static int pre_b_r_c;
 
 static double v_motor;
@@ -80,37 +80,52 @@
     while(true){
         #ifdef task_ma_time
         cam_p=1;
-        #endif   
+        #endif  
+        
            cam.read();
         
-     
-            b_r_c=cam.black_centerR();
-           // b_l_c=cam.black_centerL();
-           
-           points.push(b_r_c);           
-           
-           
+             
         #ifdef task_ma_time
         cam_p=0;
         #endif    
+        
+        int b_r_c,b_l_c;
+                
+      
+            b_r_c=cam.black_centerR();
+           // b_l_c=cam.black_centerL();
+      
+      //right
+          // printf("push :%d\r\n",b_r_c);     
+           points.push(b_r_c);           
+           
+      
            
            
             Thread::wait(t_cam);
-        
+      
         }
     
 }
 // function
+
+
+
+
+
+
+
+
 void de_thread(void const *args){
     
     while(1){
         
           
         #ifdef task_ma_time
-        de_p=0;
+        //de_p=0;
         #endif       
         
-        
+        cam.read();
     stdio_mutex.lock();    
     #ifdef Debug_cam_uart 
       #ifdef L_eye  
@@ -145,18 +160,20 @@
          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();
      
          
            
         #ifdef task_ma_time
-        de_p=1;
+        //de_p=1;
         #endif   
        
          
          
 #endif   
-             Thread::wait(1);
+             Thread::wait(10);
 
       
     }
@@ -175,30 +192,44 @@
         servo_p=1;
         #endif
         
-        int point;
-        
-        for(;points.available()!=0;){
-            points.pop(&point);
-        
+       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);
         //algorithm
-        
+           
+             sum_e+=point-black_center;  
+     
         }
-             
+             printf("error :%d compute: %f\r\n",sum_e/n_point, cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target));
+        
+        
+        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) );
+         
+        // pc.printf("dfdf");    
         //if(b_r_c!=-1)
-         v_servo=cam_to_M_ctrlr.compute(b_r_c,R_target);     
+        // 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();
-         servo.set_angle(v_servo);
+       
         
         #ifdef task_ma_time
         servo_p=0;
         #endif 
         
               
-         Thread::wait(20);    
+         Thread::wait(20);
+        
+             
         }    
     
     
@@ -241,14 +272,21 @@
   
     servo.set_angle(0.055);
     pc.baud(115200);
-   /*
-      while(1){
+   
+    
+   
+     //while(1);
+     
+   
+
+   
+  /*    while(1){
          
              if(tsi.readPercentage()>0.00011)
              break;
       }
-    */
-  
+    
+  */
     
     
     
@@ -256,15 +294,27 @@
    //   Thread thread(ctrl_thread);  
       Thread th_s(servo_thread);  
    //   Thread th_m(motor_thread);     
-    // Thread th_de(de_thread);  
+    // Thread th_de(de_thread);
+     
+     //Thread dddd(pin2_thread);  
      while(1){
          
+          #ifdef task_ma_time
+             idle_p=1;
+          #endif
+           
+           
+           
+          #ifdef task_ma_time
+             idle_p=0;
+          #endif
+           
            
      //idle
   //   stdio_mutex.lock();
     //    printf("L: %d  mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
     // stdio_mutex.unlock();     
-         // Thread::wait(2000);
+          Thread::wait(1000);
     
      }