main
Dependencies: TextLCD mbed PID
main.cpp
- Committer:
- com3
- Date:
- 2014-02-28
- Revision:
- 1:fb4277ce4d93
- Parent:
- 0:e6d14fec4954
- Child:
- 2:59edff92b599
File content as of revision 1:fb4277ce4d93:
#include "mbed.h" #include "TextLCD.h" #include "common.h" #include <math.h> #include <sstream> #define MOTOR_P 30 #define LINE_LP 30 #define LINE_FP 40 #define LINE_ON 0xFFF0 #define LINE_TIME 0.5 #define R 1.0 #define S_MAX 5 #define S1 15 #define S2 10 #define S3 5 //誤差errの範囲でvalue1がvalue2と等しければ0以外を、等しくなれば0を返す #define ERR_EQ(value1,value2,err) ( ((value1) <= ((value2)+(err)))&&((value1) >= ((value2)-(err))) ) DigitalIn sw(p5); DigitalIn start(p29); 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}; Timeout liner0; Timeout liner1; Timeout liner2; Timeout liner3; //HMC6352 dcompass(p9,p10); extern string StringFIN; extern void array(int,int,int,int); extern void micon_rx(void); //uint16_t analogHex[4] = {0}; int speed[4] = {0}; uint8_t value_ir = 0, ir_num = 0; uint8_t ping[4] = {0}; uint8_t line[4] = {0}, line_stop[4] = {0}; uint8_t back = 0; int compass = 0; int x = 0, y = 0, s = 0, i = 0, line_on = 0; int compassdef = 0, data = 0; uint8_t pingdef[4] = {0}; double way[8][2] = { { 0 , 1 }, {-0.707, 0.707}, {-1 , 0 }, {-0.707,-0.707}, { 0 ,-1 }, { 0.707,-0.707}, { 1 , 0 }, { 0.707, 0.707} }; void move(int vx, int vy, int vs){ double pwm[4] = {0}; pwm[0] = (double)((vx) + vs); pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs); pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs); pwm[3] = 0; for(i = 0; i < 4; i++){ if(pwm[i] > 100){ pwm[i] = 100; } else if(pwm[i] < -100){ pwm[i] = -100; } speed[i] = pwm[i]; } } //通信(モータ用) void tx_motor(){ array(speed[0],speed[1],speed[3],speed[2]); motor.printf("%s",StringFIN.c_str()); } //ライン判断 void line_state(){ if(line[0]){ y = -LINE_FP; } if(line[1]){ x = LINE_LP; } if(line[2]){ y = LINE_FP; } if(line[3]){ x = -LINE_LP; } } void lcds(int lcd1){ lcd.cls(); lcd.locate(0, 0); lcd.printf("%d", lcd1); //lcd.locate(2, 1); //lcd.printf("YATTIYATTI"); } void lcd_ping(){ lcd.cls(); lcd.locate(0,0); lcd.printf("%03d %03d\n%03d %03d", ping[FRONT], ping[LEFT], ping[BACK], ping[RIGHT]); } void lcd_line(){ lcd.cls(); lcd.locate(0,0); lcd.printf("%03d %03d\n%03d %03d", line[FRONT], line[LEFT], line[BACK], line[RIGHT]); } void home(){ if(ping[LEFT] > 80){ x = -20; } else if(ping[RIGHT] > 80){ x = 20; } back = 30; /* if(EER_EQ(ping[LEFT]+ping[RIGHT],pingdef[LEFT]+pingdef[RIGHT], 5)){ back = 10; }*/ if(ping[BACK] > back){ y = -20; } } void line_stop0(){ line_stop[0] = 0; } void line_stop1(){ line_stop[1] = 0; } void line_stop2(){ line_stop[2] = 0; } void line_stop3(){ line_stop[3] = 0; } int main() { //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20); uint8_t num = 0; wait(1); motor.baud(115200); //ボーレート設定 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み(モータ用) sensor.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み(センサ用) //compassdef = (compass / 10); //コンパス初期値保存 //compassdef = (dcompass.sample() / 10); sw.mode(PullUp); start.mode(PullUp); myled[0] = 1; while(start){} myled[0] = 0; /* while(1){ //x = 30; //y = 35; 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; } move(x,y,s); //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; } x = MOTOR_P * way[ir_num][0]; y = MOTOR_P * way[ir_num][1]; s = (compass - 180) / 3; if(s > S_MAX){ s = S_MAX; } else if(s < -S_MAX){ s = -S_MAX; } 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; analogHex[i] = adcIn[i].read_u16(); if(analogHex[i] > 10000){ myled[i] = 1; line[i] = 1; line_on = 1; } else { myled[i] = 0; line[i] = 0; line_on = 0; } x = 30; y = 0; if(line[1]){ x = LINE_P; } else if(line[3]){ x = -LINE_P; } if(line[0]){ y = -LINE_P; } else if(line[2]){ y = LINE_P; } move(x,y,s); if(line_on)wait(0.3); } */ /* while(1){ for(i = 0; i < 4; i++){ analogHex[i] = adcIn[i].read_u16(); if(analogHex[i] > 10000){ myled[i] = 1; line[i] = 1; } else { myled[i] = 0; line[i] = 0; } //wait(0.1); } if(line[0]){ x = -30; } else if(line[2]){ x = 30; } if(line[1]){ y = 30; } else if(line[3]){ y = -30; } move(x,y); x = 0; y = 0; //pc.printf("%05d %05d %05d %05d\n", analogHex[0], analogHex[1], analogHex[2], analogHex[3]); } */ }