Hajime Muraki
main
Dependencies: TextLCD mbed PID
Diff: main.cpp
- Revision:
- 2:59edff92b599
- Parent:
- 1:fb4277ce4d93
- Child:
- 3:440e774cc24b
--- 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;