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