Diff: main.cpp

Revision:

15:585df3979be8

Parent:

14:2d90b0066fc6

--- a/main.cpp	Sat Jun 28 12:16:33 2014 +0000
+++ b/main.cpp	Sun Jun 29 14:02:25 2014 +0000
@@ -8,19 +8,19 @@
 #include "TSISensor.h"
 
 #define Debug_cam_uart
-#define L_eye
+//#define L_eye
 #define R_eye
 #define motor_on 
 #define Pcontroller
 #define task_ma_time
 
 
-#define car_center 15
+#define car_center 64
 
 
 
 
-#define t_cam 6
+#define t_cam 2
 
 
 
@@ -37,9 +37,10 @@
 BX_motor MotorB('B');
 
 BX_pot pot1('1');
+BX_pot pot2('2');
 
                                  // 90/30=3
-PID cam_to_M_ctrlr(10.0,118.0,0.059,0.105,0.105-0.059,0.00,0.02,10);
+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);
 TSISensor tsi;
@@ -51,28 +52,31 @@
 
 
 
-//static int line3[3]; //special point 
 
-static int b_r_c=0;
-static int b_l_c=118;
-
-static int l3_p=0;
+static int b_r_c=64;
+static int pre_b_r_c;
 
 static double v_motor;
 static double v_servo;
 
+
+
+
+
 void cam_thread(void const *args){
     
     while(true){
      
-        cam.read();
-        
+           cam.read();
         
-        b_r_c=cam.black_centerR();
-      
-          
+     
+            b_r_c=cam.black_centerR();
             
-        Thread::wait(t_cam);
+            if(b_r_c==-1)
+               b_r_c=pre_b_r_c;
+            
+            pre_b_r_c=b_r_c;
+            Thread::wait(t_cam);
         
         }
     
@@ -114,9 +118,9 @@
              else          
                pc.printf("%c", cam.sign_line_imageR[i]);         
          }
-         pc.printf("\r\n");
+         pc.printf("\r\n center :%d servo: %f \r\n",cam.black_centerR(),v_servo);
      #endif
-         pc.printf("L: %d  mid: %d R: %d  center: %d \r\n", );
+         
          stdio_mutex.unlock();
      
          
@@ -139,13 +143,16 @@
     
 
         while(1){
-
+        
+      
+         v_servo=cam_to_M_ctrlr.compute(b_r_c,car_center);        
+         
+         
             
-        
-         v_servo=cam_to_M_ctrlr.compute(b_r_c,car_center);
-        
-             servo.set_angle(v_servo);
-            
+
+             // v_servo=pot2.read();
+               servo.set_angle(v_servo);
+              
          Thread::wait(20);    
         }    
     
@@ -153,6 +160,12 @@
     
 }
 
+
+
+
+
+
+
 void motor_thread(void const *args){
     
         while(1){
@@ -169,26 +182,6 @@
     
     }
 
-/*
-void ctrl_thread(void const *args){
-    
-    
-    
-      while(1){
-          
-
-         cam_to_M_ctrlr.compute(b_r_c,64.0);
-          
-          Thread::wait(1); 
-      }    
-    
-    
-    
-    
-}
-*/
-
-// global resource
 
 
 
@@ -201,6 +194,7 @@
     
 // baud rate init --- no function
   
+    servo.set_angle(0.055);
     pc.baud(115200);
    /*
       while(1){
@@ -209,11 +203,15 @@
              break;
       }
     */
+  
+    
+    
+    
       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_de(de_thread);  
      while(1){