Fumiya Fujisawa
ロボカップ2013のジャパンオープンメインプログラム
Dependencies: PID TextLCD mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 //#include "HMC6352.h" 00003 #include "TextLCD.h" 00004 #include "common.h" 00005 #include "PID.h" 00006 #include <math.h> 00007 #include <sstream> 00008 00009 #define LINE_LP 30/*line def@line_state*/ 00010 #define LINE_FP 40/*line def@line_State*/ 00011 #define LINE_ON 0x2710/*line underline*///40000 00012 #define LINE_TIME 0.5/*used lineAttach*/ 00013 00014 /****pid config *************/ 00015 #define RATE 0.08//52 00016 #define PID_BIAS 0.2 00017 #define REFERENCE 180.0 00018 #define MINIMUM 0.1 00019 #define MAXIMUM 360.0 00020 #define PID_CYCLE 0.05 //s 00021 #define P_GAIN 1.4 //0.78 00022 #define I_GAIN 0.0 //0.0 00023 #define D_GAIN 0.015 //0.009 00024 #define OUT_LIMIT 30.0 00025 #define MAX_POW 100 00026 #define MIN_POW -100 00027 /****pid config *************/ 00028 00029 /*keep your distance!!*/ 00030 #define OFFENSE_POWER 31 /*moter power strength*/ 00031 #define DEFENSE_POWER 30 00032 00033 #define BACK_HOME_TIME 0.3 00034 00035 /*dash*/ 00036 #define DASH_TIME 0.7//about 00037 #define CHARGE_DACH_TIME 0.25//about 00038 #define DASH_STRENGTH 40 00039 00040 Serial motor(p9,p10); 00041 Serial sensor(p13,p14); 00042 Serial xbee(p28,p27); 00043 Serial pc(USBTX, USBRX); 00044 00045 TextLCD lcd(p26, p25, p24, p23, p22, p21); 00046 AnalogIn adcline[4] = {p17, p18, p19, p20}; 00047 DigitalIn start(p7);/*start switch*/ 00048 DigitalIn check(p8);/*Xbee chenge switch*/ 00049 DigitalOut myled[4] = {LED1, LED2, LED3, LED4}; 00050 DigitalIn kick_check(p5); 00051 00052 /*Compass PID SetUp*/ 00053 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE); 00054 Ticker pidUpdata; 00055 00056 /*XBee interrupt*/ 00057 Ticker xbeetx; 00058 Ticker xbeerx; 00059 00060 /*line check,linestate*/ 00061 Timeout liner0; 00062 Timeout liner1; 00063 Timeout liner2; 00064 Timeout liner3; 00065 00066 /*back home*/ 00067 Timeout home; 00068 Timer timer_home; 00069 00070 /*ofence dash timer*/ 00071 Timer dash_charge; 00072 Ticker dash_start; 00073 00074 00075 extern string StringFIN; 00076 extern void array(int,int,int,int); 00077 extern void micon_rx(void); 00078 extern void xbee_tx(void); 00079 extern void xbee_rx(void); 00080 extern int count; 00081 00082 int speed[4] = {0}; /*@move,tx_moter*/ 00083 uint8_t line_stop[4] = {0},home_stop = 0; /*@line_stop(number),@back_home*/ 00084 unsigned int compass = 0; /*relevant compass*/ 00085 double standTu = 0, inputPID = 180.0, compassPID = 0.0; /*relevant compass*/ 00086 uint8_t ping[4]={0}; /*ping_data*/ 00087 uint8_t ir_min = 0,ir_num = 0, ir_main = 0; /*ir_min=25~50~70(value),ir_num=0~9,ir_main=undef*/ 00088 //double clockL = 0;//not used 00089 uint8_t dash_stop = 0; 00090 00091 00092 void move(double vx, double vy, int vs, int vk){ 00093 double pwm[4] = {0}; 00094 uint8_t i = 0; 00095 pwm[0] = (double)((vx) + vs); 00096 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs); 00097 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs); 00098 pwm[3] = vk; 00099 00100 for(i = 0; i < 4; i++){ 00101 if(pwm[i] > 100){ 00102 pwm[i] = 100; 00103 }else if(pwm[i] < -100){ 00104 pwm[i] = -100; 00105 } 00106 speed[i] = pwm[i]; 00107 } 00108 } 00109 //通信（モータ用） 00110 void tx_motor(){ 00111 array(speed[0],speed[1],speed[3],speed[2]); 00112 motor.printf("%s",StringFIN.c_str()); 00113 } 00114 void line_state(double *vx,double*vy,uint8_t *line){ 00115 /*chenge to move ,line*/ 00116 if(line[FRONT]){ 00117 *vy = -LINE_FP; 00118 } 00119 if(line[LEFT]){ 00120 *vx = LINE_LP; 00121 } 00122 if(line[BACK]){ 00123 *vy = LINE_FP; 00124 } 00125 if(line[RIGHT]){ 00126 *vx = -LINE_LP; 00127 } 00128 } 00129 void line_stop0(){ 00130 line_stop[FRONT] = 0; 00131 } 00132 void line_stop1(){ 00133 line_stop[LEFT] = 0; 00134 } 00135 void line_stop2(){ 00136 line_stop[BACK] = 0; 00137 } 00138 void line_stop3(){ 00139 line_stop[RIGHT] = 0; 00140 } 00141 void line_check(uint8_t *line){ 00142 if(!line_stop[FRONT]){ 00143 if(adcline[FRONT].read_u16() > LINE_ON){ 00144 line[FRONT] = 1; 00145 line_stop[FRONT] = 1; 00146 line_stop[BACK] = 1; 00147 myled[0] = 1; 00148 liner0.attach(&line_stop0,LINE_TIME); 00149 liner2.attach(&line_stop2,LINE_TIME); 00150 } else { 00151 line[FRONT] = 0; 00152 myled[0] = 0; 00153 } 00154 } 00155 if(!line_stop[LEFT]){ 00156 if(adcline[LEFT].read_u16() > LINE_ON){ 00157 line[LEFT] = 1; 00158 line_stop[LEFT] = 1; 00159 line_stop[RIGHT] = 1; 00160 myled[1] = 1; 00161 liner1.attach(&line_stop1,LINE_TIME); 00162 liner3.attach(&line_stop3,LINE_TIME); 00163 } else { 00164 line[LEFT] = 0; 00165 myled[1] = 0; 00166 } 00167 } 00168 if(!line_stop[BACK]){ 00169 if(adcline[BACK].read_u16() > LINE_ON){ 00170 line[BACK] = 1; 00171 line_stop[BACK] = 1; 00172 line_stop[FRONT] = 1; 00173 myled[2] = 1; 00174 liner2.attach(&line_stop2,LINE_TIME); 00175 liner0.attach(&line_stop0,LINE_TIME); 00176 } else { 00177 line[BACK] = 0; 00178 myled[2] = 0; 00179 } 00180 } 00181 if(!line_stop[RIGHT]){ 00182 if(adcline[RIGHT].read_u16() > LINE_ON){ 00183 line[RIGHT] = 1; 00184 line_stop[RIGHT] = 1; 00185 line_stop[LEFT] = 1; 00186 myled[3] = 1; 00187 liner3.attach(&line_stop3,LINE_TIME); 00188 liner1.attach(&line_stop1,LINE_TIME); 00189 } else { 00190 line[RIGHT] = 0; 00191 myled[3] = 0; 00192 } 00193 } 00194 } 00195 void PidUpdate() 00196 { 00197 pid.setSetPoint((int)((REFERENCE + standTu) / 1.0)); 00198 inputPID = compass; 00199 00200 pid.setProcessValue(inputPID); 00201 compassPID = -(pid.compute()); 00202 } 00203 00204 00205 void ir_defense0(double *vx, double *vy, int *k, int rateD){ 00206 00207 00208 } 00209 00210 void ir_defense1(double *vx, double *vy, int *k, int rateD){ 00211 00212 00213 } 00214 00215 void ir_defense2(double *vx, double *vy, int *k, int rateD){ 00216 00217 00218 } 00219 00220 void ir_defense3(double *vx, double *vy, int *k, int rateD){ 00221 00222 00223 } 00224 00225 void ir_defense4(double *vx, double *vy, int *k, int rateD){ 00226 00227 00228 } 00229 00230 void ir_defense5(double *vx, double *vy, int *k, int rateD){ 00231 00232 00233 } 00234 00235 void ir_defense6(double *vx, double *vy, int *k, int rateD){ 00236 00237 00238 } 00239 00240 void ir_defense7(double *vx, double *vy, int *k, int rateD){ 00241 if(ping[2]<20){ 00242 *vx = DEFENSE_POWER*1.2*way16[13][0]; 00243 *vy = DEFENSE_POWER*1.2*way16[13][1]; 00244 } 00245 } 00246 00247 void ir_defense8(double *vx, double *vy, int *k, int rateD){ 00248 if(ping[LEFT]>ping[RIGHT]){ 00249 *vx = DEFENSE_POWER*(-0.8+near1[ir_num][VX]+go[ir_num][VX]); 00250 *vy = DEFENSE_POWER*(0.5+near1[ir_num][VY]+go[ir_num][VY]); 00251 }else if(ping[RIGHT]>ping[LEFT]){ 00252 *vx = DEFENSE_POWER*(0.8+near1[ir_num][VX]+go[ir_num][VX]); 00253 *vy = DEFENSE_POWER*(0.5+near1[ir_num][VY]+go[ir_num][VY]); 00254 } 00255 00256 } 00257 00258 void ir_defense9(double *vx, double *vy, int *k, int rateD){ 00259 if(ping[2]<20){ 00260 *vx = DEFENSE_POWER*1.2*way16[3][0]; 00261 *vy = DEFENSE_POWER*1.2*way16[3][1]; 00262 } 00263 00264 } 00265 00266 void ir_offense0(double *vx, double *vy, int *vk, int rateD){ 00267 dash_charge.stop(); 00268 dash_charge.reset(); 00269 /* 00270 clockL = 6.50; 00271 standTu = 0; 00272 *vx = (ir_min+5)*way16[9][0]; 00273 *vy = (ir_min+5)*way16[9][1]; 00274 */ 00275 00276 } 00277 void ir_offense1(double *vx, double *vy, int *vk, int rateD){ 00278 dash_charge.stop(); 00279 dash_charge.reset(); 00280 /* 00281 clockL = 7.00; 00282 standTu = 0; 00283 *vx = ir_min*way16[10][0]; 00284 *vy = ir_min*way16[10][1]; 00285 */ 00286 } 00287 void ir_offense2(double *vx, double *vy, int *vk, int rateD){ 00288 dash_charge.stop(); 00289 dash_charge.reset(); 00290 00291 /*y 00292 if(rateD <= 0){ 00293 00294 *vx = STRENGTH*1.2*way16[0][0]; 00295 *vy = STRENGTH*1.2*way16[0][1]; 00296 } 00297 */ 00298 /* 00299 clockL = 10.0; 00300 *vx=-ir_min; 00301 *vy = ir_min+5; 00302 if(ping[LEFT]>ping[RIGHT]){ 00303 clockL = 10.5; 00304 *vx = 35*way16[14][0]; 00305 *vy = 35*way16[14][1]; 00306 *//**vx = 0; 00307 *vy =35;*/ 00308 /* 00309 } 00310 */ 00311 } 00312 void ir_offense3(double *vx, double *vy, int *vk, int rateD){ 00313 /********** 00314 一定時間待ち,その間変わらずir[3]が反応していたならば、ブースト直進キック 00315 irの最小値の位置が移動した場合,タイマーの時間を初期化、停止 00316 設定時間内に変更がなかった場合,数秒間キック直進し続ける。(ほかからの変更を拒否) 00317 00318 ********/ 00319 if(dash_charge.read() == 0){ 00320 00321 dash_charge.start(); 00322 } 00323 /* 00324 if(rateD <=0+3){ 00325 *vk = 25; 00326 }else { 00327 *vk = 0; 00328 } 00329 */ 00330 /* 00331 if(rateD <= 0){ 00332 *vx = STRENGTH*1.2*way16[0][0]; 00333 *vy = STRENGTH*1.2*way16[0][1]; 00334 }*/ 00335 /* 00336 clockL = 12.0; 00337 *vx = 0,*vy = 30; 00338 */ 00339 } 00340 void ir_offense4(double *vx, double *vy, int *vk, int rateD){ 00341 dash_charge.stop(); 00342 dash_charge.reset(); 00343 00344 /*if(rateD <= 0){ 00345 00346 *vx = STRENGTH*1.2*way16[0][0]; 00347 *vy = STRENGTH*1.2*way16[0][1]; 00348 }*/ 00349 /* 00350 clockL = 2.00; 00351 *vx=ir_min; 00352 *vy = ir_min+5; 00353 if(ping[RIGHT]>ping[LEFT]){ 00354 clockL = 2.50; 00355 *vx = 35*way16[2][0]; 00356 *vy = 35*way16[2][1]; 00357 *//**vx = 0; 00358 *vy = 35;*/ 00359 /* 00360 } 00361 */ 00362 } 00363 void ir_offense5(double *vx, double *vy, int *vk, int rateD){ 00364 dash_charge.stop(); 00365 dash_charge.reset(); 00366 /* 00367 clockL = 5.00; 00368 standTu = 0; 00369 *vx = ir_min*way16[6][0]; 00370 *vy = ir_min*way16[6][1]; 00371 */ 00372 } 00373 void ir_offense6(double *vx, double *vy, int *vk, int rateD){ 00374 dash_charge.stop(); 00375 dash_charge.reset(); 00376 /* 00377 clockL = 5.50; 00378 standTu = 0; 00379 *vx = (ir_min+5)*way16[7][0]; 00380 *vy = (ir_min+5)*way16[7][1]; 00381 */ 00382 } 00383 void ir_offense7(double *vx, double *vy, int *vk, int rateD){ 00384 dash_charge.stop(); 00385 dash_charge.reset(); 00386 /*clockL = 6.00; 00387 standTu = 0; 00388 *vx = 0; 00389 *vy = -(ir_min+8); 00390 */ 00391 } 00392 void ir_offense8(double *vx, double *vy, int *vk, int rateD){ 00393 dash_charge.stop(); 00394 dash_charge.reset(); 00395 if(rateD<(0)){ 00396 if(ping[LEFT]>ping[RIGHT]){ 00397 *vx = OFFENSE_POWER*(-1+near1[ir_num][VX]+go[ir_num][VX]); 00398 *vy = OFFENSE_POWER*(1+near1[ir_num][VY]+go[ir_num][VY]); 00399 }else if(ping[RIGHT]>ping[LEFT]){ 00400 *vx = OFFENSE_POWER*(1+near1[ir_num][VX]+go[ir_num][VX]); 00401 *vy = OFFENSE_POWER*(1+near1[ir_num][VY]+go[ir_num][VY]); 00402 } 00403 } 00404 /*standTu = 0; 00405 if(ping[LEFT]>ping[RIGHT]){ 00406 clockL = 8.00; 00407 *vx = 40*way16[11][0]; 00408 *vy = 40*way16[11][1]; 00409 }else{ 00410 clockL = 4.00; 00411 *vx = 40*way16[5][0]; 00412 *vy = 40*way16[5][1]; 00413 }*/ 00414 } 00415 void ir_offense9(double *vx, double *vy, int *vk, int rateD){ 00416 dash_charge.stop(); 00417 dash_charge.reset(); 00418 /*clockL = 6.00; 00419 standTu = 0; 00420 *vx = 0; 00421 *vy = -(ir_min+8);*/ 00422 } 00423 00424 00425 /**kick and dash function**/ 00426 void dash_reset(){ 00427 dash_stop = 0; 00428 lcd.cls(); 00429 } 00430 00431 void dist_fun_DM(int *rate_dm){//used defense first junction 00432 *rate_dm = ir_min - keep_ball[ir_num];//zastu 00433 if(*rate_dm >0){ 00434 *rate_dm = 1; 00435 }else if(*rate_dm <-7){ 00436 *rate_dm =-7; 00437 } 00438 00439 } 00440 00441 void dist_fun_D(int *rate_d){//ofense action support 00442 *rate_d = ir_main - keep_dist[ir_num]; 00443 if(*rate_d < -5){ 00444 *rate_d = -5; 00445 }else if(*rate_d >10){ 00446 *rate_d = 11; 00447 } 00448 } 00449 void dist_fun_ping(int *rate_p){//defense action support 00450 rate_p[1] = ping[1] - keep_ping[1]; 00451 if(rate_p[1] <-10){ 00452 rate_p[1] = -10; 00453 }else if(rate_p[1]>0){ 00454 rate_p[1] = 1; 00455 } 00456 rate_p[2] = keep_ping[2] - ping[2]; 00457 if(rate_p[2]<-5){ 00458 rate_p[2] = -5; 00459 }else if(rate_p[2] >6){ 00460 rate_p[2] = 6; 00461 } 00462 rate_p[3] = ping[3] - keep_ping[3]; 00463 if(rate_p[3] <-10){ 00464 rate_p[3] = -10; 00465 }else if(rate_p[3]>0){ 00466 rate_p[3] = 1; 00467 } 00468 } 00469 00470 void dist_fun_pingSTRONG(int *rate_p){//defense action support 00471 rate_p[2] = keep_pingSTRONG[2] - ping[2]; 00472 if(rate_p[2]<-5){ 00473 rate_p[2] = -5; 00474 }else if(rate_p[2] >6){ 00475 rate_p[2] = 6; 00476 } 00477 if(ping[1]+ping[3] >=155){//tyousei 00478 rate_p[1] = ping[1] - keep_pingSTRONG[1];//38 00479 if(rate_p[1] <-10){ 00480 rate_p[1] = -10; 00481 }else if(rate_p[1]>0){ 00482 rate_p[1] = 1; 00483 } 00484 00485 rate_p[3] = ping[3] - keep_pingSTRONG[3];///37 00486 if(rate_p[3] <-10){ 00487 rate_p[3] = -10; 00488 }else if(rate_p[3]>0){ 00489 rate_p[3] = 1; 00490 } 00491 00492 }else { 00493 rate_p[1] = 0; 00494 rate_p[3] = 0; 00495 } 00496 } 00497 /* 00498 void dist_fun(int *rate_d,int *rate_dm,int *rate_p){*//*This is compute some length.?*/ 00499 /* KEEP_DISTANCE is border line, ir_main is variable 00500 if ir_main> keep_dist (access) 00501 else ir_main< keep_dist (out) 00502 @retaD 00503 */ 00504 /* 00505 *rate_d = ir_main - keep_dist[ir_num]; 00506 if(*rate_d < -5){ 00507 *rate_d = -5; 00508 }else if(*rate_d >10){ 00509 *rate_d = 11; //+30 00510 } 00511 *rate_dm = ir_min - keep_ball[ir_num];//zastu 00512 rate_p[1] = ping[1] - keep_ping[1]; 00513 if(rate_p[1] <-10){ 00514 rate_p[1] = -10; 00515 }else if(rate_p[1]>0){ 00516 rate_p[1] = 1; 00517 } 00518 rate_p[2] = keep_ping[2] - ping[2]; 00519 if(rate_p[2]<-5){ 00520 rate_p[2] = -5; 00521 }else if(rate_p[2] >6){ 00522 rate_p[2] = 6; 00523 } 00524 rate_p[3] = ping[3] - keep_ping[3]; 00525 if(rate_p[3] <-10){ 00526 rate_p[3] = -10; 00527 }else if(rate_p[3]>0){ 00528 rate_p[3] = 1; 00529 } 00530 00531 00532 } 00533 */ 00534 void home_stop0(){ 00535 home_stop = 0; 00536 } 00537 00538 void back_home(double *vx,double*vy){ 00539 *vx = 0; 00540 *vy = -40; 00541 if(!home_stop){ 00542 if(timer_home.read() ==0){ 00543 timer_home.start(); 00544 } 00545 } 00546 if(ping[2]<24){ 00547 *vy = -35; 00548 }else if(ping[2]<30){ 00549 *vy = -25; 00550 } 00551 if((abs(ping[1]-ping[3]))>25){ 00552 if(ping[1]>ping[3]){ 00553 //*vy = 15; 00554 *vx = -25; 00555 }else if(ping[1]<ping[3]){ 00556 //*vy = 12; 00557 *vx = 25; 00558 } 00559 } 00560 00561 //左角を抜ける方法を考える 00562 00563 00564 if((timer_home.read()>0.6)/*||home0.read()==0*/){ 00565 *vy = 11; 00566 *vx = 0; 00567 timer_home.stop(); 00568 timer_home.reset(); 00569 home_stop = 1; 00570 home.attach(&home_stop0,BACK_HOME_TIME); 00571 //wait(0.1); 00572 } 00573 if(ping[2]<20){ 00574 *vx = 0; 00575 *vy = 5; 00576 } 00577 00578 } 00579 int main(void){ 00580 00581 uint8_t line[4] = {0}; /*@line_state,line_check*/ 00582 double vx = 0,vy = 0; /*used move(moter)*/ 00583 int vs = 0, vk = 0; /*used move(moter)*/ 00584 int rateD = 0; /*keep distance*/ 00585 int rateDM = 0; /*keep_distrance to boal //ir_max use*/ 00586 int rateP[4] = {0}; /*keep in front of goal*/ 00587 void (*offense[10])(double *,double *,int *,int); 00588 void (*defense[10])(double *,double *,int *,int); 00589 00590 //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20); 00591 motor.baud(115200); //ボーレート設定 00592 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 00593 motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用） 00594 sensor.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用） 00595 //compassdef = (compass / 10); //コンパス初期値保存 00596 //compassdef = (dcompass.sample() / 10); 00597 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def 00598 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def 00599 pid.setBias(PID_BIAS); //pid sed def 00600 pid.setMode(AUTO_MODE); //pid sed def 00601 pid.setSetPoint(REFERENCE); //pid sed def 00602 start.mode(PullDown); //digitalin pulldown 00603 check.mode(PullUp); //digitalin pullup 00604 kick_check.mode(PullUp); 00605 00606 offense[0] = ir_offense0; 00607 offense[1] = ir_offense1; 00608 offense[2] = ir_offense2; 00609 offense[3] = ir_offense3; 00610 offense[4] = ir_offense4; 00611 offense[5] = ir_offense5; 00612 offense[6] = ir_offense6; 00613 offense[7] = ir_offense7; 00614 offense[8] = ir_offense8; 00615 offense[9] = ir_offense9; 00616 00617 00618 defense[0] = ir_defense0; 00619 defense[1] = ir_defense1; 00620 defense[2] = ir_defense2; 00621 defense[3] = ir_defense3; 00622 defense[4] = ir_defense4; 00623 defense[5] = ir_defense5; 00624 defense[6] = ir_defense6; 00625 defense[7] = ir_defense7; 00626 defense[8] = ir_defense8; 00627 defense[9] = ir_defense9; 00628 00629 00630 00631 pidUpdata.attach(&PidUpdate, PID_CYCLE); 00632 //move(30,0,0,0); 00633 count = 0; 00634 if(check){// NOT use Xbee 00635 myled[3] = 0; 00636 count = 1; 00637 //lcd.printf("NOT USE XBee\nDEFENSE\n"); 00638 /* 00639 count = 1:defense 00640 count = 0:offense 00641 */ 00642 }else { 00643 myled[3] = 1; 00644 xbeerx.attach(&xbee_rx,5); 00645 xbeetx.attach(&xbee_tx,5); 00646 } 00647 myled[0] = 1; 00648 while(!start){ 00649 lcd.printf("comp %3d min %3dnum %3d main %3d",compass,ir_min,ir_num,ir_main); 00650 if(!kick_check){ 00651 move(0,0,0,25); 00652 }else{ 00653 move(0,0,0,0); 00654 } 00655 } 00656 myled[0] = 0; 00657 lcd.cls(); 00658 while(1){ 00659 vx = 0, vy = 0, vk = 0; 00660 standTu = 0; 00661 00662 if(ir_num<10){ 00663 dist_fun_DM(&rateDM);//used defense first junction 00664 00665 if(count){/**** DEFENSE ACTION!! *******/ 00666 if((rateDM > 0)||(ping[2]>33)){//key point 00667 dist_fun_ping(rateP);//check ping state. 00668 vx = DEFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]); 00669 vy = DEFENSE_POWER*(goal_state1[rateP[1]+10][1]+goal_state2[rateP[2]+5][1]+goal_state3[rateP[3]+10][1]+ball_state0[ir_num][1]); 00670 if(ping[2]<28){ 00671 00672 }else if(ping[2]<45){ 00673 vy = -30; 00674 }else { 00675 vx =0; 00676 vy = -35; 00677 } 00678 00679 00680 }else{ 00681 dist_fun_pingSTRONG(rateP); 00682 vx = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][0]+strongPing1[rateP[1]+10][0]+strongPing3[rateP[3]+10][0]); 00683 vy = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][1]+strongPing1[rateP[1]+10][1]+strongPing2[rateP[2]+10][1]+strongPing3[rateP[3]+10][1]); 00684 (*defense[ir_num])(&vx,&vy,&vk,rateD); 00685 00686 if(ir_num == 2||ir_num == 3||ir_num == 4){ 00687 vk=25; 00688 } 00689 } 00690 00691 00692 }else{/**** OFFENSE ACTION!! *****/ 00693 // if count =0 00694 if((rateDM > 0)&&((ir_num != 2)&&(ir_num !=3)&&(ir_num!=4))){ 00695 //アウトオブリーチ時後ろに下がりながらボールの直線上に移動する 00696 dist_fun_ping(rateP);//check ping_state 00697 00698 vx = OFFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]); 00699 vy = OFFENSE_POWER*(goal_state1[rateP[1]+10][1]+goal_state2[rateP[2]+5][1]+goal_state3[rateP[3]+10][1]+ball_state0[ir_num][1]); 00700 00701 if(ping[2]<30){ 00702 //stop place 00703 }else if(ping[2]<50){ 00704 vy = -30; 00705 }else { 00706 vx = 0; 00707 vy = -35; 00708 } 00709 00710 }else{/**** Main offense action */ 00711 00712 if(!dash_stop){ 00713 dist_fun_D(&rateD);//check ball state. 00714 if(rateD > 0){ 00715 //dist_fun_pingSTRONG(rateP); 00716 vx = OFFENSE_POWER*(near0[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]); 00717 vy = OFFENSE_POWER*(near0[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]); 00718 00719 }else{ 00720 vx = OFFENSE_POWER*(modulate_near[rateD+5]*near1[ir_num][0]+go[ir_num][0]); 00721 vy = OFFENSE_POWER*(modulate_near[rateD+5]*near1[ir_num][1]+go[ir_num][1]); 00722 } 00723 00724 if(dash_charge.read()>=CHARGE_DACH_TIME){ 00725 dash_start.attach(&dash_reset,DASH_TIME); 00726 dash_stop = 1; 00727 //lcd.printf(" FALCON KICK \n\n"); 00728 dash_charge.stop(); 00729 dash_charge.reset(); 00730 } 00731 } 00732 (*offense[ir_num])(&vx,&vy,&vk,rateD); 00733 if(dash_stop){ 00734 vk = 25; 00735 vx = DASH_STRENGTH*way16[0][0]; 00736 vy = DASH_STRENGTH*way16[0][1]; 00737 00738 00739 } 00740 00741 } 00742 00743 } 00744 } 00745 00746 if(ir_num >= 10){ 00747 back_home(&vx,&vy); 00748 } 00749 line_check(line); 00750 line_state(&vx,&vy,line); 00751 vs = compassPID; 00752 move(vx,vy,vs,vk); 00753 //lcd.printf("%0.2lf clock\n\n",clockL); 00754 //pc.printf("compassPID:%d\t compass:%d\n",s,compass); 00755 //pc.printf("%lf %lf\n",x,y); 00756 //pc.printf("compass: %d\n",compass); 00757 //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping[0],ping[1],ping[2],ping[3]); 00758 //pc.printf("ir_min:%d\tir_num:%d\tir_main:%d\n",ir_min,ir_num,ir_main); 00759 //pc.printf("line0:%d\tline1:%d\tline2:%d\tline3:%d\n",adcline[0].read_u16(),adcline[1].read_u16(),adcline[2].read_u16(),adcline[3].read_u16()); 00760 } 00761 00762 00763 00764 00765 00766 00767 00768 /************************************************** 00769 00770 00771 00772 00773 00774 00775 00776 00777 00778 00779 00780 00781 00782 00783 00784 00785 00786 00787 ****************************************************/ 00788 /* 00789 while(1){ 00790 vx = 0, vy = 0, vk = 0; 00791 standTu = 0; 00792 //dist_fun(&rateD,&rateDM,rateP);//3to-ri 00793 if(ir_num<10){*/ 00794 /*if(rateD >= 0){//if(ir_num =2ka4) =3 00795 vx = STRENGTH*(modulate_near[rateD+5]*near0[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]); 00796 vy = STRENGTH*(modulate_near[rateD+5]*near0[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]); 00797 }else{ 00798 vx = STRENGTH*(modulate_near[rateD+5]*near1[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]); 00799 vy = STRENGTH*(modulate_near[rateD+5]*near1[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]); 00800 }*/ 00801 /* 00802 if(rateD >= 0){ 00803 vx = STRENGTH*(modulate[rateD+5]*near0[ir_num][0]+modulate[rateD+5]*go[ir_num][0]); 00804 vy = STRENGTH*(modulate[rateD+5]*near0[ir_num][1]+modulate[rateD+5]*go[ir_num][1]); 00805 }else{ 00806 vx = STRENGTH*(modulate[rateD+5]*near1[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]); 00807 vy = STRENGTH*(modulate[rateD+5]*near1[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]); 00808 }*/ 00809 /* 00810 dist_fun_DM(&rateDM);//used defense first junction 00811 if(count){//defense 00812 if(rateDM > 0){//key point 00813 dist_fun_ping(rateP);//check ping state. 00814 vx = DEFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]); 00815 vy = DEFENSE_POWER*(goal_state1[rateP[1]+10][1]+goal_state2[rateP[2]+5][1]+goal_state3[rateP[3]+10][1]+ball_state0[ir_num][1]); 00816 if(ping[2]<35){ 00817 00818 }else if(ping[2]<48){ 00819 vy = -30; 00820 }else { 00821 vy = 0; 00822 vy = -35; 00823 } 00824 }else{*/ 00825 /*if(ir_num<7){ 00826 vx = DEFENSE_POWER*ball_state1[ir_num][0]; 00827 vy = DEFENSE_POWER*(goal_state2[rateP[2]+5][1]+ball_state1[ir_num][1]); 00828 }else if(ir_num<10){ 00829 vx = DEFENSE_POWER*(strongPing1[rateP[1]+10][0]+strongPing3[rateP[3]+10][0]+ball_state1[ir_num][0]); 00830 vx = DEFENSE_POWER*(goal_state2[rateP[2]+5][1]+ball_state1[ir_num][1]); 00831 }*/ 00832 /* 00833 dist_fun_pingSTRONG(rateP); 00834 vx = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][0]+strongPing1[rateP[1]+10][0]+strongPing3[rateP[3]+10][0]); 00835 vy = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][1]+strongPing1[rateP[1]+10][1]+strongPing2[rateP[2]+10][1]+strongPing3[rateP[3]+10][1]); 00836 if(ir_num == 2||ir_num == 3||ir_num == 4){ 00837 vk=25; 00838 } 00839 } 00840 }else{//ofense 00841 if((rateDM > 0)&&((ir_num != 2)&&(ir_num !=3)&&(ir_num!=4))){ 00842 //アウトオブリーチ時後ろに下がりながらボールの直線上に移動する 00843 dist_fun_ping(rateP);//check ping state. 00844 vx = DEFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]); 00845 vy = DEFENSE_POWER*(goal_state1[rateP[1]+10][1]+goal_state2[rateP[2]+5][1]+goal_state3[rateP[3]+10][1]+ball_state0[ir_num][1]); 00846 if(ping[2]<35){ 00847 00848 }else if(ping[2]<50){ 00849 vy = -30; 00850 }else { 00851 vx = 0; 00852 vy = -35; 00853 } 00854 }else{ 00855 00856 dist_fun_D(&rateD);//check ball state. 00857 if(rateD > 0){ 00858 dist_fun_pingSTRONG(rateP); 00859 vx = OFFENSE_POWER*(apply[rateDM+7]*near0[ir_num][0]+go[ir_num][0]); 00860 vy = OFFENSE_POWER*(apply[rateDM+7]*near0[ir_num][1]+go[ir_num][1]); 00861 }else{ 00862 vx = OFFENSE_POWER*(near1[ir_num][0]+go[ir_num][0]); 00863 vy = OFFENSE_POWER*(near1[ir_num][1]+go[ir_num][1]); 00864 } 00865 (*offense[ir_num])(&vx,&vy,&vk,rateD); 00866 } 00867 } 00868 } 00869 if(ir_num >= 10){ 00870 back_home(&vx,&vy); 00871 } 00872 */ 00873 /* 00874 if(ir_num<10){ 00875 (*offense[ir_num])(&vx,&vy,&vk); 00876 //vx =25*way10[ir_num][0],vy = 25*way10[ir_num][1]; 00877 }else{ 00878 clockL = 00.0; 00879 back_home(&x,&y); 00880 } 00881 00882 */ 00883 /* 00884 line_check(line); 00885 line_state(&vx,&vy,line); 00886 vs = compassPID; 00887 move(vx,vy,vs,vk); 00888 //lcd.printf("%0.2lf clock\n\n",clockL); 00889 //pc.printf("compassPID:%d\t compass:%d\n",s,compass); 00890 //pc.printf("%lf %lf\n",x,y); 00891 //pc.printf("compass: %d\n",compass); 00892 //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping[0],ping[1],ping[2],ping[3]); 00893 //pc.printf("ir_min:%d\tir_num:%d\tir_main:%d\n",ir_min,ir_num,ir_main); 00894 //pc.printf("line0:%d\tline1:%d\tline2:%d\tline3:%d\n",adcline[0].read_u16(),adcline[1].read_u16(),adcline[2].read_u16(),adcline[3].read_u16()); 00895 00896 00897 00898 00899 00900 00901 00902 00903 00904 00905 00906 00907 00908 00909 00910 00911 }*/ 00912 }
Generated on Wed Jul 27 2022 02:26:51 by
1.7.2