Revision 23:d6d4e8adf7fe, committed 2014-07-02

Comitter:

physicsgood

Date:

Wed Jul 02 13:33:49 2014 +0000

Parent:

22:1464a3f0a290

Commit message:

QQQQ

Changed in this revision

--- a/camera_api.cpp	Tue Jul 01 13:09:06 2014 +0000
+++ b/camera_api.cpp	Wed Jul 02 13:33:49 2014 +0000
@@ -35,7 +35,7 @@
 int BX_camera::black_centerR(void)//64~0 right eye
 {   
     int black_R_left = 0;
-    for(int i = 64; i >=6; i--){
+    for(int i = 120; i >=8; i--){
         if(sign_line_imageR[i] == 'O' && sign_line_imageR[i-1] == ' ' && sign_line_imageR[i-2] == ' '){
             return i;
         }
@@ -45,8 +45,8 @@
 int BX_camera::black_centerL(void)
 {
      int black_L_right = 128;
-     for(int i = 64; i <123 ; i++){
-        if(sign_line_imageR[i] == 'O' && sign_line_imageR[i+1] == ' ' && sign_line_imageR[i+2] == ' '){
+     for(int i = 8; i <121 ; i++){
+        if(sign_line_imageL[i] == 'O' && sign_line_imageL[i+1] == ' ' && sign_line_imageL[i+2] == ' '){
             return i;
         }
     }

--- a/controller.cpp	Tue Jul 01 13:09:06 2014 +0000
+++ b/controller.cpp	Wed Jul 02 13:33:49 2014 +0000
@@ -7,7 +7,7 @@
 PID::PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval) {
  
  //BX tune
-    Kp = 0.0004;
+    Kp = 0.0005;
     Ki = 0.0;
     Kd = 0.0;
     setInputLimits(in_min,in_max);
@@ -120,14 +120,117 @@
  
     
      
-float PID::compute(float center ,  float sp) {
+float PID::compute(int centerL, int centerR ,  int sp) {
     //turn right 122~64   122
     //turn left  64~6     8
-    float C = center;
-    float goal = sp;  // center of black
-    float error = goal - C;//
-    return 0.073+Kp*error;
- 
+       int  errorR = sp - centerR;
+       int  errorL = sp - centerL;
+       int error;
+        if(centerL ==128 && centerR == 0){
+            error = 0;
+        }
+        if (centerL == 128 && centerR != 0){
+            error = -30;
+        }
+        else if (centerL != 128 && centerR == 0){
+            error = 30;
+        }
+        else{
+            if(errorR >= 0 && errorL >=0){
+                if(errorR > errorL){
+                    if(errorR > -5 && errorR < 5)
+                        error = 0;
+                    else 
+                        error = errorR;
+                }
+                else{
+                    if(errorL > -5 && errorL < 5)
+                        error = 0;
+                    else
+                        error = errorL;
+                }
+                
+            }
+            else if(errorR < 0 && errorL >=0){
+                if(-errorR > errorL){
+                    if(errorR > -5 && errorR < 5)
+                        error = 0;
+                    else
+                        error = errorR;
+                }
+                else{
+                    if(errorL > -5 && errorL < 5)
+                        error = 0;
+                    else
+                        error = errorL;
+                }         
+            }
+             else if(errorR >= 0 && errorL <0){
+                if(errorR > -errorL){
+                    if(errorR > -5 && errorR < 5)
+                        error = 0;
+                    else
+                        error = errorR;
+                }
+                else{ 
+                    if(errorL > -5 && errorL < 5)
+                        error = 0;
+                    else 
+                        error = errorL; 
+                }         
+            }
+            else{
+                if(errorR > errorL){
+                    if(errorL > -5 && errorL < 5)
+                        error = 0;
+                    else
+                        error = errorL;
+                }
+                else{
+                    if(errorR > -5 && errorR < 5)
+                        error = 0;
+                    else 
+                        error = errorR;
+                }
+            }
+        }
+        return 0.069 + Kp*error;
+}
+
+int PID::getCenter(int centerL, int centerR) {
+    //turn right 122~64   122
+    //turn left  64~6     8
+    
+    int result = 64;
+    int errorR = 64 - centerR;
+    int errorL = 64 - centerL;
+    int negL = 1 , negR = 1;
+    
+    
+        
+
+        if(centerL == 128 && centerR == 0){// no black line
+
+            result = 64;
+
+        } else if(centerL == 128 && centerR != 0){// no left line
+
+            result = centerR;
+
+
+        } else if(centerL != 128 && centerR == 0){// no right line
+
+            result = centerL;
+
+        } else{// two black lines
+
+            negL = (errorL >= 0) ? 1 : -1;
+            negR = (errorR >= 0) ? 1 : -1;
+            errorL *= negL;
+            errorR *= negR;
+
+            result = (errorL >= errorR) ? centerL : centerR;
+        }
 }
  
 float PID::getInMin() {

--- a/controller.h	Tue Jul 01 13:09:06 2014 +0000
+++ b/controller.h	Wed Jul 02 13:33:49 2014 +0000
@@ -71,7 +71,8 @@
     * PID calculation.
     * @return The controller output as a float between outMin and outMax.
     */
-    float compute(float center, float sp);
+    float compute(int centerL, int centerR, int sp);
+    int getCenter(int centerL, int centerR);
 
     //Getters.
     float getInMin();

--- 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