Diff: main.cpp

Revision:

12:f7acda4545ba

Parent:

11:6189b2fc618a

Child:

13:0f0d341d6d61

--- a/main.cpp	Wed Jun 25 05:50:03 2014 +0000
+++ b/main.cpp	Mon Jun 30 02:07:57 2014 +0000
@@ -26,7 +26,7 @@
 BX_pot pot1('1');
 
                                  // 90/30=3
-PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,(0.104/30),0.0,0.0,10);
+PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,0.00125,0.0,0.0);
 //PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval)
 //in_min in_out  camera array
 //out_min out_max servo range
@@ -38,10 +38,10 @@
     
     
        pc.baud(115200);
+       
      while(1){
-         
-       if(tsi.readPercentage()>0.00011)
-           break;
+      if(tsi.readPercentage()>0.00011)
+          break;
      }
      
     
@@ -50,6 +50,10 @@
       double motor;
      double b_r_c;
      double PID_v;
+     
+     //double Kp = 0.00078, Ki = 0.0000, Kd = 0.0000;
+     double integral = 0, derivative = 0, error = 0, last_error = 0, turn = 0;
+     
      while(1){
          
         #ifdef task_ma_time
@@ -142,21 +146,28 @@
        
                b_r_c=(double)cam.black_centerR();
 
-         //PID_v=cam_to_M_ctrlr.compute(b_r_c,15.0);
+         PID_v=cam_to_M_ctrlr.compute(200,b_r_c,20.0);//set_angle()
          //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 = 64-b_r_c;
-            if(error<10&&error>-10){
+            //b_r_c range 0~128
+            /*error = 20 - b_r_c;
+            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<-10){
-                servo.set_angle(0.0005*error+0.085);
+            else if (error > 8 && error <108){
+                servo.set_angle(0.085+turn);
             }
-            else if(error>10){
-                 servo.set_angle(0.0005*error+0.085);
-            }
+            else if (error <-8 && error>-20){
+                servo.set_angle(0.085+turn);
+            }*/
+            
+            last_error = error;
          
-        //servo.set_angle(PID_v);
+        servo.set_angle(PID_v);