Diff: main.cpp

Revision:

21:5f7efc1ca8ad

Parent:

20:4ed21397e775

Child:

22:1464a3f0a290

--- a/main.cpp	Sun Jun 29 16:20:22 2014 +0000
+++ b/main.cpp	Mon Jun 30 08:19:43 2014 +0000
@@ -8,6 +8,7 @@
 
 #define Debug_cam_uart
 #define R_eye
+#define L_eye
 #define motor_on
 #define Pcontroller
 #define task_ma_time
@@ -59,27 +60,25 @@
         cam.read();
 #ifdef Debug_cam_uart
 #ifdef L_eye
-        for(int i=0; i<128; i++) {
-            if(i==64)
+        for(int i=127; i>=64; i--) {
+            if(i==127)
+                pc.printf("X");
+            else if(i==64)
                 pc.printf("X");
             else
                 pc.printf("%c", cam.sign_line_imageL[i]);
         }
-        pc.printf("           ||             ");
+        pc.printf("      ||        ");
 #endif
 #ifdef R_eye
-        for(int i=128; i>=0; i--) {
+        for(int i=64; i>=0; i--) {
             if(i==64)
                 pc.printf("X");
-            else if(i<10)
-                pc.printf("-");
-            else if(i>117)
-                pc.printf("-");
             else
                 pc.printf("%c", cam.sign_line_imageR[i]);
         }
         pc.printf("    ");
-        //pc.printf("\r\n");
+        pc.printf("\r\n");
 #endif
 #endif
 
@@ -90,60 +89,16 @@
         //pc.printf("%f\r\n",motor);
 #endif
 
-        b_r_c=(double)cam.black_centerR();
-        //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);
-        if(b_r_c<0.0){
-            pc.printf("\r\n");
-            continue;
-            }
-        //compute
-        //stand at 65
-        error=b_r_c-offset;
-        brc_df=b_r_c-last_brc;
-        
+        b_r_c=(double)cam.black_center();
         
-        if(first_time==true){
-                r_kp=0;
-                first_time=false;
-            }
-        else{
-                r_kp+=brc_df;
-                if(b_r_c==last_brc)
-                    r_kp=0;
-            }
-        last_brc=b_r_c;
-            
-        if(r_kp<0.0){
-                r_kp*=-1.0;
-                r_kp_neg=true;
-            }
-        kp=0.016/(50-(r_kp/10.0));
-        if(r_kp_neg==true){
-                r_kp_neg=false;
-                r_kp*=-1.0;
-            } 
-                
-        P=kp*error;
-
-        I=last_I*2.0/3.0+error;
-        last_I=I;
-        I=(kp*0.02/0.04)*I;
-
-        D=error-last_error;
-        last_error=error;
-        D=(kp*0.04/0.02)*D;
-
-        PID_v=P;
-
-        if(PID_v < 0.0) {
-            PID_v*=-1.0;
-            PID_v=0.077+PID_v;
-        } else if(PID_v > 0.0)
-            PID_v=0.077-PID_v;
-        else
-            PID_v=0.077;
-        pc.printf("%lf %lf %lf\r\n",PID_v, r_kp, b_r_c);
-        //pc.printf("%lf\r\n",angle);
+        PID_v=cam_to_M_ctrlr.compute(b_r_c,64);//set_angle()
+        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