Diff: main.cpp

Revision:

22:1464a3f0a290

Parent:

21:5f7efc1ca8ad

Child:

23:d6d4e8adf7fe

--- a/main.cpp	Mon Jun 30 08:19:43 2014 +0000
+++ b/main.cpp	Tue Jul 01 13:09:06 2014 +0000
@@ -46,7 +46,7 @@
 void run()
 {
     double motor, angle=0.0;
-    double b_r_c;
+    int centerR,centerL;
     double PID_v;
     double error , P, I, D, kp, r_kp;
     double last_error=0.0 ,last_I=0.0, last_brc, brc_df;
@@ -60,10 +60,9 @@
         cam.read();
 #ifdef Debug_cam_uart
 #ifdef L_eye
-        for(int i=127; i>=64; i--) {
-            if(i==127)
-                pc.printf("X");
-            else if(i==64)
+        pc.printf("X");
+        for(int i=122; i>=64; i--) {
+            if(i==64)
                 pc.printf("X");
             else
                 pc.printf("%c", cam.sign_line_imageL[i]);
@@ -71,13 +70,13 @@
         pc.printf("      ||        ");
 #endif
 #ifdef R_eye
-        for(int i=64; i>=0; i--) {
+        for(int i=64; i>=6; i--) {
             if(i==64)
                 pc.printf("X");
             else
                 pc.printf("%c", cam.sign_line_imageR[i]);
         }
-        pc.printf("    ");
+        pc.printf("X");
         pc.printf("\r\n");
 #endif
 #endif
@@ -89,18 +88,29 @@
         //pc.printf("%f\r\n",motor);
 #endif
 
-        b_r_c=(double)cam.black_center();
-        
-        PID_v=cam_to_M_ctrlr.compute(b_r_c,64);//set_angle()
+        centerR=cam.black_centerR();
+        centerL=cam.black_centerL();
+        if(centerR == 0 && centerL ==128){
+            servo.set_angle(0.073);
+        }
+        else if(centerR == 0 && centerL !=128){//turn right
+            PID_v=cam_to_M_ctrlr.compute(centerL,120);//set_angle()
+            servo.set_angle(PID_v);//122~64   
+        }
+        else if(centerR != 0 && centerL ==128){//turn left
+            PID_v=cam_to_M_ctrlr.compute(centerR,8);//set_angle()
+            servo.set_angle(PID_v);//64~6  64
+        }
+        else{
+            PID_v=cam_to_M_ctrlr.compute((centerR+centerL)/2,64);//set_angle()
+            servo.set_angle(PID_v);//64~6  64
+        }
+        pc.printf("centerL:   %d       centerR:   %d",centerL,centerR);
         pc.printf("\r\n");
-        //pc.printf("b_r_c:  %f\r\n",b_r_c);
         //pc.printf("angle:  %f\r\n",PID_v);
          //pc.printf("%f %d %d speed :%f  bk_center %f PID:%f \r\n",cam_to_M_ctrlr.de_v,cam.de_v,cam.de_v2,motor,center,PID_v);
             //center range 0~128
             last_error = error;
-        //pc.printf("%d %d speed :%f  bk_center %f PID:%f \r\n",cam.de_v,cam.de_v2,motor,b_r_c,PID_v);
-        servo.set_angle(PID_v);
-
 #ifdef task_ma_time
         task_pin=0;
 #endif