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