Diff: main.cpp

Revision:

11:ffd762ae141b

Parent:

10:d2401a243e8d

--- a/main.cpp	Wed Jun 25 05:07:13 2014 +0000
+++ b/main.cpp	Wed Jun 25 05:59:29 2014 +0000
@@ -50,6 +50,10 @@
       double motor;
      double b_r_c;
      double PID_v;
+     
+     double Kp = 0.0005, Ki = 0.0001, Kd = 0.0001;
+     double integral = 0, derivative = 0, error, last_error = 0, turn;
+     
      while(1){
          
         #ifdef task_ma_time
@@ -145,16 +149,21 @@
          //PID_v=cam_to_M_ctrlr.compute(b_r_c,15.0);
          //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,b_r_c,PID_v);
             pc.printf("b_r_c       %lf\r\n",b_r_c);
-            int error = b_r_c-64;
-            if(error<8&&error>-8){
+            
+            error = b_r_c - 64;
+            integral = 0.3 * integral + error;  //0.3 is the factor to make old datas become smaller
+            derivative = error - last_error;
+            
+            turn = error*Kp + integral*Ki + derivative*Kd;
+            
+            if(error < 8 && error > -8){
                 servo.set_angle(0.085);
             }
-            else if(error<-8){
-                servo.set_angle(0.1);
+            else{
+                servo.set_angle(0.085 + turn);
             }
-            else if(error>8){
-                 servo.set_angle(0.07);
-            }
+            
+            leas_error = error;
          
         //servo.set_angle(PID_v);