Hajime Muraki
main
Dependencies: TextLCD mbed PID
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "TextLCD.h" 00003 #include "PID.h" 00004 #include "common.h" 00005 #include <math.h> 00006 #include <sstream> 00007 00008 #define MOTOR_P 35 00009 #define HOME_P 30 00010 #define TURN_P 20 00011 #define BALL_P 25 00012 #define NO_BALL 45 00013 #define LINE_P 30 00014 #define LINE_PLR 17 00015 #define LINE_ON 0x2710 //10000 00016 #define LINE_TIME 0.6 00017 #define LINE_PING 40 00018 #define R 1.0 00019 #define S_MAX 5 00020 #define S1 15 00021 #define S2 10 00022 #define S3 5 00023 #define RATE 0.05 00024 #define P_GAIN 1.7 00025 #define I_GAIN 0.0 00026 #define D_GAIN 0.013 00027 #define HOME_BACK 30 00028 #define HOME 1 00029 #define KICK 25 00030 #define GOAL 30.0 00031 #define PING_LR 100 00032 #define BALL_FAR 43 00033 #define BALL_NEAR 40 00034 00035 DigitalIn sw_wh(p5); 00036 DigitalIn start(p7); 00037 DigitalOut myled[4] = {LED1, LED2, LED3, LED4}; 00038 Serial motor(p9,p10); 00039 Serial sensor(p13,p14); 00040 Serial xbee(p28,p27); 00041 Serial pc(USBTX, USBRX); 00042 TextLCD lcd(p26, p25, p24, p23, p22, p21); 00043 AnalogIn adcline[4] = {p16, p17, p19, p20}; 00044 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE); 00045 Timeout liner_F; 00046 Timeout liner_L; 00047 Timeout liner_B; 00048 Timeout liner_R; 00049 Ticker pidupdata; 00050 Ticker xbeetx; 00051 Ticker xbeerx; 00052 //HMC6352 dcompass(p9,p10); 00053 00054 extern string StringFIN; 00055 extern void array(int,int,int,int); 00056 extern void xbee_tx(void); 00057 extern void xbee_rx(void); 00058 extern void micon_rx(void); 00059 00060 //uint16_t analogHex[4] = {0}; 00061 int speed[4] = {0}; 00062 uint8_t value_ir = 0, ir_num = 0, ir_dis = 0, ball_on = 0; 00063 uint8_t ping[4] = {0}; 00064 uint8_t line[4] = {0}, line_stop[4] = {0}, line_ping[4] = {0}; 00065 uint8_t kick = 0; 00066 //uint8_t robot_state = 0; 00067 int compass = 0; 00068 int x = 0, y = 0, s = 0, i = 0, line_on = 0; 00069 int compassdef = 0, data = 0; 00070 uint8_t pingdef[4] = {0}; 00071 00072 double pidinput = 180.0; 00073 double compasspid = 0.0; 00074 double goal = 0.0; 00075 00076 double way[10][2] = { 00077 { 0 , 1 }, //FF 00078 {-0.707, 0.707}, //FL 00079 {-1 , 0 }, //LL 00080 {-0.707,-0.707}, //BL 00081 { 0 ,-1 }, //BB 00082 { 0.707,-0.707}, //BR 00083 { 1 , 0 }, //RR 00084 { 0.707, 0.707}, //FR 00085 {-0.500, 0.866}, //FFL 00086 { 0.500, 0.866} //FFR 00087 }; 00088 double ball_way[12][2] = { 00089 { 0.0 , 1.0 }, //FF 00090 {-0.866,-0.500}, //FL 00091 {-0.342,-0.940}, //LL 00092 { 0.342,-0.940}, //BL 00093 { 0.966, 0.259}, //BB 00094 {-0.342,-0.940}, //BR 00095 { 0.342,-0.940}, //RR 00096 { 0.985,-0.173}, //FR 00097 {-0.985, 0.174}, //FFL 00098 { 0.985, 0.174}, //FFR 00099 {-0.173, 0.985}, //FFFL 00100 { 0.173, 0.985} //FFFR 00101 }; 00102 00103 void move(int vx, int vy, int vs){ 00104 double pwm[4] = {0}; 00105 00106 pwm[0] = (double)((vx) + vs); 00107 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs); 00108 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs); 00109 pwm[3] = kick; 00110 00111 for(i = 0; i < 4; i++){ 00112 if(pwm[i] > 100){ 00113 pwm[i] = 100; 00114 } else if(pwm[i] < -100){ 00115 pwm[i] = -100; 00116 } 00117 speed[i] = pwm[i]; 00118 } 00119 } 00120 00121 //通信（モータ用） 00122 void tx_motor(){ 00123 array(speed[0],speed[1],speed[3],speed[2]); 00124 motor.printf("%s",StringFIN.c_str()); 00125 } 00126 00127 //ライン判断 00128 void line_state(){ 00129 if(line[LEFT]){ 00130 x = LINE_P; 00131 } else if(line[RIGHT]){ 00132 x = -LINE_P; 00133 } 00134 00135 if(line[FRONT]){ 00136 if(line[LEFT] | line[RIGHT]){ 00137 y = -LINE_PLR; 00138 } else { 00139 y = -LINE_P; 00140 } 00141 } else if(line[BACK]){ 00142 if(line[LEFT] | line[RIGHT]){ 00143 y = LINE_PLR; 00144 } else { 00145 y = LINE_P; 00146 } 00147 } 00148 } 00149 00150 void line_stop_F(){ 00151 line_stop[FRONT] = 0; 00152 } 00153 void line_stop_L(){ 00154 line_stop[LEFT] = 0; 00155 } 00156 void line_stop_B(){ 00157 line_stop[BACK] = 0; 00158 } 00159 void line_stop_R(){ 00160 line_stop[RIGHT] = 0; 00161 } 00162 00163 void line_check() 00164 { 00165 if(!line_stop[FRONT]){ 00166 if(adcline[FRONT].read_u16() > LINE_ON){ 00167 line[FRONT] = 1; 00168 line_stop[FRONT] = 1; 00169 line_stop[BACK] = 1; 00170 //line_ping[FRONT] = 1; 00171 myled[0] = 1; 00172 liner_F.attach(&line_stop_F,LINE_TIME); 00173 liner_B.attach(&line_stop_B,LINE_TIME); 00174 } else { 00175 line[FRONT] = 0; 00176 myled[0] = 0; 00177 } 00178 } 00179 if(!line_stop[LEFT]){ 00180 if(adcline[LEFT].read_u16() > LINE_ON){ 00181 line[LEFT] = 1; 00182 line_stop[LEFT] = 1; 00183 line_stop[RIGHT] = 1; 00184 //line_ping[LEFT] = 1; 00185 myled[1] = 1; 00186 liner_L.attach(&line_stop_L,LINE_TIME); 00187 liner_R.attach(&line_stop_R,LINE_TIME); 00188 } else { 00189 line[LEFT] = 0; 00190 myled[1] = 0; 00191 } 00192 } 00193 if(!line_stop[BACK]){ 00194 if(adcline[BACK].read_u16() > LINE_ON){ 00195 line[BACK] = 1; 00196 line_stop[BACK] = 1; 00197 line_stop[FRONT] = 1; 00198 //line_ping[BACK] = 1; 00199 myled[2] = 1; 00200 liner_B.attach(&line_stop_B,LINE_TIME); 00201 liner_F.attach(&line_stop_F,LINE_TIME); 00202 } else { 00203 line[BACK] = 0; 00204 myled[2] = 0; 00205 } 00206 } 00207 if(!line_stop[RIGHT]){ 00208 if(adcline[RIGHT].read_u16() > LINE_ON){ 00209 line[RIGHT] = 1; 00210 line_stop[RIGHT] = 1; 00211 line_stop[LEFT] = 1; 00212 //line_ping[RIGHT] = 1; 00213 myled[3] = 1; 00214 liner_R.attach(&line_stop_R,LINE_TIME); 00215 liner_L.attach(&line_stop_L,LINE_TIME); 00216 } else { 00217 line[RIGHT] = 0; 00218 myled[3] = 0; 00219 } 00220 } 00221 } 00222 00223 00224 void pidupdate() 00225 { 00226 //pid.setSetPoint(180.0 + goal); 00227 pidinput = compass; 00228 pid.setProcessValue(pidinput); 00229 00230 compasspid = -pid.compute(); 00231 } 00232 00233 void home() 00234 { 00235 x = 0; 00236 y = 0; 00237 kick = 0; 00238 goal = 0.0; 00239 00240 // if((145 < ping[LEFT]+ping[RIGHT]) && (ping[LEFT]+ping[RIGHT] < 155)){ 00241 if(ping[LEFT] > PING_LR){ 00242 x = -HOME_P; 00243 y = -5; 00244 } else if(ping[RIGHT] > PING_LR){ 00245 x = HOME_P; 00246 y = -5; 00247 } 00248 // } 00249 00250 if(ping[BACK] > HOME_BACK){ 00251 y = -HOME_P; 00252 } else if(ping[BACK] < 5){ 00253 y = HOME_P; 00254 } 00255 } 00256 00257 int main() { 00258 00259 //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20); 00260 //uint8_t num = 0; 00261 uint8_t dir = 0, power = 0, lcd_count = 0; 00262 int x_ball = 0, x_turn = 0, y_ball = 0, y_turn = 0; 00263 00264 wait(1); 00265 00266 motor.baud(115200); //ボーレート設定 00267 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 00268 motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用） 00269 sensor.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用） 00270 xbeerx.attach(&xbee_rx,5); 00271 xbeetx.attach(&xbee_tx,5); 00272 //compassdef = (compass / 10); //コンパス初期値保存 00273 //compassdef = (dcompass.sample() / 10); 00274 pid.setInputLimits(0.0,360.0); 00275 pid.setOutputLimits(-30.0,30.0); 00276 pid.setBias(0.0); 00277 pid.setMode(AUTO_MODE); 00278 pid.setSetPoint(180.0); 00279 pidupdata.attach(&pidupdate, 0.05); 00280 00281 sw_wh.mode(PullUp); 00282 start.mode(PullDown); 00283 00284 lcd.cls(); 00285 lcd.locate(0,0); 00286 lcd.printf("Chronograph"); 00287 00288 00289 //move(x,y,s); 00290 00291 myled[0] = 1; 00292 00293 //スタート前チェック 00294 while(!start){ 00295 if(lcd_count == 0){ 00296 lcd.locate(6,1); 00297 lcd.printf("%3d", compass); 00298 } else if(lcd_count == 1){ 00299 lcd.locate(0,1); 00300 lcd.printf("ir_dis = %3d", ir_dis); 00301 } else if(lcd_count == 2){ 00302 lcd.locate(0,0); 00303 lcd.printf("ir_num = %3d\nir_val = %3d", ir_num, value_ir); 00304 } else if(lcd_count == 3){ 00305 lcd.locate(0,0); 00306 lcd.printf("F:%3d B:%3d\nL:%3d R:%3d", ping[FRONT], ping[BACK], ping[LEFT], ping[RIGHT]); 00307 } else if(lcd_count == 4){ 00308 lcd.locate(0,0); 00309 lcd.printf("F:%5d B:%5d\nL:%5d R:%5d", adcline[FRONT].read_u16(), adcline[BACK].read_u16(), adcline[LEFT].read_u16(), adcline[RIGHT].read_u16()); 00310 } 00311 if(!sw_wh){ 00312 lcd_count++; 00313 if(lcd_count == 5){ 00314 lcd_count = 0; 00315 lcd.cls(); 00316 lcd.locate(0,0); 00317 lcd.printf("Chronograph"); 00318 } 00319 wait(0.5); 00320 } 00321 wait(0.1); 00322 } 00323 00324 00325 00326 00327 myled[0] = 0; 00328 lcd.cls(); 00329 lcd.locate(0,0); 00330 lcd.printf("Chronograph"); 00331 lcd.locate(7,1); 00332 lcd.printf("ABURASOBA"); 00333 00334 00335 00336 while(1){ 00337 x = 0; 00338 y = 0; 00339 s = compasspid; 00340 power = MOTOR_P; 00341 00342 if((ir_dis < 35)/* && ( (ir_num == FF) || (ir_num == FFL) || (ir_num == FFR) )*/){ 00343 kick = KICK; 00344 } else { 00345 kick = 0; 00346 //goal = 0.0; 00347 } 00348 00349 if((ir_num != FF) && (ir_num != FFL) && (ir_num != FFR)){ 00350 if(ir_dis > BALL_FAR){ 00351 x_turn = TURN_P * way[ir_num][0]; 00352 y_turn = TURN_P * way[ir_num][1]; 00353 00354 x_ball = BALL_P * ball_way[ir_num][0]; 00355 y_ball = BALL_P * ball_way[ir_num][1]; 00356 00357 if(ir_num == BB){ 00358 if(ping[LEFT] > ping[RIGHT]){ 00359 x_ball = -x_ball; 00360 } 00361 } 00362 00363 x = x_turn + x_ball; 00364 y = y_turn + y_ball; 00365 00366 } else if(ir_dis < BALL_NEAR){ 00367 x_turn = -(TURN_P - 5) * way[ir_num][0]; 00368 y_turn = -(TURN_P - 5) * way[ir_num][1]; 00369 00370 x_ball = BALL_P * ball_way[ir_num][0]; 00371 y_ball = BALL_P * ball_way[ir_num][1]; 00372 00373 if(ir_num == BB){ 00374 if(ping[LEFT] > ping[RIGHT]){ 00375 x_ball = -x_ball; 00376 } 00377 } 00378 00379 x = x_turn + x_ball; 00380 y = y_turn + y_ball; 00381 00382 } else { 00383 if((ir_num == FL) || (ir_num == FR)){ 00384 power = 20; 00385 } 00386 x = power * ball_way[ir_num][0]; 00387 y = power * ball_way[ir_num][1]; 00388 if(ir_num == BB){ 00389 if(ping[LEFT] > ping[RIGHT]){ 00390 x = -x; 00391 } 00392 } 00393 } 00394 00395 } else { 00396 if(ir_num == FF){ 00397 power = MOTOR_P + 10; 00398 } 00399 x = power * ball_way[ir_num][0]; 00400 y = power * ball_way[ir_num][1]; 00401 00402 } 00403 00404 line_check(); 00405 00406 if((ir_num == NO_BALL)/* || (ball_on < 40)*/){ 00407 home(); 00408 } 00409 00410 //x = 30; y = 10; 00411 line_state(); 00412 00413 if((x == 0) && (y == 0)){ 00414 if(ping[BACK] > 25){ 00415 y = -HOME_P; 00416 } 00417 } 00418 00419 //x = 0; y = 0; 00420 00421 move(x,y,s); 00422 } 00423 00424 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00425 00426 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00427 00428 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00429 00430 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00431 00432 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00433 00434 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00435 00436 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00437 00438 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00439 00440 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00441 00442 while(1){ 00443 00444 x = 0; 00445 y = 0; 00446 s = compasspid; 00447 power = MOTOR_P; 00448 00449 if(/*(ir_dis < 60) && */( (ir_num == FF) || (ir_num == FFL) || (ir_num == FFR) )){ 00450 //kick = 25; 00451 //if(130 < ping[LEFT]+ping[RIGHT]){ 00452 //if(goal == 0){ 00453 /* if(ping[LEFT] > 100){ 00454 goal = -GOAL; 00455 }*//* else if(ping[RIGHT] > 100){ 00456 //goal = GOAL; 00457 }*/ 00458 //} 00459 //} 00460 00461 } else { 00462 kick = 0; 00463 goal = 0.0; 00464 } 00465 00466 if((ir_dis > 130) && (ir_dis < 150) && (ir_num != BB) && (ir_num != FL) && (ir_num != FR) && (ir_num != BL) && (ir_num != BR)){ 00467 x = power * way[ir_num][0]; 00468 y = power * way[ir_num][1]; 00469 } else { 00470 if((ir_num == FF) || (ir_num == FFFL) || (ir_num == FFFR)){ 00471 power = MOTOR_P + 10; 00472 } else if((ir_num == FFL) || (ir_num == FFR)){ 00473 power = MOTOR_P; 00474 } else { 00475 power = MOTOR_P + 5; 00476 } 00477 x = power * ball_way[ir_num][0]; 00478 y = power * ball_way[ir_num][1]; 00479 if(ir_num == BB){ 00480 if(ping[LEFT] > ping[RIGHT]){ 00481 x = -x; 00482 } 00483 } 00484 } 00485 00486 line_check(); 00487 00488 if((ir_num == NO_BALL)/* || (ball_on < 40)*/){ 00489 home(); 00490 } 00491 00492 //x = 30; y = 10; 00493 line_state(); 00494 00495 if((x == 0) && (y == 0)){ 00496 if(ping[BACK] > 25){ 00497 y = -HOME_P; 00498 } 00499 } 00500 00501 //x = 0; y = 0; 00502 00503 move(x,y,s); 00504 } 00505 }
Generated on Thu Jul 14 2022 13:22:08 by
1.7.2