Diff: main.cpp

Revision:

23:d6d4e8adf7fe

Parent:

22:1464a3f0a290

--- a/main.cpp	Tue Jul 01 13:09:06 2014 +0000
+++ b/main.cpp	Wed Jul 02 13:33:49 2014 +0000
@@ -7,8 +7,8 @@
 
 
 #define Debug_cam_uart
-#define R_eye
-#define L_eye
+//#define R_eye
+//#define L_eye
 #define motor_on
 #define Pcontroller
 #define task_ma_time
@@ -46,10 +46,11 @@
 void run()
 {
     double motor, angle=0.0;
-    int centerR,centerL;
+    int centerR,centerL,center;
     double PID_v;
-    double error , P, I, D, kp, r_kp;
-    double last_error=0.0 ,last_I=0.0, last_brc, brc_df;
+    double P, I, D, kp = 0.0004, r_kp;
+    int errorR,errorL,error,last_error;
+    double last_I=0.0, last_brc, brc_df;
     bool first_time=true, r_kp_neg=false;
 
     while(1) {
@@ -61,7 +62,7 @@
 #ifdef Debug_cam_uart
 #ifdef L_eye
         pc.printf("X");
-        for(int i=122; i>=64; i--) {
+        for(int i=122; i>=44; i--) {
             if(i==64)
                 pc.printf("X");
             else
@@ -70,7 +71,7 @@
         pc.printf("      ||        ");
 #endif
 #ifdef R_eye
-        for(int i=64; i>=6; i--) {
+        for(int i=84; i>=6; i--) {
             if(i==64)
                 pc.printf("X");
             else
@@ -90,27 +91,22 @@
 
         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
+        center = cam_to_M_ctrlr.getCenter(centerL,centerR);
+        
+        PID_v = (double)cam_to_M_ctrlr.compute(centerL,centerR,64);
+        //servo.set_angle(PID_v);
+        servo.set_angle(PID_v);
+        pc.printf("centerL:   %d       centerR:   %d       center:   %d",centerL,centerR,center);
+        pc.printf("\r\n");
+        for(int i=118; i>=10; i--){
+            if(i==64)
+                pc.printf("X");
+            else if(i>=center-4 && i<=center+4)
+                pc.printf(" ");
+            else
+                pc.printf("O");
         }
-        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("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;
 #ifdef task_ma_time
         task_pin=0;
 #endif