Naoya Muramatsu
ロボカップJrのブロック大会2015で使用したプログラム
Dependencies: BurstSPI LSM9DS0 PID PWMMotorDriver SDFileSystem mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /*------------------------ 00002 00003 * XBee1 00004 ID : 5102 00005 00006 DH : 0013A200 00007 DL : 407ABE67 00008 00009 * XBee2 00010 ID : 5102 00011 00012 DH : 0013A200 00013 DL : 407C04BD 00014 00015 ------------------------*/ 00016 00017 /*------------------------ 00018 mv /Users/muramatsunaoya/Downloads/RoboCup_2015_LPC1768.bin /Volumes/MBED/ && diskutil unmount /Volumes/MBED/ 00019 ------------------------*/ 00020 00021 00022 00023 #include "mbed.h" 00024 // ライブラリ 00025 #include "MotorDriver.h" 00026 #include "PID.h" 00027 // 自作のクラス 00028 #include "xbeeAPI.h" 00029 00030 00031 /*--- 定義 ---*/ 00032 #define PI 3.1415926 00033 #define Deg2Rad (3.1415926/180) 00034 #define Byte2Rad (3.1415926/255/2) 00035 00036 #define mC 261.626 00037 #define mD 293.665 00038 #define mE 329.628 00039 #define mF 349.228 00040 #define mG 391.995 00041 #define mA 440.000 00042 #define mB 493.883 00043 00044 // PIDに関する部分 00045 #define IndividualValueL 0.4 00046 #define IndividualValueR 0.4 00047 #define IndividualValueB 0.4 00048 00049 // 0.4 300.0 0.00001 00050 #define PID_Kc 0.6 00051 #define PID_Ti 0.0 // <- 0.0でもいいかも？ 00052 #define PID_Td 0.03 00053 #define RATE 0.00006 00054 00055 #define PID_Kp 0.2 00056 #define PID_Ki 0.0 00057 #define PID_Kd 0.0 00058 00059 00060 00061 00062 // csピン <-> mbedピン の対応 00063 #define cs01 p23 00064 #define cs02 p22 00065 #define cs03 p15 00066 #define cs04 p14 00067 #define cs05 p16 00068 #define cs06 p17 00069 #define cs07 p18 00070 #define cs08 p19 00071 #define cs09 p20 00072 #define cs10 p30 00073 #define cs11 p29 00074 #define cs12 p26 00075 #define cs13 p25 00076 00077 // デバッグ用の定義_ 00078 // #define PING_RLB_ // 超音波（左,右,後） 00079 #define MOTOR_ // モータドライバ 00080 // #define KICK_DIV_ // キッカー, ドリブラー 00081 // #define IR_FR_ // IRボール距離（右前） 00082 // #define IR_BR_ // IRボール距離（右後） 00083 // #define IR_BL_ // IRボール距離（左前） 00084 // #define IR_FL_ // IRボール距離（左後） 00085 #define IR__ // IRボール方向 00086 // #define SDCARD_ // SDカード 00087 #define PING_F_ // 超音波（前,斜め） 00088 #define IMU_ // IMU（LSM9DS0） 00089 #define DEBUG_ // デバッグ 00090 // #define MESSAGE 00091 // #define PROG_TIME 00092 00093 // SPI通信に関するパラメータ 00094 #define SPI_WAIT_US 2 // SPI通信の待ち時間(us) 00095 #define t_SS_SCLK 0.002 // SSを指定してから、通信開始までの待ち時間(us) 00096 00097 // IRに関するマスク用数値 00098 #define DEG045 0x80 00099 #define DEG090 0x40 00100 #define DEG135 0x20 00101 #define DEG180 0x10 00102 #define DEG225 0x08 00103 #define DEG270 0x04 00104 #define DEG315 0x02 00105 #define DEG000 0x01 00106 00107 00108 00109 /*--- ピンの設定 ---*/ 00110 // LEDの設定 00111 DigitalOut led_Motor(LED1, 0); 00112 DigitalOut led_IR(LED2, 0); 00113 DigitalOut led_IMU(LED3, 0); 00114 DigitalOut led4(LED4, 0); 00115 // SW 00116 DigitalIn SW(p24); // OFF then 1, ON then 0 00117 // ラインセンサの許可信号 00118 DigitalOut linePermission(p5, 1); 00119 // ラインセンサのinputピン 00120 DigitalIn lineLeft(p6); 00121 DigitalIn lineRight(p7); 00122 // Kickerのピン設定 00123 DigitalIn kickerInput(cs03); 00124 DigitalOut kickerOuput(p8, 1); 00125 // SPI通信用のピン設定 00126 SPI spi(p11, p12, p13); // mosi, miso, clk 00127 DigitalOut cs_PingRLB(cs01, 1); // 超音波（左,右,後） 00128 DigitalOut cs_Motor(cs02, 1); // モータドライバ 00129 // DigitalOut cs_KickerDribbler(cs03, 1); // キッカー, ドリブラー 00130 DigitalOut cs_Speed(cs04, 1); // マウスセンサ（ADNS9800） 00131 DigitalOut cs_IRFrDistance(cs05, 1); // IRボール距離（右前） 00132 DigitalOut cs_IRBrDistance(cs06, 1); // IRボール距離（右後） 00133 DigitalOut cs_IRBlDistance(cs07, 1); // IRボール距離（左前） 00134 DigitalOut cs_IRFlDistance(cs08, 1); // IRボール距離（左後） 00135 DigitalOut cs_IR(cs09, 1); // IRボール方向 00136 DigitalOut cs_SDcard(cs10, 1); // SDカード 00137 DigitalOut cs_PingF(cs11, 1); // 超音波（前,斜め） 00138 DigitalOut cs_IMU(cs12, 1); // IMU（LSM9DS0） 00139 DigitalOut cs_Debug(cs13, 1); // デバッグ 00140 Ticker pidupdata; 00141 // デバッグ用のSerial通信用のピン設定 00142 PwmOut sound(p21); 00143 #if 0 00144 Serial pc(p9, p10); // tx, rx 00145 #else 00146 Serial pc(USBTX, USBRX); // tx, rx 00147 #endif 00148 00149 // #define XBEE 00150 #ifdef XBEE 00151 Serial xbee(p9, p10); 00152 #endif 00153 00154 00155 00156 /*--- グローバル変数 ---*/ 00157 int LoopCount = 0; 00158 PID pidController(PID_Kc, PID_Ti, PID_Td, RATE); // Kp, Ki, Kd, RATE 00159 float moveDirection = 0.0; // ロボットの進む方向 00160 // PINGに関する変数 00161 uint8_t pingFF, pingFR, pingFL; 00162 uint8_t pingBR, pingBL, pingR, pingL; 00163 // ロボットの行動に関する変数 00164 int Integral=0; 00165 int Direction = 0; 00166 int8_t Angle=0, ex_Angle=0; 00167 int dx=0, ex_dx=0, ex2_dx=0; 00168 float R = 2.3; 00169 float Omega = 0.0; 00170 float Theta = 0.0; 00171 00172 // デバッグに関する変数 00173 Timer t; 00174 uint8_t MyRemoteAddress[] = {0x00,0x13,0xa2,0x00,0x40,0x7c,0x04,0xbd}; // コントローラのアドレス 00175 00176 00177 // プロトタイプ宣言 00178 inline int bitCount(uint8_t bits); 00179 inline void lineMove(); 00180 inline void lineCheck(); 00181 inline bool SWcheck(); 00182 void PingRead(); 00183 inline int IRRead(); 00184 void ImuRead(); 00185 void PwmLRB1(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed); 00186 void PwmLRB2(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed); 00187 void PwmLRB3(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed); 00188 void RegulationHeading(float* pPwmL, float* pPwmR, float* pPwmB); 00189 inline void Motor(float direction, float speed); 00190 inline float MoveDirection(int iBallDirection); 00191 bool spiChecking(); 00192 inline void ControlCommand(uint8_t command); 00193 inline void serialChecking(); 00194 00195 00196 00197 00198 /*--- メッセージ表示用関数 ---*/ 00199 void msgD(int iTab, char *cMsg, int iValue, bool bNew=true) 00200 { 00201 #ifdef MESSAGE 00202 for (int i=0; i<iTab; i++) pc.printf(" "); 00203 pc.printf("%s : %d", cMsg, iValue); 00204 if (bNew) pc.printf("\n"); 00205 else pc.printf(" "); 00206 return; 00207 #endif 00208 } 00209 void msgF(int iTab, char *cMsg, float fValue, bool bNew=true) 00210 { 00211 #ifdef MESSAGE 00212 for (int i=0; i<iTab; i++) pc.printf(" "); 00213 pc.printf("%s : %7.4f", cMsg, fValue); 00214 if (bNew) pc.printf("\n"); 00215 else pc.printf(" "); 00216 return; 00217 #endif 00218 } 00219 void msgX(int iTab, char *cMsg, uint8_t ucValue, bool bNew=true) 00220 { 00221 #ifdef MESSAGE 00222 for (int i=0; i<iTab; i++) pc.printf(" "); 00223 pc.printf("%s : %02x", cMsg, ucValue); 00224 if (bNew) pc.printf("\n"); 00225 else pc.printf(" "); 00226 return; 00227 #endif 00228 } 00229 /*--- エラー表示用関数 ---*/ 00230 void errorMsgHex(int iTab, char *cMsg, uint8_t ucValue, bool bNew=true) // tab, message, value, new line 00231 { 00232 #ifdef MESSAGE 00233 for (int i=0; i<iTab; i++) pc.printf(" "); 00234 pc.printf("error: %s rx_data: %02x", cMsg, ucValue); 00235 if (bNew) pc.printf("\n"); 00236 else pc.printf(" "); 00237 return; 00238 #endif 00239 } 00240 /*--- BitCount関数 ---*/ 00241 inline int bitCount(uint8_t bits) 00242 { 00243 bits = (bits & 0x55) + (bits >> 1 & 0x55); 00244 bits = (bits & 0x33) + (bits >> 2 & 0x33); 00245 bits = (bits & 0x0f) + (bits >> 4 & 0x0f); 00246 bits = (bits & 0xff) + (bits >> 8 & 0xff); 00247 return (bits & 0xff) + (bits >>16 & 0xff); 00248 } 00249 00250 inline void lineMove() 00251 { 00252 #if 1 00253 /*--- 反対方向に進む ---*/ 00254 // if (Direction == 1000.0) 00255 // return; 00256 // else if (Direction > 0.0) 00257 // Motor(Direction-180.0, 1.0); 00258 // else 00259 // Motor(Direction+180.0, 1.0); 00260 // wait(0.4); 00261 int stopDirecetion = 0; 00262 if (Direction > 0.0) 00263 stopDirecetion = Direction-180.0; 00264 else 00265 stopDirecetion = Direction+180.0; 00266 for (int i=0; i<700; i++) { 00267 ImuRead(); 00268 pidController.setProcessValue(Angle); 00269 Omega = pidController.compute(); 00270 Motor(1000.0, 0.0); 00271 } 00272 Motor(stopDirecetion, 0.4); 00273 wait(0.3); 00274 #endif 00275 00276 #if 0 00277 /*--- 中央に戻ろうとする ---*/ 00278 if (Direction == 1000.0) 00279 return; 00280 else if (lineLeft == 1 && lineRight == 0) { 00281 Motor(-90.0, 0.5); 00282 } 00283 else if (lineLeft == 0 && lineRight == 1) { 00284 Motor(90.0, 0.5); 00285 } 00286 else if (lineLeft == 1 && lineRight == 1) { 00287 if (Direction > 0.0) 00288 Motor(Direction-180.0, 1.0); 00289 else 00290 Motor(Direction+180.0, 1.0); 00291 } 00292 else return; 00293 #endif 00294 00295 #if 0 00296 /*--- IRsensorの状況を見て行動 ---*/ 00297 while (IRRead() != 1000.0) { 00298 ImuRead(); 00299 pidController.setProcessValue(Angle); 00300 Omega = pidController.compute(); 00301 Motor(moveDirection, 0.5); 00302 Direction = moveDirection; 00303 } 00304 #endif 00305 } 00306 00307 00308 inline void lineCheck() 00309 { 00310 // ラインセンサの状態をチェック 00311 if (lineLeft || lineRight) { 00312 lineMove(); 00313 } 00314 } 00315 00316 inline bool SWcheck() 00317 { 00318 /*--- 00319 ボタンが押されているかをチェック 00320 - ON-> '1' 00321 - OFF-> '0' 00322 ---*/ 00323 if (!SW) { 00324 wait(0.3); 00325 if (SW) { 00326 wait(0.2); 00327 return 1; 00328 } 00329 } 00330 return 0; 00331 } 00332 00333 /*--- 超音波による距離の測定 ---*/ 00334 void PingRead() 00335 { 00336 cs_PingF = 0; 00337 pingFF = spi.write(0x01); // 開始信号 00338 cs_PingF = 1; 00339 cs_PingF = 0; 00340 pingFF = spi.write(0x02); // 正面 00341 cs_PingF = 1; 00342 cs_PingF = 0; 00343 pingFR = spi.write(0x03); // 右前 00344 cs_PingF = 1; 00345 cs_PingF = 0; 00346 pingFL = spi.write(0x04); // 左前 00347 cs_PingF = 1; 00348 00349 #if 0 00350 cs_PingRLB = 0; 00351 pingL = spi.write(0x01); // 開始信号 00352 cs_PingRLB = 1; 00353 cs_PingRLB = 0; 00354 pingL = spi.write(0x02); // 左 00355 cs_PingRLB = 1; 00356 cs_PingRLB = 0; 00357 pingR = spi.write(0x03); // 右 00358 cs_PingRLB = 1; 00359 cs_PingRLB = 0; 00360 pingBL = spi.write(0x04); // 左後ろ 00361 cs_PingRLB = 1; 00362 cs_PingRLB = 0; 00363 pingBR = spi.write(0x05); // 右後ろ 00364 cs_PingRLB = 1; 00365 #endif 00366 } 00367 00368 /*--- ロボットから見たボールの方向を返す ---*/ 00369 inline int IRRead() 00370 { 00371 /*--- 00372 00373 - IRのnearとfarのデータを取得 00374 - ボールのある方向を返す 00375 00376 ---*/ 00377 00378 uint8_t near, far; 00379 // データを取得する 00380 cs_IR = 0; wait_us(t_SS_SCLK); far = spi.write(0x00); cs_IR = 1; 00381 wait_us(SPI_WAIT_US); 00382 cs_IR = 0; wait_us(t_SS_SCLK); near = ~spi.write(0xFF); cs_IR = 1; 00383 wait_us(SPI_WAIT_US); 00384 // データを解析して、進行方向を決定 00385 int iDirection = 0; 00386 int directionNear = 0; 00387 int directionFar = 0; 00388 // int bitCountNear = bitCount(near); 00389 // int bitCountFar = bitCount(far); 00390 00391 // directionNear = ( ((DEG045 & near)?1:0) * 45 00392 // + ((DEG090 & near)?1:0) * 90 00393 // + ((DEG135 & near)?1:0) * 135 00394 // + ((DEG180 & near)?1:0) * 180 00395 // + ((DEG225 & near)?1:0) * 225 00396 // + ((DEG270 & near)?1:0) * 270 00397 // + ((DEG315 & near)?1:0) * 315 00398 // + ((DEG000 & near)?1:0) * 000) 00399 // / bitCountNear; 00400 // directionFar = ( ((DEG045 & far)?1:0) * 45 00401 // + ((DEG090 & far)?1:0) * 90 00402 // + ((DEG135 & far)?1:0) * 135 00403 // + ((DEG180 & far)?1:0) * 180 00404 // + ((DEG225 & far)?1:0) * 225 00405 // + ((DEG270 & far)?1:0) * 270 00406 // + ((DEG315 & far)?1:0) * 315 00407 // + ((DEG000 & far)?1:0) * 000) 00408 // / bitCountFar; 00409 00410 00411 int iBitCountNear = 0; 00412 bool bBitFlag = false; 00413 int aiAngleData[] = {0, -45, -90, -135, 180, 135, 90, 45}; 00414 int iAngleSum = 0; 00415 int iBaseAngle = 0; 00416 for (int i=0, j=0; i<8; i++) { 00417 if (near & 0x01) { 00418 iBitCountNear++; 00419 if (bBitFlag) { 00420 iAngleSum += aiAngleData[i-j]; 00421 } 00422 else { 00423 bBitFlag = true; 00424 iBaseAngle = aiAngleData[i]; 00425 j = i; 00426 } 00427 } 00428 near >>= 1; 00429 } 00430 iAngleSum /= iBitCountNear; 00431 directionNear = iBaseAngle + iAngleSum; 00432 00433 if (iBitCountNear > 0) { 00434 iDirection = directionNear; 00435 } 00436 // else if (bitCountFar > 0) { 00437 // iDirection = directionFar; 00438 // } 00439 else { 00440 iDirection = 1000.0; 00441 } 00442 00443 // 角度を -180~+180 に修正 00444 if (iDirection > 180.0 && iDirection != 1000.0){ 00445 iDirection -= 360.0; 00446 } 00447 else if (iDirection < -180.0 && iDirection != 1000.0){ 00448 iDirection += 360.0; 00449 } 00450 00451 msgD(0, "Direction", iDirection); 00452 00453 return iDirection; 00454 } 00455 00456 /*--- Union ---*/ 00457 union uni_8bits { 00458 uint8_t uint8; 00459 int8_t int8; 00460 }; 00461 /*--- IMUのデータを取得、Angleに代入 ---*/ 00462 void ImuRead() 00463 { 00464 // 変数宣言 00465 int count = 5; 00466 uni_8bits data = {0}; 00467 // データを受信 00468 do { 00469 cs_IMU = 0; 00470 data.uint8 = spi.write(0x50); 00471 cs_IMU = 1; 00472 count--; 00473 wait_us(SPI_WAIT_US); 00474 } while ((data.uint8==0xFF || data.uint8==0x00) && count); 00475 // 0~255 -> -128~127 00476 Angle = data.int8; 00477 msgD(0, "IMU", Angle); 00478 } 00479 /*--- 偏差から操作量Omegaを求める関数 ---*/ 00480 #if 0 00481 void GetOmega() { 00482 // 偏差の差を求める 00483 if (-3<=Angle && Angle<=3) Angle=0; 00484 00485 dx = Angle - ex_Angle; 00486 00487 if (-3<=dx && dx<=3) { 00488 dx = 0; 00489 Angle = ex_Angle = (Angle+ex_Angle) / 2; 00490 } 00491 Integral /= 10; 00492 Integral += (Angle + ex_Angle) / 2; 00493 00494 #if 1 00495 // 操作量Omegaを求める（PID制御） 00496 Omega = - (PID_Kp * (float)(Angle / 127.0)) 00497 - (PID_Ki * (float)(Integral / 127.0)) 00498 - (PID_Kd * (float)(dx / 127.0)); 00499 #else 00500 // webページものも 00501 Omega+= PID_Kp * (float)(dx) 00502 +PID_Ki * (float)(Angle) 00503 +PID_Kd * (float)((dx - ex_dx)); 00504 #endif 00505 00506 if (Omega < -1.0) { 00507 Omega = -1.0; 00508 } 00509 else if (Omega > 1.0) { 00510 Omega = 1.0; 00511 } 00512 00513 msgF(0, "Omega", Omega); 00514 00515 // データをずらす 00516 ex_dx = dx; 00517 ex_Angle = Angle; 00518 } 00519 #endif 00520 00521 /*--- pwmLRBを求める関数 ---*/ 00522 void PwmLRB1(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed) 00523 { 00524 /*--- 00525 00526 このサイトに載っている公式を使用 (http://wiki.tokor.org/index.php?%A5%AA%A5%E0%A5%CB%A5%DB%A5%A4%A1%BC%A5%EB%A4%CB%A4%E8%A4%EB%C1%B4%CA%FD%B8%FE%B0%DC%C6%B0%BC%D6%A4%CE%C0%A9%B8%E6%A1%A1-%A1%A1%B5%A1%B3%A3%C0%A9%B8%E6) 00527 00528 ---*/ 00529 float pwmL = vx * cos(Theta+PI*5/6)+ vy * sin(Theta+PI*5/6) - R*Omega; 00530 float pwmR = vx * cos(Theta+PI/6) + vy * sin(Theta+PI/6) - R*Omega; 00531 float pwmB = vx * cos(Theta-PI/2) + vy * sin(Theta-PI/2) - R*Omega; 00532 /*--- 最大値を求める ---*/ 00533 float pwmMAX = fabs(pwmL); 00534 float aPwmR = fabs(pwmR); 00535 float aPwmB = fabs(pwmB); 00536 pwmMAX = (pwmMAX > aPwmR) ? pwmMAX : aPwmR; 00537 pwmMAX = (pwmMAX > aPwmB) ? pwmMAX : aPwmB; 00538 /*--- 求めた最大値で正規化 ---*/ 00539 pwmL = pwmL / pwmMAX * speed * IndividualValueL; 00540 pwmR = pwmR / pwmMAX * speed * IndividualValueR; 00541 pwmB = pwmB / pwmMAX * speed * IndividualValueB; 00542 00543 /*--- 値を反映 ---*/ 00544 *pPwmL = pwmL; 00545 *pPwmR = pwmR; 00546 *pPwmB = pwmB; 00547 } 00548 void PwmLRB2(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed) 00549 { 00550 float pwmL = vx * cos(Theta+PI*5/6)+ vy * sin(Theta+PI*5/6); 00551 float pwmR = vx * cos(Theta+PI/6) + vy * sin(Theta+PI/6) ; 00552 float pwmB = vx * cos(Theta-PI/2) + vy * sin(Theta-PI/2) ; 00553 /*--- 最大値を求める ---*/ 00554 float pwmMAX = fabs(pwmL); 00555 float aPwmR = fabs(pwmR); 00556 float aPwmB = fabs(pwmB); 00557 pwmMAX = (pwmMAX > aPwmR) ? pwmMAX : aPwmR; 00558 pwmMAX = (pwmMAX > aPwmB) ? pwmMAX : aPwmB; 00559 /*--- 求めた最大値で正規化 ---*/ 00560 pwmL = pwmL / pwmMAX * speed * IndividualValueL * 0.7 - 0.3*Omega; 00561 pwmR = pwmR / pwmMAX * speed * IndividualValueR * 0.7 - 0.3*Omega; 00562 pwmB = pwmB / pwmMAX * speed * IndividualValueB * 0.7 - 0.3*Omega; 00563 00564 /*--- 値を反映 ---*/ 00565 *pPwmL = pwmL; 00566 *pPwmR = pwmR; 00567 *pPwmB = pwmB; 00568 } 00569 void PwmLRB3(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed) 00570 { 00571 /*--- 00572 00573 このサイトに載っていた方法の応用を使う (http://yukispanicworld.tumblr.com/post/106053191409) 00574 - 旋回は、求めた値の割合の変化分を足し合わせることによって行う 00575 00576 ---*/ 00577 float pwmL = vx * cos(Theta+PI*5/6)+ vy * sin(Theta+PI*5/6); 00578 float pwmR = vx * cos(Theta+PI/6) + vy * sin(Theta+PI/6) ; 00579 float pwmB = vx * cos(Theta-PI/2) + vy * sin(Theta-PI/2) ; 00580 /*--- 最大値を求める ---*/ 00581 float pwmMAX = fabs(pwmL); 00582 float aPwmR = fabs(pwmR); 00583 float aPwmB = fabs(pwmB); 00584 pwmMAX = (pwmMAX > aPwmR) ? pwmMAX : aPwmR; 00585 pwmMAX = (pwmMAX > aPwmB) ? pwmMAX : aPwmB; 00586 /*--- 求めた最大値で正規化 ---*/ 00587 pwmL = pwmL / pwmMAX * speed; 00588 pwmR = pwmR / pwmMAX * speed; 00589 pwmB = pwmB / pwmMAX * speed; 00590 /*--- 旋回を考慮する ---*/ 00591 float k = 0.07; 00592 pwmL += pwmL * k*Omega; 00593 pwmR += pwmR * k*Omega; 00594 pwmB += pwmB * k*Omega; 00595 /*--- 値を反映 ---*/ 00596 *pPwmL = pwmL; 00597 *pPwmR = pwmR; 00598 *pPwmB = pwmB; 00599 } 00600 void RegulationHeading(float* pPwmL, float* pPwmR, float* pPwmB) 00601 { 00602 float k = 1.0; // 調節用の係数 00603 float pwmL = - Omega * k * IndividualValueL; 00604 float pwmR = - Omega * k * IndividualValueR; 00605 float pwmB = - Omega * k * IndividualValueB; 00606 00607 /*--- 値を反映 ---*/ 00608 *pPwmL = pwmL; 00609 *pPwmR = pwmR; 00610 *pPwmB = pwmB; 00611 } 00612 /*--- モータの計算と通信 ---*/ 00613 inline void Motor(float direction, float speed) 00614 { 00615 uint8_t rx_data; 00616 00617 /*--- 計算に必要となる値 --- 00618 * 方向 ... direction (deg) 00619 * 速度 ... speed (m/s) 00620 * 機体の回転数 ... Omega (rad/s) 00621 * 機体の半径 ... R (m) 00622 * 機体の基準に対する傾き ... Theta (rad) 00623 * pwmL(左), pwmB(後), pwmR(右) 00624 --*/ 00625 00626 // 変数宣言 00627 float pwmL, pwmB, pwmR; 00628 00629 00630 00631 // 停止コマンドのときの処理 00632 if (direction == 1000.0) { 00633 RegulationHeading(&pwmL, &pwmR, &pwmB); 00634 } 00635 // 普通のときの処理 00636 else { 00637 // Theta = Angle * Byte2Rad; 00638 direction *= Deg2Rad; 00639 float vx = cos(direction); 00640 float vy = sin(direction); 00641 00642 PwmLRB1(&pwmL, &pwmR, &pwmB, vx, vy, speed); 00643 // PwmLRB2(&pwmL, &pwmR, &pwmB, vx, vy, speed); 00644 // PwmLRB3(&pwmL, &pwmR, &pwmB, vx, vy, speed); 00645 } 00646 00647 msgF(0, "pwmL", pwmL, false); 00648 msgF(0, "pwmR", pwmR, false); 00649 msgF(0, "pwmB", pwmB, false); 00650 00651 // 回転方向を決定 と マイナスだったらプラスに変える 00652 uint8_t data0 = 0x3F; // 0011 1111 00653 if (pwmL!=0.0 && pwmR!=0.0 && pwmB!=0.0) { 00654 // 左モータ 00655 if (pwmL>0) { 00656 data0 &= 0x37; // 0011 0111 00657 } 00658 else { 00659 data0 &= 0x3B; // 0011 1011 00660 pwmL *= -1.0; 00661 } 00662 if (pwmL>1.0) pwmL=1.0; 00663 00664 // 右モータ 00665 if (pwmR>0) { 00666 data0 &= 0x3D; // 0011 1101 00667 } 00668 else { 00669 data0 &= 0x3E; // 0011 1110 00670 pwmR *= -1.0; 00671 } 00672 if (pwmR>1.0) pwmR=1.0; 00673 00674 // 後モータ 00675 if (pwmB>0) { 00676 data0 &= 0x1F; // 0001 1111 00677 } 00678 else { 00679 data0 &= 0x2F; // 0010 1111 00680 pwmB *= -1.0; 00681 } 00682 if (pwmB>1.0) pwmB=1.0; 00683 } 00684 else data0 = 0x00; 00685 00686 uint8_t dataL = (uint8_t)(pwmL * 255.0); // 左モータの回転数 00687 uint8_t dataR = (uint8_t)(pwmR * 255.0); // 右モータの回転数 00688 uint8_t dataB = (uint8_t)(pwmB * 255.0); // 後モータの回転数 00689 if (dataL == 0x00) { 00690 data0 &= 0x33; // 0011 0011 00691 data0 |= 0x0C; 00692 dataL++; 00693 } 00694 if (dataR == 0x00) { 00695 data0 &= 0x3C; // 0011 1100 00696 data0 |= 0x03; 00697 dataR++; 00698 } 00699 if (dataB == 0x00) { 00700 data0 &= 0x0F; // 0000 1111 00701 data0 |= 0x30; 00702 dataB++; 00703 } 00704 00705 // データ確認用の値を埋め込む 00706 dataL = dataL & 0xFC | 0x01; 00707 dataR = dataR & 0xFC | 0x02; 00708 dataB = dataB & 0xFC | 0x03; 00709 00710 // デバッグのため、送信データを表示 00711 // pc.printf(" "); 00712 msgX(0, "data0", data0, false); 00713 msgX(0, "dataL", dataL, false); 00714 msgX(0, "dataR", dataR, false); 00715 msgX(0, "dataB", dataB, true); 00716 00717 // データを送信 00718 int error = 0; 00719 int count = 0; 00720 while (1) { 00721 // 通信開始の合図 00722 cs_Motor=0; wait_us(t_SS_SCLK); 00723 rx_data=spi.write((uint8_t)(error%4)); wait_us(t_SS_SCLK); 00724 cs_Motor=1; 00725 if (rx_data!=0xAA) { 00726 errorMsgHex(0, "start", rx_data); 00727 // errorMsgHex(0, "start_data", (uint8_t)(error%4)); 00728 } 00729 wait_us(SPI_WAIT_US); 00730 // 回転方向のデータ 00731 do { 00732 cs_Motor=0; wait_us(t_SS_SCLK); 00733 rx_data=spi.write(data0); wait_us(t_SS_SCLK); 00734 cs_Motor=1; 00735 count++; 00736 } while(rx_data != 0x00 && count < 5); 00737 if (rx_data!=0x00) { 00738 errorMsgHex(0, "data0", rx_data); 00739 led_Motor = 0; 00740 error++; 00741 continue; 00742 } else led_Motor = 1; 00743 msgD(0, "count", count); 00744 count = 0; 00745 wait_us(SPI_WAIT_US); 00746 // MotorLのデータ 00747 do { 00748 cs_Motor=0; wait_us(t_SS_SCLK); 00749 rx_data=spi.write(dataL); wait_us(t_SS_SCLK); 00750 cs_Motor=1; 00751 count++; 00752 } while(rx_data != data0 && count < 5); 00753 if (rx_data!=data0) { 00754 errorMsgHex(0, "dataL", rx_data); 00755 led_Motor = 0; 00756 error++; 00757 continue; 00758 } else led_Motor = 1; 00759 msgD(0, "count", count); 00760 count = 0; 00761 wait_us(SPI_WAIT_US); 00762 // MotorRのデータ 00763 do { 00764 cs_Motor=0; wait_us(t_SS_SCLK); 00765 rx_data=spi.write(dataR); wait_us(t_SS_SCLK); 00766 cs_Motor=1; 00767 count++; 00768 } while(rx_data != dataL && count < 5); 00769 if (rx_data!=dataL) { 00770 errorMsgHex(0, "dataR", rx_data); 00771 led_Motor = 0; 00772 error++; 00773 continue; 00774 } else led_Motor = 1; 00775 msgD(0, "count", count); 00776 count = 0; 00777 wait_us(SPI_WAIT_US); 00778 // MotorBのデータ 00779 do { 00780 cs_Motor=0; wait_us(t_SS_SCLK); 00781 rx_data=spi.write(dataB); wait_us(t_SS_SCLK); 00782 cs_Motor=1; 00783 count++; 00784 } while(rx_data != dataR && count < 5); 00785 if (rx_data!=dataR) { 00786 errorMsgHex(0, "dataB", rx_data); 00787 led_Motor = 0; 00788 error++; 00789 continue; 00790 } else led_Motor = 1; 00791 msgD(0, "count", count); 00792 count = 0; 00793 wait_us(SPI_WAIT_US); 00794 00795 break; 00796 } 00797 00798 return; 00799 } 00800 00801 /*--- ボールの方向から、ロボットの進行方向を決める関数 ---*/ 00802 inline float MoveDirection(int iBallDirection) 00803 { 00804 /*--- 00805 00806 - ボールの方向から、ロボットの進行方向を求める関数 00807 - -180 < iBallDirection < +180 00808 00809 ---*/ 00810 00811 if (iBallDirection == 1000.0) return 1000.0; 00812 00813 float fMoveDirection = 0.0; 00814 if (-30<iBallDirection && iBallDirection<30) fMoveDirection = 0.0; 00815 else if (iBallDirection > 0) fMoveDirection = iBallDirection+60.0; 00816 else if (iBallDirection < 0) fMoveDirection = iBallDirection-60.0; 00817 else fMoveDirection = 180.0; 00818 00819 // 角度を -180~+180 に修正 00820 if (fMoveDirection > 180.0){ 00821 return fMoveDirection - 360.0; 00822 } 00823 if (fMoveDirection < -180.0){ 00824 return fMoveDirection + 360.0; 00825 } 00826 00827 return fMoveDirection; 00828 } 00829 00830 00831 /*--- SPI通信確認 ---*/ 00832 bool spiChecking() 00833 { 00834 int spicheck = 0; 00835 uint8_t data = 0; 00836 uint8_t missData1 = 0; 00837 uint8_t missData2 = 0; 00838 uint8_t missData3 = 0; 00839 uint8_t missData4 = 0; 00840 pc.printf("\nChecking SPI...\n"); 00841 00842 #ifdef PING_RLB_ 00843 cs_PingRLB = 0; missData1 = spi.write(0x50); cs_PingRLB = 1; wait_us(SPI_WAIT_US); 00844 cs_PingRLB = 0; missData2 = spi.write(0x50); cs_PingRLB = 1; wait_us(SPI_WAIT_US); 00845 cs_PingRLB = 0; data = spi.write(0x50); cs_PingRLB = 1; 00846 if (data != 0xAA) { 00847 pc.printf("##Bad## mbed <-> LPC1114 <-> Ping(RLB)\n"); 00848 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data); 00849 spicheck++; 00850 } 00851 #endif 00852 00853 #ifdef MOTOR_ 00854 led_Motor = 0; 00855 // ちゃんと値が入っているかをチェック 00856 cs_Motor = 0; missData1 = spi.write(0x51); cs_Motor = 1; wait_us(SPI_WAIT_US); 00857 cs_Motor = 0; missData2 = spi.write(0x51); cs_Motor = 1; wait_us(SPI_WAIT_US); 00858 cs_Motor = 0; missData3 = spi.write(0x51); cs_Motor = 1; wait_us(SPI_WAIT_US); 00859 cs_Motor = 0; missData4 = spi.write(0x51); cs_Motor = 1; wait_us(SPI_WAIT_US); 00860 cs_Motor = 0; data = spi.write(0x01); cs_Motor = 1; 00861 pc.printf(" ReturnValue1 = %x\n ReturnValue2 = %x\n ReturnValue3 = %x\n ReturnValue4 = %x\n ReturnValue5 = %x\n", missData1, missData2, missData3, missData4, data); 00862 // SPI通信のチェック 00863 cs_Motor = 0; missData1 = spi.write(0x50); cs_Motor = 1; wait_us(SPI_WAIT_US); 00864 cs_Motor = 0; missData2 = spi.write(0x50); cs_Motor = 1; wait_us(SPI_WAIT_US); 00865 cs_Motor = 0; data = spi.write(0x01); cs_Motor = 1; 00866 if (data != 0xAA) { 00867 pc.printf("##Bad## mbed <-> CY8C29466 <-> Motor\n"); 00868 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data); 00869 spicheck++; 00870 } 00871 else led_Motor=1; 00872 #endif 00873 00874 #ifdef KICK_DIV_ 00875 cs_KickerDribbler = 0; missData1 = spi.write(0x50); cs_KickerDribbler = 1; wait_us(SPI_WAIT_US); 00876 cs_KickerDribbler = 0; missData2 = spi.write(0x50); cs_KickerDribbler = 1; wait_us(SPI_WAIT_US); 00877 cs_KickerDribbler = 0; 00878 data = spi.write(0x01); 00879 cs_KickerDribbler = 1; 00880 if (data != 0xAA) { 00881 pc.printf("##Bad## mbed <-> CY8C29466 <-> Kicker&Dribbler\n"); 00882 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data); 00883 spicheck++; 00884 } 00885 #endif 00886 00887 #ifdef IR_FR_ 00888 cs_IRFrDistance = 0; missData1 = spi.write(0x50); cs_IRFrDistance = 1; wait_us(SPI_WAIT_US); 00889 cs_IRFrDistance = 0; missData2 = spi.write(0x50); cs_IRFrDistance = 1; wait_us(SPI_WAIT_US); 00890 cs_IRFrDistance = 0; data = spi.write(0x01); cs_IRFrDistance = 1; 00891 if (data != 0xAA) { 00892 pc.printf("##Bad## mbed <-> CY8C24123A <-> IR_FR_distance\n"); 00893 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data); 00894 spicheck++; 00895 } 00896 #endif 00897 00898 #ifdef IR_BR_ 00899 cs_IRBrDistance = 0; missData1 = spi.write(0x50); cs_IRBrDistance = 1; wait_us(SPI_WAIT_US); 00900 cs_IRBrDistance = 0; missData2 = spi.write(0x50); cs_IRBrDistance = 1; wait_us(SPI_WAIT_US); 00901 cs_IRBrDistance = 0; data = spi.write(0x01); cs_IRBrDistance = 1; 00902 if (data != 0xAA) { 00903 pc.printf("##Bad## mbed <-> CY8C24123A <-> IR_BR_distance\n"); 00904 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data); 00905 spicheck++; 00906 } 00907 #endif 00908 00909 #ifdef IR_BL_ 00910 cs_IRBlDistance = 0; missData1 = spi.write(0x50); cs_IRBlDistance = 1; wait_us(SPI_WAIT_US); 00911 cs_IRBlDistance = 0; missData2 = spi.write(0x50); cs_IRBlDistance = 1; wait_us(SPI_WAIT_US); 00912 cs_IRBlDistance = 0; data = spi.write(0x01); cs_IRBlDistance = 1; 00913 if (data != 0xAA) { 00914 pc.printf("##Bad## mbed <-> CY8C24123A <-> IR_BL_distance\n"); 00915 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data); 00916 spicheck++; 00917 } 00918 #endif 00919 00920 #ifdef IR_FL_ 00921 cs_IRFlDistance = 0; missData1 = spi.write(0x50); cs_IRFlDistance = 1; wait_us(SPI_WAIT_US); 00922 cs_IRFlDistance = 0; missData2 = spi.write(0x50); cs_IRFlDistance = 1; wait_us(SPI_WAIT_US); 00923 cs_IRFlDistance = 0; data = spi.write(0x01); cs_IRFlDistance = 1; 00924 if (data != 0xAA) { 00925 pc.printf("##Bad## mbed <-> CY8C24123A <-> IR_FL_distance\n"); 00926 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data); 00927 spicheck++; 00928 } 00929 #endif 00930 00931 #ifdef IR__ 00932 led_IR = 0; 00933 cs_IR = 0; missData1 = spi.write(0x50); cs_IR = 1; wait_us(SPI_WAIT_US); 00934 cs_IR = 0; missData2 = spi.write(0x50); cs_IR = 1; wait_us(SPI_WAIT_US); 00935 cs_IR = 0; data = spi.write(0x01); cs_IR = 1; 00936 if (data != 0xAA) { 00937 pc.printf("##Bad## mbed <-> CY8C29466 <-> IR_direction\n"); 00938 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data); 00939 spicheck++; 00940 } 00941 else led_IR=1; 00942 #endif 00943 00944 #ifdef PING_F_ 00945 cs_PingF = 0; missData1 = spi.write(0x50); cs_PingF = 1; wait_us(SPI_WAIT_US); 00946 cs_PingF = 0; missData2 = spi.write(0x50); cs_PingF = 1; wait_us(SPI_WAIT_US); 00947 cs_PingF = 0; data = spi.write(0x01); cs_PingF = 1; 00948 if (data != 0xAA) { 00949 pc.printf("##Bad## mbed <-> LPC1114 <-> Ping(F)\n"); 00950 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data); 00951 spicheck++; 00952 } 00953 #endif 00954 00955 #ifdef IMU_ 00956 led_IMU = 0; 00957 int count = 5; 00958 do { 00959 cs_IMU = 0; data = spi.write(0x50); cs_IMU = 1; wait_us(SPI_WAIT_US); 00960 pc.printf("data : %x\n", data); 00961 count--; 00962 } while ((data==0xFF || data==0x00) && count); 00963 00964 if (count < 1) { 00965 spicheck++; 00966 pc.printf("##Bad## mbed <-> LPC1114 <-> LSM9DS0\n"); 00967 } 00968 else led_IMU=1; 00969 pc.printf(" IMU : %d\n", data); 00970 #endif 00971 00972 #ifdef DEBUG_ 00973 cs_Debug = 0; missData1 = spi.write(0x50); cs_Debug = 1; wait_us(SPI_WAIT_US); 00974 cs_Debug = 0; missData2 = spi.write(0x50); cs_Debug = 1; wait_us(SPI_WAIT_US); 00975 cs_Debug = 0; data = spi.write(0x01); cs_Debug = 1; 00976 if (data != 0xAA) { 00977 pc.printf("##Bad## mbed <-> LPC1114_DEBUG\n"); 00978 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data); 00979 spicheck++; 00980 } 00981 #endif 00982 00983 // 最後にメッセージを表示 00984 if (spicheck) { 00985 pc.printf("waring : %d\n", spicheck); 00986 led4 = 0; 00987 // while (!pc.readable()); // PCから指示があるまで待つ 00988 // pc.getc(); 00989 return 0; 00990 wait(2); 00991 } 00992 else { 00993 pc.printf("All SPIdevices complete!!\n"); 00994 led4 = 1; 00995 return 1; 00996 wait(1); 00997 } 00998 pc.printf("\n"); 00999 } 01000 01001 01002 /*--- PCとの通信用関数 ---*/ 01003 inline void ControlCommand(uint8_t command) 01004 { 01005 /*--- コマンド一覧 --- 01006 * 必要なparameter 01007 0x01 PID_Kp PID制御の比例 01008 0x02 PID_Ki PID制御の積分 01009 0x03 PID_Kd PID制御の微分 01010 0x04 Robo_R 機体の半径 01011 01012 * 必要なcommand 01013 0x10 modeStop 停止コマンド 01014 0x11 modeBattle1 戦闘モード1 01015 0x12 modeBattle2 戦闘モード2 01016 0x13 modeDebug デバッグモード 01017 0x21 goFlont 前進 01018 0x22 goBack 後退 01019 0x23 goLeft 左 01020 0x24 goRight 右 01021 0x25 turnCW 時計回りに一回転 01022 0x26 turnCCW 反時計回りに一回転 01023 0x31 checkSensor センサーチェック 01024 0x31 checkIMU IMUからの情報を画面に表示 01025 0xAA Reset リセット（SDカードから、値を再読み込み） 01026 ---*/ 01027 float wait_time = 1.0; 01028 switch (command) 01029 { 01030 case 0xAA: 01031 break; 01032 case 0x13: // modeDebug 01033 spiChecking(); 01034 wait(3); 01035 break; 01036 case 0x21: // goFlont 01037 Motor(0.0, 0.3); 01038 wait(wait_time); 01039 break; 01040 case 0x22: // goBack 01041 Motor(180.0, 0.3); 01042 wait(wait_time); 01043 break; 01044 case 0x23: // goLeft 01045 Motor(-90.0, 0.3); 01046 wait(wait_time); 01047 break; 01048 case 0x24: // goRight 01049 Motor(90.0, 0.3); 01050 wait(wait_time); 01051 break; 01052 default: 01053 break; 01054 } 01055 } 01056 01057 01058 #ifdef XBEE 01059 /*--- 送信用関数 ---*/ 01060 void xbeeTx(uint8_t *address, uint8_t data) { 01061 int i=0; 01062 01063 xbee.putc(0x7E); 01064 xbee.putc(0x00); // フレーム長 01065 xbee.putc(0x0F); 01066 xbee.putc(0x10); // フレームタイプ 01067 xbee.putc(0x01); // フレームID 01068 for (int i=0; i<8; i++) 01069 xbee.putc(address[i]); // 62bit宛先アドレス 01070 xbee.putc(0xFF); // 16bit宛先ネットワークアドレス 01071 xbee.putc(0xFE); 01072 xbee.putc(0x00); // ブロードキャスト半径 01073 xbee.putc(0x00); // オプション 01074 xbee.putc(data); // RFデータ 01075 // チェックサムの計算 01076 uint8_t checksum = 0xFF - (0x10+0x01+0xFF+0xFE+data); 01077 for (i=0; i<8; i++) checksum -= address[i]; 01078 xbee.putc(checksum); // チェックサム 01079 01080 // 送信ステータスの確認 01081 if (xbee.readable()) { 01082 if (xbee.getc() == 0x7E) { 01083 for (i=0; i<8; i++) xbee.getc(); 01084 if (xbee.getc() == 0x00) 01085 // pc.printf("Send success!\n"); 01086 else 01087 // pc.printf("Send missed.\n"); 01088 for (i=9; i<10; i++) xbee.getc(); 01089 } 01090 } 01091 } 01092 /*--- 受信用関数 ---*/ 01093 xbeeAPI xbeeRx() { 01094 xbeeAPI returnApi; // 返り値用の変数 01095 returnApi.reset(); 01096 if (xbee.readable() > 0) { 01097 // pc.printf("readable\n"); 01098 if (xbee.getc() == 0x7E) { 01099 // pc.printf("startBit\n"); 01100 int i=0; 01101 uint8_t buffer[18]; 01102 01103 buffer[0] = 0x7E; 01104 for (i=1; i<18; i++) { 01105 buffer[i] = xbee.getc(); 01106 } 01107 // 送信元アドレスを抽出 01108 for (i=0; i<8; i++) { 01109 returnApi.address[i] = buffer[i+4]; 01110 } 01111 // 受信データを抽出 01112 returnApi.direction = buffer[15]; 01113 returnApi.turning = buffer[16]; 01114 01115 // データをPCに表示 01116 // for (i=0; i<8; i++) pc.printf("%x", returnApi.address[i]); 01117 // pc.printf("\n"); 01118 // pc.printf("%x\n", returnApi.direction); 01119 // pc.printf("%x\n", returnApi.turning); 01120 } 01121 } 01122 return returnApi; 01123 } 01124 #endif 01125 01126 /*--- serialの通信チェック関数 ---*/ 01127 inline void serialChecking() 01128 { 01129 if ( pc.readable() ) { 01130 uint8_t command = pc.getc(); 01131 printf("command : %x\n", pc.getc()); 01132 ControlCommand(command); 01133 } 01134 } 01135 01136 01137 01138 01139 int main() 01140 { 01141 /*--- 変数宣言 ---*/ 01142 int direction = 0; // ボールのある方向 01143 float speed = 0.0; // ロボットの速度 01144 01145 /*--- 初期処理 ---*/ 01146 // 音 01147 float mm[]={mC,mD,mE,mF,mG,mA,mB,mC*2}; // ドレミファソラシ 01148 sound.period(1.0/mm[0]); 01149 sound = 0.03; 01150 // Serialの設定 01151 pc.baud(115200); 01152 // SPIの初期設定 01153 spi.format(8, 3); // DataFlameのビット数(4~16) 01154 spi.frequency(1000000); // クロック周波数(デフォルト:1MHz) 01155 // ラインセンサに許可信号を 01156 linePermission=1; 01157 wait(1); // 各モジュールのセットアップが完了するまで待つ 01158 01159 // PIDライブラリの初期設定 01160 pidController.setInputLimits(-128.0, 127.0); 01161 pidController.setOutputLimits(-1.0, 1.0); 01162 pidController.setBias(0.0); 01163 pidController.setMode(AUTO_MODE); 01164 pidController.setSetPoint(0.0); 01165 // pidupdata.attach(&ImuRead, RATE); 01166 01167 // lineの設定 01168 // lineLeft.rise(&lineMove); 01169 // lineRight.rise(&lineMove); 01170 01171 // SPIの通信確認 01172 while ( !spiChecking() ); 01173 01174 01175 /*--- スタートの合図を待つ ---*/ 01176 kickerOuput = 1; 01177 // SWが押されるまで待つ 01178 sound.period(1.0/mm[1]); 01179 sound = 0.06; 01180 while(!SWcheck()); 01181 // 音を止める 01182 sound = 0.00; 01183 01184 01185 /*--- メイン処理 ---*/ 01186 while(true) { 01187 01188 #ifdef PROG_TIME 01189 t.start(); 01190 #endif 01191 01192 /*--- pcとの通信があったかをCheck ---*/ 01193 // serialChecking(); 01194 /*--- ストップボタンをCheck ---*/ 01195 if (SWcheck()) { 01196 Motor(0.0, 0.0); 01197 sound.period(1.0/mm[1]); 01198 sound = 0.06; 01199 while(!SWcheck()); 01200 sound = 0.00; 01201 } 01202 lineCheck(); 01203 01204 // PingRead(); 01205 01206 /*--- 現在の方角を確認 ---*/ 01207 // Kicker使用時は、IMUの値は参考にならないので無視する 01208 // if (kickerInput) { 01209 // kickerOuput = 1; 01210 // wait(0.8); 01211 // kickerOuput = 0; 01212 // } else { 01213 // ImuRead(); 01214 // pidController.setProcessValue(Angle); 01215 // } 01216 ImuRead(); 01217 pidController.setProcessValue(Angle); 01218 lineCheck(); 01219 01220 /*--- ロボットから見たボールの方向を確認 ---*/ 01221 direction = IRRead(); 01222 lineCheck(); 01223 01224 /*--- ボールの方向の絶対値を算出 ---*/ 01225 if (direction != 1000.0) { 01226 direction += Angle; 01227 if (direction > 180.0){ 01228 direction -= 360.0; 01229 } 01230 else if (direction < -180.0){ 01231 direction += 360.0; 01232 } 01233 } 01234 lineCheck(); 01235 01236 /*--- ロボットの進行方向を決定 ---*/ 01237 moveDirection = MoveDirection(direction); 01238 // pc.printf("%f\n", moveDirection); 01239 lineCheck(); 01240 01241 /*--- 修正のための角速度を算出 ---*/ 01242 // GetOmega(); 01243 Omega = pidController.compute(); 01244 if (-5<Angle && Angle<5) { 01245 Omega = 0.0; 01246 } 01247 lineCheck(); 01248 01249 /*--- 実際にロボットを動かす ---*/ 01250 // moveDirection = 0.0; 01251 // moveDirection = 1000.0; 01252 Motor(moveDirection, 0.4); 01253 lineCheck(); 01254 01255 Direction = moveDirection; 01256 01257 01258 #ifdef PROG_TIME 01259 t.stop(); 01260 pc.printf("%f\n", t.read()); 01261 t.reset(); 01262 #endif 01263 01264 LoopCount++; 01265 } 01266 } 01267 01268 01269 01270 01271 01272 01273 01274 01275 01276 01277 01278
Generated on Fri Aug 12 2022 22:37:42 by
1.7.2