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