ryo seki
final
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include <math.h> 00002 #include <sstream> 00003 #include "mbed.h" 00004 #include "HMC6352.h" 00005 #include "PID.h" 00006 #include "main.h" 00007 00008 00009 00010 void PidUpdate() 00011 { 00012 static uint8_t Fflag = 0; 00013 00014 inputPID = (((int)(compass.sample() - ((standard + standTu) * 10.0) + 9000.0) % 3600) / 10.0); 00015 00016 //pc.printf("%f\n",timer1.read()); 00017 pid.setProcessValue(inputPID); 00018 //timer1.reset(); 00019 00020 compassPID = -(pid.compute()); 00021 00022 if(!Fflag){ 00023 Fflag = 1; 00024 compassPID = 0; 00025 } 00026 //pc.printf("%f\n",standard); 00027 //pc.printf("%d\n",diff); 00028 //pc.printf("compass.sample = %f\n",compass.sample() / 1.0); 00029 //pc.printf("%d\n",(int)compassPID); 00030 //pc.printf("%d\t%d\n",Distance,direction); 00031 //pc.printf("%d\t%d\t%d\t%d\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]); 00032 } 00033 00034 void move(int vxx, int vyy, int vss) 00035 { 00036 double motVal[MOT_NUM] = {0}; 00037 00038 motVal[0] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1); 00039 motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT2); 00040 motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3); 00041 motVal[3] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT4); 00042 00043 for(uint8_t i = 0 ; i < MOT_NUM ; i++){ 00044 if(motVal[i] > MAX_POW)motVal[i] = MAX_POW; 00045 else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW; 00046 speed[i] = (int)motVal[i]; 00047 } 00048 } 00049 00050 /*********** Serial interrupt ***********/ 00051 00052 void Tx_interrupt() 00053 { 00054 array(speed[0],speed[1],speed[2],speed[3]); 00055 driver.printf("%s",StringFIN.c_str()); 00056 //pc.printf("%s",StringFIN.c_str()); 00057 } 00058 /* 00059 void Rx_interrupt() 00060 { 00061 if(driver.readable()){ 00062 //pc.printf("%d\n",driver.getc()); 00063 } 00064 }*/ 00065 00066 00067 /*********** Serial interrupt **********/ 00068 00069 00070 void init() 00071 { 00072 int scanfSuccess; 00073 int printfSuccess; 00074 int closeSuccess; 00075 int close2Success; 00076 uint8_t MissFlag = 0; 00077 FILE *fp; 00078 00079 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); 00080 00081 StartButton.mode(PullUp); 00082 CalibEnterButton.mode(PullUp); 00083 CalibExitButton.mode(PullUp); 00084 EEPROMButton.mode(PullUp); 00085 00086 driver.baud(BAUD_RATE); 00087 device2.baud(BAUD_RATE2); 00088 wait_ms(MOTDRIVER_WAIT); 00089 driver.printf("1F0002F0003F0004F000\r\n"); 00090 device2.printf("START"); 00091 00092 driver.attach(&Tx_interrupt, Serial::TxIrq); 00093 //driver.attach(&Rx_interrupt, Serial::RxIrq); 00094 device2.attach(&dev_rx,Serial::RxIrq); 00095 device2.attach(&dev_tx,Serial::TxIrq); 00096 00097 pid.setInputLimits(MINIMUM,MAXIMUM); 00098 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); 00099 pid.setBias(PID_BIAS); 00100 pid.setMode(AUTO_MODE); 00101 pid.setSetPoint(REFERENCE); 00102 00103 Survtimer.start(); 00104 00105 mbedleds = 1; 00106 00107 while(StartButton){ 00108 MissFlag = 0; 00109 if(!CalibEnterButton){ 00110 mbedleds = 2; 00111 compass.setCalibrationMode(ENTER); 00112 while(CalibExitButton); 00113 compass.setCalibrationMode(EXIT); 00114 wait(BUT_WAIT); 00115 mbedleds = 4; 00116 } 00117 00118 if(!EEPROMButton){ 00119 Survtimer.reset(); 00120 fp = fopen("/local/out.txt", "r"); 00121 if(fp == NULL){ 00122 wait(BUT_WAIT); 00123 MissFlag = 1; 00124 }else{ 00125 scanfSuccess = fscanf(fp, "%lf",&standard); 00126 if(scanfSuccess == EOF){ 00127 wait(BUT_WAIT); 00128 MissFlag = 1; 00129 }else{ 00130 closeSuccess = fclose(fp); 00131 if(closeSuccess == EOF){ 00132 wait(BUT_WAIT); 00133 MissFlag = 1; 00134 }else{ 00135 wait(BUT_WAIT); 00136 } 00137 } 00138 } 00139 if((Survtimer.read() > (BUT_WAIT + 0.1)) || (MissFlag)){ 00140 for(uint8_t i = 0;i < 2;i++){ 00141 mbedleds = 15; 00142 wait(0.1); 00143 mbedleds = 0; 00144 wait(0.1); 00145 } 00146 mbedleds = 15; 00147 }else{ 00148 mbedleds = 8; 00149 } 00150 } 00151 00152 if(!CalibExitButton){ 00153 Survtimer.reset(); 00154 00155 standard = compass.sample() / 10.0; 00156 00157 fp = fopen("/local/out.txt", "w"); 00158 if(fp == NULL){ 00159 wait(BUT_WAIT); 00160 MissFlag = 1; 00161 }else{ 00162 printfSuccess = fprintf(fp, "%f",standard); 00163 if(printfSuccess == EOF){ 00164 wait(BUT_WAIT); 00165 MissFlag = 1; 00166 }else{ 00167 close2Success = fclose(fp); 00168 if(close2Success == EOF){ 00169 wait(BUT_WAIT); 00170 MissFlag = 1; 00171 }else{ 00172 wait(BUT_WAIT); 00173 } 00174 } 00175 } 00176 if((Survtimer.read() > (BUT_WAIT + 0.2)) || (MissFlag)){ 00177 for(uint8_t i = 0;i < 4;i++){ 00178 mbedleds = 15; 00179 wait(0.1); 00180 mbedleds = 0; 00181 wait(0.1); 00182 } 00183 mbedleds = 15; 00184 }else{ 00185 mbedleds = 10; 00186 } 00187 } 00188 } 00189 00190 mbedleds = 0; 00191 00192 Survtimer.stop(); 00193 00194 pidUpdata.attach(&PidUpdate, PID_CYCLE); 00195 00196 //timer1.start(); 00197 } 00198 00199 int main() 00200 { 00201 int vx=0,vy=0,vs=0; 00202 00203 init(); 00204 00205 while(1){ 00206 00207 hold_flag = 0; 00208 vx = 0; 00209 vy = 0; 00210 vs = (int)compassPID; 00211 00212 //move(vx,vy,vs); 00213 00214 /****************************************************** 00215 ******************************************************* 00216 ********************Change state ********************** 00217 ******************************************************* 00218 ******************************************************/ 00219 00220 if(state == HOLD){ 00221 if(FRONT_SONIC < 100)hold_flag = 1; 00222 00223 if(Distance > 140){ //審判のボール持ち上げ判定 00224 state = HOME_WAIT; 00225 }else if(!((direction == 0) || (direction == 15) || (direction == 1))){//ボールを見失った 00226 state = DIFFENCE; 00227 }else if(!(Distance <= 30)){//ボールを見失った 00228 state = DIFFENCE; 00229 } 00230 }else{ 00231 standTu = 0; 00232 if(state == DIFFENCE){ 00233 if((direction == 0) && (Distance <= 10) && (IR_found) && (!xbee)){ 00234 state = HOLD; 00235 }else if((Distance < 180) && (IR_found) && (!xbee)){ 00236 state = DIFFENCE; 00237 }else{ 00238 if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){ 00239 if((IR_found) && (!xbee)){ 00240 state = DIFFENCE; 00241 } 00242 }else if((diff > 15) && (!xbee) && (IR_found)){ 00243 state = DIFFENCE; 00244 }else{ 00245 state = HOME_WAIT; 00246 } 00247 } 00248 00249 }else{ 00250 if((direction == 0) && (Distance <= 10) && (IR_found) && (!xbee)){ 00251 state = HOLD; 00252 }else if((Distance <= 140) && (IR_found) && (!xbee)){ 00253 state = DIFFENCE; 00254 }else{ 00255 if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){ 00256 if((IR_found) && (!xbee)){ 00257 state = DIFFENCE; 00258 } 00259 }else if((diff > 15) && (!xbee) && (IR_found)){ 00260 state = DIFFENCE; 00261 }else{ 00262 state = HOME_WAIT; 00263 } 00264 } 00265 } 00266 } 00267 00268 /****************************************************** 00269 ******************************************************* 00270 ********************Change state ********************** 00271 ******************************************************* 00272 ******************************************************/ 00273 00274 if(state == HOME_WAIT){ 00275 mbedleds = 1; 00276 if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 850.0)){ 00277 if((LEFT_SONIC > 425.0) && (RIGHT_SONIC > 425.0)){ 00278 vx = 0; 00279 }else if(RIGHT_SONIC < 425.0){ 00280 vx = (int)((RIGHT_SONIC - 450.0) * 0.02 - 10.0); 00281 if(vx < -20)vx = -20; 00282 }else if(LEFT_SONIC < 425.0){ 00283 vx = (int)((450.0 - LEFT_SONIC ) * 0.02 + 10.0); 00284 if(vx > 20)vx = 20; 00285 } 00286 00287 if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){ 00288 if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){ 00289 vy = 0; 00290 }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){ 00291 vy = 4; 00292 }else{ 00293 vy = (int)(0.01 * (30.0 - BACK_SONIC) - 4); 00294 if(vy < -10)vy = -10; 00295 } 00296 }else{ 00297 if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){ 00298 vy = 0; 00299 }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){ 00300 vy = 4; 00301 }else{ 00302 vy = (int)(0.01 * (130.0 - BACK_SONIC) - 4); 00303 if(vy < -10)vy = -10; 00304 } 00305 } 00306 }else if((RIGHT_SONIC + LEFT_SONIC) <= 850.0){ 00307 if(BACK_SONIC < 200.0){ 00308 if(RIGHT_SONIC > LEFT_SONIC){ 00309 vx = 10; 00310 vy = 3; 00311 }else{ 00312 vx = -10; 00313 vy = 3; 00314 } 00315 }else{ 00316 vx = 0; 00317 vy = -10; 00318 } 00319 }else{ 00320 if(BACK_SONIC > 500.0){ 00321 if(RIGHT_SONIC > LEFT_SONIC){ 00322 vx = 10; 00323 vy = -5; 00324 }else{ 00325 vx = -10; 00326 vy = -5; 00327 } 00328 } 00329 } 00330 }else if(state == DIFFENCE){ 00331 mbedleds = 4; 00332 if(direction == 6){ 00333 vx = 15; 00334 00335 if(LEFT_SONIC < 330.0){ 00336 if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){ 00337 vy = 0; 00338 }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){ 00339 vy = 4; 00340 }else{ 00341 vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4); 00342 if(vy < -15)vy = -15; 00343 } 00344 }else if(RIGHT_SONIC < 330.0){ 00345 vx = 10; 00346 vy = -5; 00347 }else{ 00348 if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){ 00349 vy = 0; 00350 }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){ 00351 vy = 4; 00352 }else{ 00353 vy = (int)(0.015 * (130.0 - BACK_SONIC) - 4); 00354 if(vy < -15)vy = -15; 00355 } 00356 } 00357 00358 }else if(direction == 10){ 00359 vx = -15; 00360 00361 if(RIGHT_SONIC < 330.0){ 00362 if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){ 00363 vy = 0; 00364 }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){ 00365 vy = 4; 00366 }else{ 00367 vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4); 00368 if(vy < -15)vy = -15; 00369 } 00370 }else if(LEFT_SONIC < 330.0){ 00371 vx = -10; 00372 vy = -10; 00373 }else{ 00374 if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){ 00375 vy = 0; 00376 }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){ 00377 vy = 4; 00378 }else{ 00379 vy = (int)(0.015 * (130.0 - BACK_SONIC) - 4); 00380 if(vy < -15)vy = -15; 00381 } 00382 } 00383 }else if(direction == 4){ 00384 00385 vx = 15; 00386 00387 if(LEFT_SONIC < 330.0){ 00388 if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){ 00389 vy = 0; 00390 }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){ 00391 vy = 4; 00392 }else{ 00393 vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4); 00394 if(vy < -15)vy = -15; 00395 } 00396 }else if(RIGHT_SONIC < 330.0){ 00397 vx = 10; 00398 vy = -10; 00399 }else{ 00400 if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){ 00401 vy = 0; 00402 }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){ 00403 vy = 4; 00404 }else{ 00405 vy = (int)(0.015 * (130.0 - BACK_SONIC) - 4); 00406 if(vy < -15)vy = -15; 00407 } 00408 } 00409 00410 00411 }else if(direction == 12){ 00412 00413 vx = -15; 00414 00415 if(RIGHT_SONIC < 330.0){ 00416 if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){ 00417 vy = 0; 00418 }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){ 00419 vy = 4; 00420 }else{ 00421 vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4); 00422 if(vy < -15)vy = -15; 00423 } 00424 }else if(LEFT_SONIC < 330.0){ 00425 vx = -10; 00426 vy = -10; 00427 }else{ 00428 if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){ 00429 vy = 0; 00430 }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){ 00431 vy = 4; 00432 }else{ 00433 vy = (int)(0.015 * (130.0 - BACK_SONIC) - 4); 00434 if(vy < -15)vy = -15; 00435 } 00436 } 00437 00438 }else if(direction == 8){ 00439 00440 if(LEFT_SONIC > RIGHT_SONIC){ 00441 vx = -15; 00442 }else{ 00443 vx = 15; 00444 } 00445 if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){ 00446 if(BACK_SONIC < 45.0){ 00447 vy = -5; 00448 }else{ 00449 vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4); 00450 if(vy < -15)vy = -15; 00451 } 00452 }else{ 00453 if(BACK_SONIC < 145.0){ 00454 vy = -5; 00455 }else{ 00456 vy = (int)(0.015 * (130.0 - BACK_SONIC) - 4); 00457 if(vy < -15)vy = -15; 00458 } 00459 } 00460 00461 }else if(direction == 2){ 00462 00463 if(BACK_SONIC < 150.0){ 00464 vx = 20; 00465 vy = 0; 00466 }else{ 00467 vx = 12; 00468 vy = 0; 00469 } 00470 }else if(direction == 14){ 00471 00472 if(BACK_SONIC < 150.0){ 00473 vx = -20; 00474 vy = -4; 00475 }else{ 00476 vx = -12; 00477 vy = -2; 00478 } 00479 00480 }else if(direction == 1){ 00481 00482 if(BACK_SONIC < 150.0){ 00483 vx = 15; 00484 vy = 0; 00485 }else{ 00486 vx = 10; 00487 vy = 0; 00488 } 00489 00490 }else if(direction == 15){ 00491 00492 if(BACK_SONIC < 150.0){ 00493 vx = -15; 00494 vy = -3; 00495 }else{ 00496 vx = -10; 00497 vy = -1; 00498 } 00499 }else if(direction == 0){ 00500 00501 vx = 0; 00502 vy = 10; 00503 00504 }else{//error 00505 00506 vx = 0; 00507 vy = 0; 00508 00509 } 00510 }else if(state == WARNING){ 00511 mbedleds = 2; 00512 if(direction == 0){ 00513 00514 vx = 0; 00515 00516 }else if(direction == 1){ 00517 00518 vx = 10; 00519 00520 }else if(direction == 2){ 00521 00522 vx = 15; 00523 00524 }else if(direction == 14){ 00525 00526 vx = -15; 00527 00528 }else if(direction == 15){ 00529 00530 vx = -10; 00531 00532 } 00533 00534 if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){ 00535 if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){ 00536 vy = 0; 00537 }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){ 00538 vy = 4; 00539 }else{ 00540 vy = (int)(0.01 * (30.0 - BACK_SONIC) - 4); 00541 if(vy < -10)vy = -10; 00542 } 00543 }else{ 00544 if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){ 00545 vy = 0; 00546 }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){ 00547 vy = 4; 00548 }else{ 00549 vy = (int)(0.01 * (130.0 - BACK_SONIC) - 4); 00550 if(vy < -10)vy = -10; 00551 } 00552 } 00553 }else if(state == HOLD){ 00554 mbedleds = 8; 00555 vy = 15; 00556 if(FRONT_SONIC < 100){ 00557 if(RIGHT_SONIC < LEFT_SONIC){ 00558 if(BACK_SONIC > 500){ 00559 vy = 0; 00560 vs = 3; 00561 } 00562 }else{ 00563 if(BACK_SONIC > 500){ 00564 vy = 0; 00565 vs = -3; 00566 } 00567 } 00568 }else{ 00569 if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 800.0)){ 00570 standTu = (RIGHT_SONIC - LEFT_SONIC) / 30.0; 00571 } 00572 } 00573 } 00574 //move(vx,vy,vs); 00575 } 00576 }
Generated on Sat Jul 16 2022 09:03:35 by
1.7.2