Revision 14:303b22b76d7a, committed 2014-06-28

Comitter:

TonyLin

Date:

Sat Jun 28 07:32:39 2014 +0000

Parent:

13:a33a7705fe2b

Commit message:

Dynamic kp!!!!

Changed in this revision

--- a/main.cpp	Sat Jun 28 07:06:54 2014 +0000
+++ b/main.cpp	Sat Jun 28 07:32:39 2014 +0000
@@ -8,17 +8,14 @@
 
 #define Debug_cam_uart
 #define R_eye
-#define motor_on 
+#define motor_on
 #define Pcontroller
 #define task_ma_time
 #define offset 65
-#define kp 1
-#define ki 0.1
-#define kd 1
 #include "TSISensor.h"
 
 Serial pc(USBTX, USBRX);
-BX_servo servo; 
+BX_servo servo;
 BX_camera cam;
 BX_motor MotorA('A');
 BX_motor MotorB('B');
@@ -30,101 +27,120 @@
 
 void run();
 
-int main(){
+int main()
+{
     pc.baud(115200);
-    
-    while(1){    
+
+    while(1) {
         if(tsi.readPercentage()>0.00011)
-           break;
+            break;
     }
     run();
-    
-       pc.baud(115200);
+
+    pc.baud(115200);
     return 0;
-    }
+}
 
 
 
-void run(){
-     double motor;
-     double b_r_c;
-     double PID_v;
-     
-     while(1){   
-        #ifdef task_ma_time
-             task_pin=1;
-        #endif 
+void run()
+{
+    double motor;
+    double b_r_c;
+    double PID_v;
+    double error , P, I, D, kp, r_kp;
+    double last_error=0.0 ,last_I=0.0, last_brc, brc_df;
+    bool first_time=true, r_kp_neg=false;
+
+    while(1) {
+#ifdef task_ma_time
+        task_pin=1;
+#endif
+
+        cam.read();
+#ifdef Debug_cam_uart
+#ifdef L_eye
+        for(int i=0; i<128; i++) {
+            if(i==64)
+                pc.printf("X");
+            else
+                pc.printf("%c", cam.sign_line_imageL[i]);
+        }
+        pc.printf("           ||             ");
+#endif
+#ifdef R_eye
+        for(int i=128; 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("\r\n");
+#endif
+#endif
+
+#ifdef motor_on
+        motor=pot1.read();
+        MotorA.rotate(motor);
+        MotorB.rotate(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);
+
+        //compute
+        //stand at 65
+        error=b_r_c-offset;
+        brc_df=b_r_c-last_brc;
+        last_brc=b_r_c;
         
-        cam.read();  
-        #ifdef Debug_cam_uart
-            #ifdef L_eye  
-            for(int i=0;i<128;i++){
-                if(i==64)
-                    pc.printf("X");
-                else          
-                    pc.printf("%c", cam.sign_line_imageL[i]);
+        if(first_time==true){
+                r_kp=error;
+                first_time=false;
             }
-            pc.printf("           ||             ");
-            #endif
-            #ifdef R_eye
-                for(int i=128;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("\r\n");
-            #endif
-        #endif   
-     
-        #ifdef motor_on 
-            motor=pot1.read();
-            MotorA.rotate(motor);
-            MotorB.rotate(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);        
-        
-        //compute
-        //stand at 100
-        double error , P, I, D;
-        double last_error=0.0, last_I=0.0;
-        
-        if(error<20.0 && error>0.0)
-            PID_v=0.0;
         else{
-            error=b_r_c-offset;
-            P=kp*error;
-                
-            I=last_I*2.0/3.0+error;
-            last_I=I;
-            I=ki*I;
+                r_kp+=df;
+            }
             
-            D=error-last_error;
-            last_error=error;
-            D=kd*D;
-            
-            PID_v=P+I+D;
-        }
-        
-        if(PID_v < 0.0){
+        if(r_kp<0.0){
+                r_kp*=-1.0;
+                r_kp_neg=true;
+            }
+        kp=0.025/(4000/r_kp);
+        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+D;
+
+
+        if(PID_v < 0.0) {
             PID_v*=-1;
-            PID_v=0.085-(PID_v/125*0.025);
-            }
-        else if(PID_v > 0.0)
-            PID_v=0.085+(PID_v/125*0.025);
+            PID_v=0.085-(PID_v);
+        } else if(PID_v > 0.0)
+            PID_v=0.085+(PID_v);
         else
             PID_v=0.085;
-        
+
         servo.set_angle(PID_v);
-        
-        #ifdef task_ma_time
-            task_pin=0;
-        #endif    
-     }
+
+#ifdef task_ma_time
+        task_pin=0;
+#endif
+    }
 }
\ No newline at end of file