Diff: main.cpp

Revision:

10:9f0ce6ba7663

Parent:

9:33b99cb45e99

--- a/main.cpp	Tue Jun 24 10:06:54 2014 +0000
+++ b/main.cpp	Thu Jun 26 14:29:53 2014 +0000
@@ -11,156 +11,153 @@
 #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_camera cam;
-
 BX_motor MotorA('A');
 BX_motor MotorB('B');
-
-BX_pot pot1('1');
-
-                                 // 90/30=3
+BX_pot pot1('1');            // 90/30=3
+BX_pot pot2('2');
 PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,(0.104/30),0.0,0.0,10);
-
 DigitalOut task_pin(PTD1);
 TSISensor tsi;
-int main() {
+
+double Computes(double, double, double);
+void run();
+
+int main(){
+    pc.baud(115200);
     
+    while(1){    
+        if(tsi.readPercentage()>0.00011)
+           break;
+    }
+    run();
     
-       pc.baud(115200);
+    return 0;
+    }
+
+double Computes(double b_r_c, double* last_I, double* last_error){
+    //stand at 100
+    double error , P, I, D, PID;
     
-     while(1){
-         
-       if(tsi.readPercentage()>0.00011)
-           break;
-     }
-     
+    error=b_r_c-100;
+    P=kp*error;
+        
+    I=*last_I*2.0/3.0+error;
+    *last_I=I;
+    I=ki*I;
+    
+    D=error-*last_error+error;
+    *last_error=error;
+    D=kd*D;
+    
+    PID=P+I+D;
     
-     
-     
-      double motor;
+    if(PID<0.0){
+        PID*=-1;
+        PID=0.085-(PID*0.025);
+        }
+    else if(PID>0.0)
+        PID=0.085+(PID*0.025);
+    else
+        PID=0.085;
+    
+    return PID;
+    }
+
+void run(){
+     double motor;
      double b_r_c;
      double PID_v;
-     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");
-         
-         
-       
+     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   
      
-     #endif
-         
-         
-     
-            
-     
-     
-  
-     
-   
-
-#endif   
-     
-      
-     
-     
-     
-     
-     
-     
-     
-       
-         #ifdef motor_on 
-           
-     
-             motor=pot1.read();
-     
-    
-    
-  
-             MotorA.rotate(motor);
-             MotorB.rotate(motor);
+        #ifdef motor_on 
+            motor=pot1.read();
+            MotorA.rotate(motor);
+            MotorB.rotate(motor);
         #endif
        
-       
-       
-       
-       
-               b_r_c=(double)cam.black_centerR();
+        b_r_c=(double)cam.black_centerR();
 
-         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);
+        //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);
          
-         
-        servo.set_angle(PID_v);
+        //servo.set_angle(PID_v);
+        
         
         
-       
-     
-      #ifdef task_ma_time
-        task_pin=0;
-        #endif 
+        //PID_v = Computes(b_r_c, &last_I, &last_error);
+        //compute
+        //stand at 100
+        double error , P, I, D;
+        double last_error=0.0, last_I=0.0;
         
-     }
-   
-     
-     
-    
-    
-    
-    
-    return 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;
+            
+            D=error-last_error;
+            last_error=error;
+            D=kd*D;
+            
+            PID_v=P+I+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);
+        else
+            PID_v=0.085;
+        
+        servo.set_angle(PID_v);
+        
+        #ifdef task_ma_time
+            task_pin=0;
+        #endif 
+    } 
+}
\ No newline at end of file