Revision 2:59edff92b599, committed 2014-03-03

Comitter:

com3

Date:

Mon Mar 03 00:24:44 2014 +0000

Parent:

1:fb4277ce4d93

Child:

3:440e774cc24b

Commit message:

pid

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Mon Mar 03 00:24:44 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19

--- a/main.cpp	Fri Feb 28 10:50:11 2014 +0000
+++ b/main.cpp	Mon Mar 03 00:24:44 2014 +0000
@@ -1,12 +1,14 @@
 #include "mbed.h"
 #include "TextLCD.h"
+#include "PID.h"
 #include "common.h"
 #include <math.h>
 #include <sstream>
 
 #define MOTOR_P 30
+#define NO_IR 45
 #define LINE_LP 30
-#define LINE_FP 40
+#define LINE_FP 30
 #define LINE_ON 0xFFF0
 #define LINE_TIME 0.5
 #define R 1.0
@@ -14,21 +16,27 @@
 #define S1 15
 #define S2 10
 #define S3 5
+#define RATE 0.05
+#define P_GAIN 0.8
+#define I_GAIN 0.0
+#define D_GAIN 0.02
 //誤差errの範囲でvalue1がvalue2と等しければ0以外を、等しくなれば0を返す
 #define ERR_EQ(value1,value2,err) ( ((value1) <= ((value2)+(err)))&&((value1) >= ((value2)-(err))) )
 
 DigitalIn sw(p5);
-DigitalIn start(p29);
+DigitalIn start(p7);
 DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
 Serial motor(p9,p10);
 Serial sensor(p13,p14);
 Serial pc(USBTX, USBRX);
 TextLCD lcd(p26, p25, p24, p23, p22, p21);
 AnalogIn adcline[4] = {p16, p17, p19, p20};
+PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
 Timeout liner0;
 Timeout liner1;
 Timeout liner2;
 Timeout liner3;
+Ticker pidupdata;
 //HMC6352 dcompass(p9,p10);
 
 extern string StringFIN;
@@ -46,6 +54,9 @@
 int compassdef = 0, data = 0;
 uint8_t pingdef[4] = {0};
 
+double pidinput = 180.0;
+double compasspid = 0;
+
 double way[8][2] = {
     { 0    , 1    },
     {-0.707, 0.707},
@@ -145,7 +156,64 @@
 }
 void line_stop3(){
     line_stop[3] = 0;
-}        
+}
+
+void line_check()
+{
+    if(!line_stop[0]){
+        if(adcline[0].read_u16() > LINE_ON){
+            line[0] = 1;
+            line_stop[0] = 1;
+            myled[0] = 1;
+            liner0.attach(&line_stop0,LINE_TIME);
+        } else {
+            line[0] = 0;
+            myled[0] = 0;
+        }
+    }
+    if(!line_stop[1]){
+        if(adcline[1].read_u16() > LINE_ON){
+            line[1] = 1;
+            line_stop[1] = 1;
+            myled[1] = 1;
+            liner1.attach(&line_stop1,LINE_TIME);
+        } else {
+            line[1] = 0;
+            myled[1] = 0;
+        }
+    }
+    if(!line_stop[2]){
+        if(adcline[2].read_u16() > LINE_ON){
+            line[2] = 1;
+            line_stop[2] = 1;
+            myled[2] = 1;
+            liner2.attach(&line_stop2,LINE_TIME);
+        } else {
+            line[2] = 0;
+            myled[2] = 0;
+        }
+    }
+    if(!line_stop[3]){
+        if(adcline[3].read_u16() > LINE_ON){
+            line[3] = 1;
+            line_stop[3] = 1;
+            myled[3] = 1;
+            liner3.attach(&line_stop3,LINE_TIME);
+        } else {
+            line[3] = 0;
+            myled[3] = 0;
+        }
+    }
+}
+
+
+void pidupdate()
+{
+    pidinput = compass;
+    pid.setProcessValue(pidinput);
+    
+    compasspid = -pid.compute();
+}
 
 int main() {
     
@@ -160,6 +228,12 @@
     sensor.attach(&micon_rx,Serial::RxIrq);         //送信空き割り込み（センサ用）
     //compassdef = (compass / 10);           //コンパス初期値保存
     //compassdef = (dcompass.sample() / 10);
+    pid.setInputLimits(0.0,360.0);
+    pid.setOutputLimits(-30.0,30.0);
+    pid.setBias(0.0);
+    pid.setMode(AUTO_MODE);
+    pid.setSetPoint(180.0);
+    pidupdata.attach(&pidupdate, 0.05);
     
     sw.mode(PullUp);
     start.mode(PullUp);
@@ -167,10 +241,68 @@
     myled[0] = 1;
     while(start){}
     myled[0] = 0;
-/*
+    
+ /*   
+    while(1){
+        s = compasspid;
+        pc.printf("%d\n", s);
+        wait(0.1);
+    }
+    */
+
+    while(1){
+        line_check();
+        
+        //x = MOTOR_P;
+        //y = 0;
+        if(ir_num > 7){
+            ir_num = 0;
+        }
+        x = MOTOR_P * way[ir_num][0];
+        y = MOTOR_P * way[ir_num][1];
+        s = compasspid;
+        
+        if(value_ir > NO_IR){
+            home();
+        }
+        
+        line_state();
+        
+        move(x,y,s);
+    }
     while(1){
-        //x = 30;
-        //y = 35;
+        line_check();
+        //pc.printf("%d\n", adcline[1].read_u16());
+        //pc.printf("%d %d %d %d\n", line[0], line[1], line[2], line[3]);
+        if(ir_num > 7){
+            ir_num = 0;
+        }
+        x = MOTOR_P;//MOTOR_P * way[ir_num][0];
+        y = 0;//MOTOR_P * way[ir_num][1];
+        s = (compass - 180) / 3;
+        if(s > S1){
+            s = S1;
+        } else if(s < -S1){
+            s = -S1;
+        } else if(s > S2){
+            s = S2;
+        } else if(s < -S2){
+            s = -S2;
+        } else if(s > 0){
+            s = S3;
+        } else if(s < 0){
+            s = -S3;
+        }
+                
+        line_state();
+        
+        move(x,y,s);        
+    }
+    
+/*    
+    while(1){
+        x = MOTOR_P * way[num][0];
+        y = MOTOR_P * way[num][1];
         s = (compass - 180) / 3;
         if(s > S1){
             s = S1;
@@ -185,17 +317,35 @@
         } else if(s < -S3){
             s = -S3;
         }
+        if(!sw){
+            num++;
+            wait(0.2);
+            if(num > 7){
+                num = 0;
+            }
+            lcds(num);
+        }
+        //pc.printf("%d\n", s);
+        
         move(x,y,s);
+        //wait(0.1);
+    }
+*/
+/*
+    while(1){
+        x = 30;
+        y = 0;
+        move(x,y,s);
+        wait(0.5);
+        x = -30;
+        y = 0;
+        move(x,y,s);
+        wait(0.5);
         //pc.printf("%d\n", s);
         //pc.printf("%d\n", adcline[3].read_u16());
     }
 */
-/*    while(1){
-        //lcd_ping();
-        lcd_line();
-        wait(0.2);
-    }
-*/
+
 /*    while(1){
         if(ir_num > 7){
             ir_num = 0;
@@ -211,96 +361,7 @@
         
         move(x,y,s);
     }
-    */
-    while(1){
-        if(!line_stop[0]){
-            if(adcline[0].read_u16() > LINE_ON){
-                line[0] = 1;
-                line_stop[0] = 1;
-                myled[0] = 1;
-                liner0.attach(&line_stop0,LINE_TIME);
-            } else {
-                line[0] = 0;
-                myled[0] = 0;
-            }
-        }
-        if(!line_stop[1]){
-            if(adcline[1].read_u16() > LINE_ON){
-                line[1] = 1;
-                line_stop[1] = 1;
-                myled[1] = 1;
-                liner1.attach(&line_stop1,LINE_TIME);
-            } else {
-                line[1] = 0;
-                myled[1] = 0;
-            }
-        }
-        if(!line_stop[2]){
-            if(adcline[2].read_u16() > LINE_ON){
-                line[2] = 1;
-                line_stop[2] = 1;
-                myled[2] = 1;
-                liner2.attach(&line_stop2,LINE_TIME);
-            } else {
-                line[2] = 0;
-                myled[2] = 0;
-            }
-        }
-        if(!line_stop[3]){
-            if(adcline[3].read_u16() > LINE_ON){
-                line[3] = 1;
-                line_stop[3] = 1;
-                myled[3] = 1;
-                liner3.attach(&line_stop3,LINE_TIME);
-            } else {
-                line[3] = 0;
-                myled[3] = 0;
-            }
-        }
-        //pc.printf("%d\n", adcline[1].read_u16());
-        //pc.printf("%d %d %d %d\n", line[0], line[1], line[2], line[3]);
-        if(ir_num > 7){
-            ir_num = 0;
-        }
-        x = 30;//MOTOR_P * way[ir_num][0];
-        y = 0;//MOTOR_P * way[ir_num][1];
-        s = (compass - 180) / 3;
-        if(s > S1){
-            s = S1;
-        } else if(s < -S1){
-            s = -S1;
-        } else if(s > S2){
-            s = S2;
-        } else if(s < -S2){
-            s = -S2;
-        } else if(s > S3){
-            s = S3;
-        } else if(s < -S3){
-            s = -S3;
-        }
-        line_state();
-        move(x,y,s);        
-    }    
-    //y = 20;
-    
-    while(1){
-        x = 20 * way[num][0];
-        y = 20 * way[num][1];
-        s = (compass - 180) / 3;
-        
-        if(!sw){
-            num++;
-            wait(0.2);
-            if(num > 7){
-                num = 0;
-            }
-            lcds(num);
-        }
-        //pc.printf("%d\n", s);
-        
-        move(x,y,s);
-        //wait(0.1);
-    }
+    */    
 /*    
     while(1){
         i = 3;