main
Dependencies: TextLCD mbed PID
main.cpp
- Committer:
- com3
- Date:
- 2014-03-12
- Revision:
- 3:440e774cc24b
- Parent:
- 2:59edff92b599
- Child:
- 4:536cd493a337
File content as of revision 3:440e774cc24b:
#include "mbed.h" #include "TextLCD.h" #include "PID.h" #include "common.h" #include <math.h> #include <sstream> #define MOTOR_P 26 #define HOME_P 25 #define NO_BALL 45 #define LINE_LP 30 #define LINE_FP 35 #define LINE_ON 0xFFF0 #define LINE_TIME 0.6 #define LINE_PING 40 #define R 1.0 #define S_MAX 5 #define S1 15 #define S2 10 #define S3 5 #define RATE 0.05 #define P_GAIN 1.2 #define I_GAIN 0.0 #define D_GAIN 0.015 #define HOME_BACK 30 #define HOME 1 #define GOAL 30.0 #define PING_LR 100 //誤差errの範囲でvalue1がvalue2と等しければ0以外を、等しくなれば0を返す #define ERR_EQ(value1,value2,err) ( ((value1) <= ((value2)+(err)))&&((value1) >= ((value2)-(err))) ) DigitalIn sw(p5); 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; 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, ir_dis = 0, ball_on = 0; uint8_t ping[4] = {0}; uint8_t line[4] = {0}, line_stop[4] = {0}, line_ping[4] = {0}; uint8_t kick = 0; //uint8_t robot_state = 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 pidinput = 180.0; double compasspid = 0.0; double goal = 0.0; double way[10][2] = { { 0 , 1 }, //FF {-0.707, 0.707}, //FL {-1 , 0 }, //LL {-0.707,-0.707}, //BL { 0 ,-1 }, //BB { 0.707,-0.707}, //BR { 1 , 0 }, //RR { 0.707, 0.707}, //FR {-0.500, 0.866}, //FFL { 0.500, 0.866} //FFR }; double ball_way[12][2] = { { 0.0 , 1.0 }, //FF {-0.985,-0.173}, //FL {-0.342,-0.940}, //LL { 0.342,-0.940}, //BL { 0.966, 0.259}, //BB {-0.342,-0.940}, //BR { 0.342,-0.940}, //RR { 0.985,-0.173}, //FR {-0.985, 0.174}, //FFL { 0.985, 0.174}, //FFR {-0.173, 0.985}, //FFFL { 0.173, 0.985} //FFFR }; double all_way[10][2] = { { 0 , 1 }, //FF {-0.707, 0.707}, //FL {-0.939,-0.342}, //LL {-1 , 0 }, {-0.707,-0.707}, //BL { 0 ,-1 }, //BB { 0.707,-0.707}, //BR { 0.939,-0.342}, //RR { 1 , 0 }, { 0.707, 0.707}, //FR {-0.500, 0.866}, //FFL { 0.500, 0.866} //FFR }; 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] = kick; 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[LEFT]){ x = LINE_LP; } else if(line_ping[LEFT]){ if(ping[LEFT] < LINE_PING){ if(x < 10){ x = 10; } } else { line_ping[LEFT] = 0; myled[1] = 0; } } if(line[BACK]){ y = LINE_FP; } else if(line_ping[BACK]){ if(ping[BACK] < LINE_PING){ if(y < 10){ y = 10; } } else { line_ping[BACK] = 0; myled[2] = 0; } } if(line[RIGHT]){ x = -LINE_LP; } else if(line_ping[RIGHT]){ if(ping[RIGHT] < LINE_PING){ if(x > 10){ x = -10; } } else { line_ping[RIGHT] = 0; myled[3] = 0; } } if(line[FRONT]){ y = -LINE_FP; } else if(line_ping[FRONT]){ if(ping[FRONT] < LINE_PING){ if(y > 10){ y = -10; } } else { line_ping[FRONT] = 0; myled[0] = 0; } } } 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 line_stop0(){ line_stop[FRONT] = 0; } void line_stop1(){ line_stop[LEFT] = 0; } void line_stop2(){ line_stop[BACK] = 0; } void line_stop3(){ line_stop[RIGHT] = 0; } void line_check() { if(!line_stop[FRONT]){ if(adcline[FRONT].read_u16() > LINE_ON){ line[FRONT] = 1; line_stop[FRONT] = 1; line_ping[FRONT] = 1; myled[0] = 1; liner0.attach(&line_stop0,1); } else { line[FRONT] = 0; ////myled[0] = 0; } } if(!line_stop[LEFT]){ if(adcline[LEFT].read_u16() > LINE_ON){ line[LEFT] = 1; line_stop[LEFT] = 1; line_ping[LEFT] = 1; myled[1] = 1; liner1.attach(&line_stop1,LINE_TIME); } else { line[LEFT] = 0; ////myled[1] = 0; } } if(!line_stop[BACK]){ if(adcline[BACK].read_u16() > LINE_ON){ line[BACK] = 1; line_stop[BACK] = 1; line_stop[FRONT] = 1; line_ping[BACK] = 1; myled[2] = 1; liner0.attach(&line_stop0,LINE_TIME); liner2.attach(&line_stop2,LINE_TIME); } else { line[BACK] = 0; ////myled[2] = 0; } } if(!line_stop[RIGHT]){ if(adcline[RIGHT].read_u16() > LINE_ON){ line[RIGHT] = 1; line_stop[RIGHT] = 1; line_ping[RIGHT] = 1; myled[3] = 1; liner3.attach(&line_stop3,LINE_TIME); } else { line[RIGHT] = 0; ////myled[3] = 0; } } } void pidupdate() { pid.setSetPoint(180.0 + goal); pidinput = compass; pid.setProcessValue(pidinput); compasspid = -pid.compute(); } void home() { x = 0; y = 0; kick = 0; goal = 0.0; // if((145 < ping[LEFT]+ping[RIGHT]) && (ping[LEFT]+ping[RIGHT] < 155)){ if(ping[LEFT] > PING_LR){ x = -HOME_P; y = -5; } else if(ping[RIGHT] > PING_LR){ x = HOME_P; y = -5; } // } if(ping[BACK] > HOME_BACK){ y = -HOME_P; } else if(ping[BACK] < 5){ y = HOME_P; } } int main() { //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20); //uint8_t num = 0; uint8_t dir = 0, power = 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); 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); lcd.cls(); lcd.locate(0,0); lcd.printf("Chronograph"); myled[0] = 1; while(!start){ lcd.locate(6,1); lcd.printf("%d", compass); wait(0.1); } myled[0] = 0; lcd.cls(); lcd.locate(0,0); lcd.printf("Chronograph"); lcd.locate(7,1); lcd.printf("ABURASOBA"); while(1){ x = 0; y = 0; s = compasspid; power = MOTOR_P; if(/*(ir_dis < 60) && */( (ir_num == FF) || (ir_num == FFL) || (ir_num == FFR) )){ kick = 25; //if(130 < ping[LEFT]+ping[RIGHT]){ //if(goal == 0){ /* if(ping[LEFT] > 100){ goal = -GOAL; }*//* else if(ping[RIGHT] > 100){ //goal = GOAL; }*/ //} //} } else { kick = 0; goal = 0.0; } if((ir_dis > 50) && (ir_dis < 150) && (ir_num != BB) && (ir_num != FL) && (ir_num != FR) && (ir_num != BL) && (ir_num != BR)){ x = power * way[ir_num][0]; y = power * way[ir_num][1]; } else { if((ir_num == FF) || (ir_num == FFFL) || (ir_num == FFFR)){ power = MOTOR_P + 5; } else if((ir_num == FFL) || (ir_num == FFR)){ power = MOTOR_P; } else { power = MOTOR_P + 5; } x = power * ball_way[ir_num][0]; y = power * ball_way[ir_num][1]; if(ir_num == BB){ if(ping[LEFT] > ping[RIGHT]){ x = -x; } } } line_check(); if((ir_num == NO_BALL)/* || (ball_on < 40)*/){ home(); } line_state(); if((x == 0) && (y == 0)){ if(ping[BACK] > 25){ y = -HOME_P; } } //x = 0; y = 0; move(x,y,s); } }