Naoya Muramatsu / Mbed 2 deprecated RoboCup_2015

ロボカップJrのブロック大会2015で使用したプログラム

Dependencies:   BurstSPI LSM9DS0 PID PWMMotorDriver SDFileSystem mbed

main.cpp@0:ab144c574081, 2016-05-11 (annotated)

Committer:
denden
Date:
Wed May 11 09:39:07 2016 +0000
Revision:
0:ab144c574081
publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
denden 0:ab144c574081 1 /*------------------------
denden 0:ab144c574081 2
denden 0:ab144c574081 3 * XBee1
denden 0:ab144c574081 4 ID : 5102
denden 0:ab144c574081 5
denden 0:ab144c574081 6 DH : 0013A200
denden 0:ab144c574081 7 DL : 407ABE67
denden 0:ab144c574081 8
denden 0:ab144c574081 9 * XBee2
denden 0:ab144c574081 10 ID : 5102
denden 0:ab144c574081 11
denden 0:ab144c574081 12 DH : 0013A200
denden 0:ab144c574081 13 DL : 407C04BD
denden 0:ab144c574081 14
denden 0:ab144c574081 15 ------------------------*/
denden 0:ab144c574081 16
denden 0:ab144c574081 17 /*------------------------
denden 0:ab144c574081 18 mv /Users/muramatsunaoya/Downloads/RoboCup_2015_LPC1768.bin /Volumes/MBED/ && diskutil unmount /Volumes/MBED/
denden 0:ab144c574081 19 ------------------------*/
denden 0:ab144c574081 20
denden 0:ab144c574081 21
denden 0:ab144c574081 22
denden 0:ab144c574081 23 #include "mbed.h"
denden 0:ab144c574081 24 // ライブラリ
denden 0:ab144c574081 25 #include "MotorDriver.h"
denden 0:ab144c574081 26 #include "PID.h"
denden 0:ab144c574081 27 // 自作のクラス
denden 0:ab144c574081 28 #include "xbeeAPI.h"
denden 0:ab144c574081 29
denden 0:ab144c574081 30
denden 0:ab144c574081 31 /*--- 定義 ---*/
denden 0:ab144c574081 32 #define PI 3.1415926
denden 0:ab144c574081 33 #define Deg2Rad (3.1415926/180)
denden 0:ab144c574081 34 #define Byte2Rad (3.1415926/255/2)
denden 0:ab144c574081 35
denden 0:ab144c574081 36 #define mC 261.626
denden 0:ab144c574081 37 #define mD 293.665
denden 0:ab144c574081 38 #define mE 329.628
denden 0:ab144c574081 39 #define mF 349.228
denden 0:ab144c574081 40 #define mG 391.995
denden 0:ab144c574081 41 #define mA 440.000
denden 0:ab144c574081 42 #define mB 493.883
denden 0:ab144c574081 43
denden 0:ab144c574081 44 // PIDに関する部分
denden 0:ab144c574081 45 #define IndividualValueL 0.4
denden 0:ab144c574081 46 #define IndividualValueR 0.4
denden 0:ab144c574081 47 #define IndividualValueB 0.4
denden 0:ab144c574081 48
denden 0:ab144c574081 49 // 0.4 300.0 0.00001
denden 0:ab144c574081 50 #define PID_Kc 0.6
denden 0:ab144c574081 51 #define PID_Ti 0.0 // <- 0.0でもいいかも？
denden 0:ab144c574081 52 #define PID_Td 0.03
denden 0:ab144c574081 53 #define RATE 0.00006
denden 0:ab144c574081 54
denden 0:ab144c574081 55 #define PID_Kp 0.2
denden 0:ab144c574081 56 #define PID_Ki 0.0
denden 0:ab144c574081 57 #define PID_Kd 0.0
denden 0:ab144c574081 58
denden 0:ab144c574081 59
denden 0:ab144c574081 60
denden 0:ab144c574081 61
denden 0:ab144c574081 62 // csピン <-> mbedピン　の対応
denden 0:ab144c574081 63 #define cs01 p23
denden 0:ab144c574081 64 #define cs02 p22
denden 0:ab144c574081 65 #define cs03 p15
denden 0:ab144c574081 66 #define cs04 p14
denden 0:ab144c574081 67 #define cs05 p16
denden 0:ab144c574081 68 #define cs06 p17
denden 0:ab144c574081 69 #define cs07 p18
denden 0:ab144c574081 70 #define cs08 p19
denden 0:ab144c574081 71 #define cs09 p20
denden 0:ab144c574081 72 #define cs10 p30
denden 0:ab144c574081 73 #define cs11 p29
denden 0:ab144c574081 74 #define cs12 p26
denden 0:ab144c574081 75 #define cs13 p25
denden 0:ab144c574081 76
denden 0:ab144c574081 77 // デバッグ用の定義_
denden 0:ab144c574081 78 // #define PING_RLB_ // 超音波（左,右,後）
denden 0:ab144c574081 79 #define MOTOR_ // モータドライバ
denden 0:ab144c574081 80 // #define KICK_DIV_ // キッカー, ドリブラー
denden 0:ab144c574081 81 // #define IR_FR_ // IRボール距離（右前）
denden 0:ab144c574081 82 // #define IR_BR_ // IRボール距離（右後）
denden 0:ab144c574081 83 // #define IR_BL_ // IRボール距離（左前）
denden 0:ab144c574081 84 // #define IR_FL_ // IRボール距離（左後）
denden 0:ab144c574081 85 #define IR__ // IRボール方向
denden 0:ab144c574081 86 // #define SDCARD_ // SDカード
denden 0:ab144c574081 87 #define PING_F_ // 超音波（前,斜め）
denden 0:ab144c574081 88 #define IMU_ // IMU（LSM9DS0）
denden 0:ab144c574081 89 #define DEBUG_ // デバッグ
denden 0:ab144c574081 90 // #define MESSAGE
denden 0:ab144c574081 91 // #define PROG_TIME
denden 0:ab144c574081 92
denden 0:ab144c574081 93 // SPI通信に関するパラメータ
denden 0:ab144c574081 94 #define SPI_WAIT_US 2 // SPI通信の待ち時間(us)
denden 0:ab144c574081 95 #define t_SS_SCLK 0.002 // SSを指定してから、通信開始までの待ち時間(us)
denden 0:ab144c574081 96
denden 0:ab144c574081 97 // IRに関するマスク用数値
denden 0:ab144c574081 98 #define DEG045 0x80
denden 0:ab144c574081 99 #define DEG090 0x40
denden 0:ab144c574081 100 #define DEG135 0x20
denden 0:ab144c574081 101 #define DEG180 0x10
denden 0:ab144c574081 102 #define DEG225 0x08
denden 0:ab144c574081 103 #define DEG270 0x04
denden 0:ab144c574081 104 #define DEG315 0x02
denden 0:ab144c574081 105 #define DEG000 0x01
denden 0:ab144c574081 106
denden 0:ab144c574081 107
denden 0:ab144c574081 108
denden 0:ab144c574081 109 /*--- ピンの設定 ---*/
denden 0:ab144c574081 110 // LEDの設定
denden 0:ab144c574081 111 DigitalOut led_Motor(LED1, 0);
denden 0:ab144c574081 112 DigitalOut led_IR(LED2, 0);
denden 0:ab144c574081 113 DigitalOut led_IMU(LED3, 0);
denden 0:ab144c574081 114 DigitalOut led4(LED4, 0);
denden 0:ab144c574081 115 // SW
denden 0:ab144c574081 116 DigitalIn SW(p24); // OFF then 1, ON then 0
denden 0:ab144c574081 117 // ラインセンサの許可信号
denden 0:ab144c574081 118 DigitalOut linePermission(p5, 1);
denden 0:ab144c574081 119 // ラインセンサのinputピン
denden 0:ab144c574081 120 DigitalIn lineLeft(p6);
denden 0:ab144c574081 121 DigitalIn lineRight(p7);
denden 0:ab144c574081 122 // Kickerのピン設定
denden 0:ab144c574081 123 DigitalIn kickerInput(cs03);
denden 0:ab144c574081 124 DigitalOut kickerOuput(p8, 1);
denden 0:ab144c574081 125 // SPI通信用のピン設定
denden 0:ab144c574081 126 SPI spi(p11, p12, p13); // mosi, miso, clk
denden 0:ab144c574081 127 DigitalOut cs_PingRLB(cs01, 1); // 超音波（左,右,後）
denden 0:ab144c574081 128 DigitalOut cs_Motor(cs02, 1); // モータドライバ
denden 0:ab144c574081 129 // DigitalOut cs_KickerDribbler(cs03, 1); // キッカー, ドリブラー
denden 0:ab144c574081 130 DigitalOut cs_Speed(cs04, 1); // マウスセンサ（ADNS9800）
denden 0:ab144c574081 131 DigitalOut cs_IRFrDistance(cs05, 1); // IRボール距離（右前）
denden 0:ab144c574081 132 DigitalOut cs_IRBrDistance(cs06, 1); // IRボール距離（右後）
denden 0:ab144c574081 133 DigitalOut cs_IRBlDistance(cs07, 1); // IRボール距離（左前）
denden 0:ab144c574081 134 DigitalOut cs_IRFlDistance(cs08, 1); // IRボール距離（左後）
denden 0:ab144c574081 135 DigitalOut cs_IR(cs09, 1); // IRボール方向
denden 0:ab144c574081 136 DigitalOut cs_SDcard(cs10, 1); // SDカード
denden 0:ab144c574081 137 DigitalOut cs_PingF(cs11, 1); // 超音波（前,斜め）
denden 0:ab144c574081 138 DigitalOut cs_IMU(cs12, 1); // IMU（LSM9DS0）
denden 0:ab144c574081 139 DigitalOut cs_Debug(cs13, 1); // デバッグ
denden 0:ab144c574081 140 Ticker pidupdata;
denden 0:ab144c574081 141 // デバッグ用のSerial通信用のピン設定
denden 0:ab144c574081 142 PwmOut sound(p21);
denden 0:ab144c574081 143 #if 0
denden 0:ab144c574081 144 Serial pc(p9, p10); // tx, rx
denden 0:ab144c574081 145 #else
denden 0:ab144c574081 146 Serial pc(USBTX, USBRX); // tx, rx
denden 0:ab144c574081 147 #endif
denden 0:ab144c574081 148
denden 0:ab144c574081 149 // #define XBEE
denden 0:ab144c574081 150 #ifdef XBEE
denden 0:ab144c574081 151 Serial xbee(p9, p10);
denden 0:ab144c574081 152 #endif
denden 0:ab144c574081 153
denden 0:ab144c574081 154
denden 0:ab144c574081 155
denden 0:ab144c574081 156 /*--- グローバル変数 ---*/
denden 0:ab144c574081 157 int LoopCount = 0;
denden 0:ab144c574081 158 PID pidController(PID_Kc, PID_Ti, PID_Td, RATE); // Kp, Ki, Kd, RATE
denden 0:ab144c574081 159 float moveDirection = 0.0; // ロボットの進む方向
denden 0:ab144c574081 160 // PINGに関する変数
denden 0:ab144c574081 161 uint8_t pingFF, pingFR, pingFL;
denden 0:ab144c574081 162 uint8_t pingBR, pingBL, pingR, pingL;
denden 0:ab144c574081 163 // ロボットの行動に関する変数
denden 0:ab144c574081 164 int Integral=0;
denden 0:ab144c574081 165 int Direction = 0;
denden 0:ab144c574081 166 int8_t Angle=0, ex_Angle=0;
denden 0:ab144c574081 167 int dx=0, ex_dx=0, ex2_dx=0;
denden 0:ab144c574081 168 float R = 2.3;
denden 0:ab144c574081 169 float Omega = 0.0;
denden 0:ab144c574081 170 float Theta = 0.0;
denden 0:ab144c574081 171
denden 0:ab144c574081 172 // デバッグに関する変数
denden 0:ab144c574081 173 Timer t;
denden 0:ab144c574081 174 uint8_t MyRemoteAddress[] = {0x00,0x13,0xa2,0x00,0x40,0x7c,0x04,0xbd}; // コントローラのアドレス
denden 0:ab144c574081 175
denden 0:ab144c574081 176
denden 0:ab144c574081 177 // プロトタイプ宣言
denden 0:ab144c574081 178 inline int bitCount(uint8_t bits);
denden 0:ab144c574081 179 inline void lineMove();
denden 0:ab144c574081 180 inline void lineCheck();
denden 0:ab144c574081 181 inline bool SWcheck();
denden 0:ab144c574081 182 void PingRead();
denden 0:ab144c574081 183 inline int IRRead();
denden 0:ab144c574081 184 void ImuRead();
denden 0:ab144c574081 185 void PwmLRB1(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed);
denden 0:ab144c574081 186 void PwmLRB2(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed);
denden 0:ab144c574081 187 void PwmLRB3(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed);
denden 0:ab144c574081 188 void RegulationHeading(float* pPwmL, float* pPwmR, float* pPwmB);
denden 0:ab144c574081 189 inline void Motor(float direction, float speed);
denden 0:ab144c574081 190 inline float MoveDirection(int iBallDirection);
denden 0:ab144c574081 191 bool spiChecking();
denden 0:ab144c574081 192 inline void ControlCommand(uint8_t command);
denden 0:ab144c574081 193 inline void serialChecking();
denden 0:ab144c574081 194
denden 0:ab144c574081 195
denden 0:ab144c574081 196
denden 0:ab144c574081 197
denden 0:ab144c574081 198 /*--- メッセージ表示用関数 ---*/
denden 0:ab144c574081 199 void msgD(int iTab, char *cMsg, int iValue, bool bNew=true)
denden 0:ab144c574081 200 {
denden 0:ab144c574081 201 #ifdef MESSAGE
denden 0:ab144c574081 202 for (int i=0; i<iTab; i++) pc.printf(" ");
denden 0:ab144c574081 203 pc.printf("%s : %d", cMsg, iValue);
denden 0:ab144c574081 204 if (bNew) pc.printf("\n");
denden 0:ab144c574081 205 else pc.printf(" ");
denden 0:ab144c574081 206 return;
denden 0:ab144c574081 207 #endif
denden 0:ab144c574081 208 }
denden 0:ab144c574081 209 void msgF(int iTab, char *cMsg, float fValue, bool bNew=true)
denden 0:ab144c574081 210 {
denden 0:ab144c574081 211 #ifdef MESSAGE
denden 0:ab144c574081 212 for (int i=0; i<iTab; i++) pc.printf(" ");
denden 0:ab144c574081 213 pc.printf("%s : %7.4f", cMsg, fValue);
denden 0:ab144c574081 214 if (bNew) pc.printf("\n");
denden 0:ab144c574081 215 else pc.printf(" ");
denden 0:ab144c574081 216 return;
denden 0:ab144c574081 217 #endif
denden 0:ab144c574081 218 }
denden 0:ab144c574081 219 void msgX(int iTab, char *cMsg, uint8_t ucValue, bool bNew=true)
denden 0:ab144c574081 220 {
denden 0:ab144c574081 221 #ifdef MESSAGE
denden 0:ab144c574081 222 for (int i=0; i<iTab; i++) pc.printf(" ");
denden 0:ab144c574081 223 pc.printf("%s : %02x", cMsg, ucValue);
denden 0:ab144c574081 224 if (bNew) pc.printf("\n");
denden 0:ab144c574081 225 else pc.printf(" ");
denden 0:ab144c574081 226 return;
denden 0:ab144c574081 227 #endif
denden 0:ab144c574081 228 }
denden 0:ab144c574081 229 /*--- エラー表示用関数 ---*/
denden 0:ab144c574081 230 void errorMsgHex(int iTab, char *cMsg, uint8_t ucValue, bool bNew=true) // tab, message, value, new line
denden 0:ab144c574081 231 {
denden 0:ab144c574081 232 #ifdef MESSAGE
denden 0:ab144c574081 233 for (int i=0; i<iTab; i++) pc.printf(" ");
denden 0:ab144c574081 234 pc.printf("error: %s rx_data: %02x", cMsg, ucValue);
denden 0:ab144c574081 235 if (bNew) pc.printf("\n");
denden 0:ab144c574081 236 else pc.printf(" ");
denden 0:ab144c574081 237 return;
denden 0:ab144c574081 238 #endif
denden 0:ab144c574081 239 }
denden 0:ab144c574081 240 /*--- BitCount関数 ---*/
denden 0:ab144c574081 241 inline int bitCount(uint8_t bits)
denden 0:ab144c574081 242 {
denden 0:ab144c574081 243 bits = (bits & 0x55) + (bits >> 1 & 0x55);
denden 0:ab144c574081 244 bits = (bits & 0x33) + (bits >> 2 & 0x33);
denden 0:ab144c574081 245 bits = (bits & 0x0f) + (bits >> 4 & 0x0f);
denden 0:ab144c574081 246 bits = (bits & 0xff) + (bits >> 8 & 0xff);
denden 0:ab144c574081 247 return (bits & 0xff) + (bits >>16 & 0xff);
denden 0:ab144c574081 248 }
denden 0:ab144c574081 249
denden 0:ab144c574081 250 inline void lineMove()
denden 0:ab144c574081 251 {
denden 0:ab144c574081 252 #if 1
denden 0:ab144c574081 253 /*--- 反対方向に進む ---*/
denden 0:ab144c574081 254 // if (Direction == 1000.0)
denden 0:ab144c574081 255 // return;
denden 0:ab144c574081 256 // else if (Direction > 0.0)
denden 0:ab144c574081 257 // Motor(Direction-180.0, 1.0);
denden 0:ab144c574081 258 // else
denden 0:ab144c574081 259 // Motor(Direction+180.0, 1.0);
denden 0:ab144c574081 260 // wait(0.4);
denden 0:ab144c574081 261 int stopDirecetion = 0;
denden 0:ab144c574081 262 if (Direction > 0.0)
denden 0:ab144c574081 263 stopDirecetion = Direction-180.0;
denden 0:ab144c574081 264 else
denden 0:ab144c574081 265 stopDirecetion = Direction+180.0;
denden 0:ab144c574081 266 for (int i=0; i<700; i++) {
denden 0:ab144c574081 267 ImuRead();
denden 0:ab144c574081 268 pidController.setProcessValue(Angle);
denden 0:ab144c574081 269 Omega = pidController.compute();
denden 0:ab144c574081 270 Motor(1000.0, 0.0);
denden 0:ab144c574081 271 }
denden 0:ab144c574081 272 Motor(stopDirecetion, 0.4);
denden 0:ab144c574081 273 wait(0.3);
denden 0:ab144c574081 274 #endif
denden 0:ab144c574081 275
denden 0:ab144c574081 276 #if 0
denden 0:ab144c574081 277 /*--- 中央に戻ろうとする ---*/
denden 0:ab144c574081 278 if (Direction == 1000.0)
denden 0:ab144c574081 279 return;
denden 0:ab144c574081 280 else if (lineLeft == 1 && lineRight == 0) {
denden 0:ab144c574081 281 Motor(-90.0, 0.5);
denden 0:ab144c574081 282 }
denden 0:ab144c574081 283 else if (lineLeft == 0 && lineRight == 1) {
denden 0:ab144c574081 284 Motor(90.0, 0.5);
denden 0:ab144c574081 285 }
denden 0:ab144c574081 286 else if (lineLeft == 1 && lineRight == 1) {
denden 0:ab144c574081 287 if (Direction > 0.0)
denden 0:ab144c574081 288 Motor(Direction-180.0, 1.0);
denden 0:ab144c574081 289 else
denden 0:ab144c574081 290 Motor(Direction+180.0, 1.0);
denden 0:ab144c574081 291 }
denden 0:ab144c574081 292 else return;
denden 0:ab144c574081 293 #endif
denden 0:ab144c574081 294
denden 0:ab144c574081 295 #if 0
denden 0:ab144c574081 296 /*--- IRsensorの状況を見て行動 ---*/
denden 0:ab144c574081 297 while (IRRead() != 1000.0) {
denden 0:ab144c574081 298 ImuRead();
denden 0:ab144c574081 299 pidController.setProcessValue(Angle);
denden 0:ab144c574081 300 Omega = pidController.compute();
denden 0:ab144c574081 301 Motor(moveDirection, 0.5);
denden 0:ab144c574081 302 Direction = moveDirection;
denden 0:ab144c574081 303 }
denden 0:ab144c574081 304 #endif
denden 0:ab144c574081 305 }
denden 0:ab144c574081 306
denden 0:ab144c574081 307
denden 0:ab144c574081 308 inline void lineCheck()
denden 0:ab144c574081 309 {
denden 0:ab144c574081 310 // ラインセンサの状態をチェック
denden 0:ab144c574081 311 if (lineLeft || lineRight) {
denden 0:ab144c574081 312 lineMove();
denden 0:ab144c574081 313 }
denden 0:ab144c574081 314 }
denden 0:ab144c574081 315
denden 0:ab144c574081 316 inline bool SWcheck()
denden 0:ab144c574081 317 {
denden 0:ab144c574081 318 /*---
denden 0:ab144c574081 319 ボタンが押されているかをチェック
denden 0:ab144c574081 320 - ON-> '1'
denden 0:ab144c574081 321 - OFF-> '0'
denden 0:ab144c574081 322 ---*/
denden 0:ab144c574081 323 if (!SW) {
denden 0:ab144c574081 324 wait(0.3);
denden 0:ab144c574081 325 if (SW) {
denden 0:ab144c574081 326 wait(0.2);
denden 0:ab144c574081 327 return 1;
denden 0:ab144c574081 328 }
denden 0:ab144c574081 329 }
denden 0:ab144c574081 330 return 0;
denden 0:ab144c574081 331 }
denden 0:ab144c574081 332
denden 0:ab144c574081 333 /*--- 超音波による距離の測定 ---*/
denden 0:ab144c574081 334 void PingRead()
denden 0:ab144c574081 335 {
denden 0:ab144c574081 336 cs_PingF = 0;
denden 0:ab144c574081 337 pingFF = spi.write(0x01); // 開始信号
denden 0:ab144c574081 338 cs_PingF = 1;
denden 0:ab144c574081 339 cs_PingF = 0;
denden 0:ab144c574081 340 pingFF = spi.write(0x02); // 正面
denden 0:ab144c574081 341 cs_PingF = 1;
denden 0:ab144c574081 342 cs_PingF = 0;
denden 0:ab144c574081 343 pingFR = spi.write(0x03); // 右前
denden 0:ab144c574081 344 cs_PingF = 1;
denden 0:ab144c574081 345 cs_PingF = 0;
denden 0:ab144c574081 346 pingFL = spi.write(0x04); // 左前
denden 0:ab144c574081 347 cs_PingF = 1;
denden 0:ab144c574081 348
denden 0:ab144c574081 349 #if 0
denden 0:ab144c574081 350 cs_PingRLB = 0;
denden 0:ab144c574081 351 pingL = spi.write(0x01); // 開始信号
denden 0:ab144c574081 352 cs_PingRLB = 1;
denden 0:ab144c574081 353 cs_PingRLB = 0;
denden 0:ab144c574081 354 pingL = spi.write(0x02); // 左
denden 0:ab144c574081 355 cs_PingRLB = 1;
denden 0:ab144c574081 356 cs_PingRLB = 0;
denden 0:ab144c574081 357 pingR = spi.write(0x03); // 右
denden 0:ab144c574081 358 cs_PingRLB = 1;
denden 0:ab144c574081 359 cs_PingRLB = 0;
denden 0:ab144c574081 360 pingBL = spi.write(0x04); // 左後ろ
denden 0:ab144c574081 361 cs_PingRLB = 1;
denden 0:ab144c574081 362 cs_PingRLB = 0;
denden 0:ab144c574081 363 pingBR = spi.write(0x05); // 右後ろ
denden 0:ab144c574081 364 cs_PingRLB = 1;
denden 0:ab144c574081 365 #endif
denden 0:ab144c574081 366 }
denden 0:ab144c574081 367
denden 0:ab144c574081 368 /*--- ロボットから見たボールの方向を返す ---*/
denden 0:ab144c574081 369 inline int IRRead()
denden 0:ab144c574081 370 {
denden 0:ab144c574081 371 /*---
denden 0:ab144c574081 372
denden 0:ab144c574081 373 - IRのnearとfarのデータを取得
denden 0:ab144c574081 374 - ボールのある方向を返す
denden 0:ab144c574081 375
denden 0:ab144c574081 376 ---*/
denden 0:ab144c574081 377
denden 0:ab144c574081 378 uint8_t near, far;
denden 0:ab144c574081 379 // データを取得する
denden 0:ab144c574081 380 cs_IR = 0; wait_us(t_SS_SCLK); far = spi.write(0x00); cs_IR = 1;
denden 0:ab144c574081 381 wait_us(SPI_WAIT_US);
denden 0:ab144c574081 382 cs_IR = 0; wait_us(t_SS_SCLK); near = ~spi.write(0xFF); cs_IR = 1;
denden 0:ab144c574081 383 wait_us(SPI_WAIT_US);
denden 0:ab144c574081 384 // データを解析して、進行方向を決定
denden 0:ab144c574081 385 int iDirection = 0;
denden 0:ab144c574081 386 int directionNear = 0;
denden 0:ab144c574081 387 int directionFar = 0;
denden 0:ab144c574081 388 // int bitCountNear = bitCount(near);
denden 0:ab144c574081 389 // int bitCountFar = bitCount(far);
denden 0:ab144c574081 390
denden 0:ab144c574081 391 // directionNear = ( ((DEG045 & near)?1:0) * 45
denden 0:ab144c574081 392 // + ((DEG090 & near)?1:0) * 90
denden 0:ab144c574081 393 // + ((DEG135 & near)?1:0) * 135
denden 0:ab144c574081 394 // + ((DEG180 & near)?1:0) * 180
denden 0:ab144c574081 395 // + ((DEG225 & near)?1:0) * 225
denden 0:ab144c574081 396 // + ((DEG270 & near)?1:0) * 270
denden 0:ab144c574081 397 // + ((DEG315 & near)?1:0) * 315
denden 0:ab144c574081 398 // + ((DEG000 & near)?1:0) * 000)
denden 0:ab144c574081 399 // / bitCountNear;
denden 0:ab144c574081 400 // directionFar = ( ((DEG045 & far)?1:0) * 45
denden 0:ab144c574081 401 // + ((DEG090 & far)?1:0) * 90
denden 0:ab144c574081 402 // + ((DEG135 & far)?1:0) * 135
denden 0:ab144c574081 403 // + ((DEG180 & far)?1:0) * 180
denden 0:ab144c574081 404 // + ((DEG225 & far)?1:0) * 225
denden 0:ab144c574081 405 // + ((DEG270 & far)?1:0) * 270
denden 0:ab144c574081 406 // + ((DEG315 & far)?1:0) * 315
denden 0:ab144c574081 407 // + ((DEG000 & far)?1:0) * 000)
denden 0:ab144c574081 408 // / bitCountFar;
denden 0:ab144c574081 409
denden 0:ab144c574081 410
denden 0:ab144c574081 411 int iBitCountNear = 0;
denden 0:ab144c574081 412 bool bBitFlag = false;
denden 0:ab144c574081 413 int aiAngleData[] = {0, -45, -90, -135, 180, 135, 90, 45};
denden 0:ab144c574081 414 int iAngleSum = 0;
denden 0:ab144c574081 415 int iBaseAngle = 0;
denden 0:ab144c574081 416 for (int i=0, j=0; i<8; i++) {
denden 0:ab144c574081 417 if (near & 0x01) {
denden 0:ab144c574081 418 iBitCountNear++;
denden 0:ab144c574081 419 if (bBitFlag) {
denden 0:ab144c574081 420 iAngleSum += aiAngleData[i-j];
denden 0:ab144c574081 421 }
denden 0:ab144c574081 422 else {
denden 0:ab144c574081 423 bBitFlag = true;
denden 0:ab144c574081 424 iBaseAngle = aiAngleData[i];
denden 0:ab144c574081 425 j = i;
denden 0:ab144c574081 426 }
denden 0:ab144c574081 427 }
denden 0:ab144c574081 428 near >>= 1;
denden 0:ab144c574081 429 }
denden 0:ab144c574081 430 iAngleSum /= iBitCountNear;
denden 0:ab144c574081 431 directionNear = iBaseAngle + iAngleSum;
denden 0:ab144c574081 432
denden 0:ab144c574081 433 if (iBitCountNear > 0) {
denden 0:ab144c574081 434 iDirection = directionNear;
denden 0:ab144c574081 435 }
denden 0:ab144c574081 436 // else if (bitCountFar > 0) {
denden 0:ab144c574081 437 // iDirection = directionFar;
denden 0:ab144c574081 438 // }
denden 0:ab144c574081 439 else {
denden 0:ab144c574081 440 iDirection = 1000.0;
denden 0:ab144c574081 441 }
denden 0:ab144c574081 442
denden 0:ab144c574081 443 // 角度を -180~+180 に修正
denden 0:ab144c574081 444 if (iDirection > 180.0 && iDirection != 1000.0){
denden 0:ab144c574081 445 iDirection -= 360.0;
denden 0:ab144c574081 446 }
denden 0:ab144c574081 447 else if (iDirection < -180.0 && iDirection != 1000.0){
denden 0:ab144c574081 448 iDirection += 360.0;
denden 0:ab144c574081 449 }
denden 0:ab144c574081 450
denden 0:ab144c574081 451 msgD(0, "Direction", iDirection);
denden 0:ab144c574081 452
denden 0:ab144c574081 453 return iDirection;
denden 0:ab144c574081 454 }
denden 0:ab144c574081 455
denden 0:ab144c574081 456 /*--- Union ---*/
denden 0:ab144c574081 457 union uni_8bits {
denden 0:ab144c574081 458 uint8_t uint8;
denden 0:ab144c574081 459 int8_t int8;
denden 0:ab144c574081 460 };
denden 0:ab144c574081 461 /*--- IMUのデータを取得、Angleに代入 ---*/
denden 0:ab144c574081 462 void ImuRead()
denden 0:ab144c574081 463 {
denden 0:ab144c574081 464 // 変数宣言
denden 0:ab144c574081 465 int count = 5;
denden 0:ab144c574081 466 uni_8bits data = {0};
denden 0:ab144c574081 467 // データを受信
denden 0:ab144c574081 468 do {
denden 0:ab144c574081 469 cs_IMU = 0;
denden 0:ab144c574081 470 data.uint8 = spi.write(0x50);
denden 0:ab144c574081 471 cs_IMU = 1;
denden 0:ab144c574081 472 count--;
denden 0:ab144c574081 473 wait_us(SPI_WAIT_US);
denden 0:ab144c574081 474 } while ((data.uint8==0xFF || data.uint8==0x00) && count);
denden 0:ab144c574081 475 // 0~255 -> -128~127
denden 0:ab144c574081 476 Angle = data.int8;
denden 0:ab144c574081 477 msgD(0, "IMU", Angle);
denden 0:ab144c574081 478 }
denden 0:ab144c574081 479 /*--- 偏差から操作量Omegaを求める関数 ---*/
denden 0:ab144c574081 480 #if 0
denden 0:ab144c574081 481 void GetOmega() {
denden 0:ab144c574081 482 // 偏差の差を求める
denden 0:ab144c574081 483 if (-3<=Angle && Angle<=3) Angle=0;
denden 0:ab144c574081 484
denden 0:ab144c574081 485 dx = Angle - ex_Angle;
denden 0:ab144c574081 486
denden 0:ab144c574081 487 if (-3<=dx && dx<=3) {
denden 0:ab144c574081 488 dx = 0;
denden 0:ab144c574081 489 Angle = ex_Angle = (Angle+ex_Angle) / 2;
denden 0:ab144c574081 490 }
denden 0:ab144c574081 491 Integral /= 10;
denden 0:ab144c574081 492 Integral += (Angle + ex_Angle) / 2;
denden 0:ab144c574081 493
denden 0:ab144c574081 494 #if 1
denden 0:ab144c574081 495 // 操作量Omegaを求める（PID制御）
denden 0:ab144c574081 496 Omega = - (PID_Kp * (float)(Angle / 127.0))
denden 0:ab144c574081 497 - (PID_Ki * (float)(Integral / 127.0))
denden 0:ab144c574081 498 - (PID_Kd * (float)(dx / 127.0));
denden 0:ab144c574081 499 #else
denden 0:ab144c574081 500 // webページものも
denden 0:ab144c574081 501 Omega+= PID_Kp * (float)(dx)
denden 0:ab144c574081 502 +PID_Ki * (float)(Angle)
denden 0:ab144c574081 503 +PID_Kd * (float)((dx - ex_dx));
denden 0:ab144c574081 504 #endif
denden 0:ab144c574081 505
denden 0:ab144c574081 506 if (Omega < -1.0) {
denden 0:ab144c574081 507 Omega = -1.0;
denden 0:ab144c574081 508 }
denden 0:ab144c574081 509 else if (Omega > 1.0) {
denden 0:ab144c574081 510 Omega = 1.0;
denden 0:ab144c574081 511 }
denden 0:ab144c574081 512
denden 0:ab144c574081 513 msgF(0, "Omega", Omega);
denden 0:ab144c574081 514
denden 0:ab144c574081 515 // データをずらす
denden 0:ab144c574081 516 ex_dx = dx;
denden 0:ab144c574081 517 ex_Angle = Angle;
denden 0:ab144c574081 518 }
denden 0:ab144c574081 519 #endif
denden 0:ab144c574081 520
denden 0:ab144c574081 521 /*--- pwmLRBを求める関数 ---*/
denden 0:ab144c574081 522 void PwmLRB1(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed)
denden 0:ab144c574081 523 {
denden 0:ab144c574081 524 /*---
denden 0:ab144c574081 525
denden 0:ab144c574081 526 このサイトに載っている公式を使用 (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)
denden 0:ab144c574081 527
denden 0:ab144c574081 528 ---*/
denden 0:ab144c574081 529 float pwmL = vx * cos(Theta+PI*5/6)+ vy * sin(Theta+PI*5/6) - R*Omega;
denden 0:ab144c574081 530 float pwmR = vx * cos(Theta+PI/6) + vy * sin(Theta+PI/6) - R*Omega;
denden 0:ab144c574081 531 float pwmB = vx * cos(Theta-PI/2) + vy * sin(Theta-PI/2) - R*Omega;
denden 0:ab144c574081 532 /*--- 最大値を求める ---*/
denden 0:ab144c574081 533 float pwmMAX = fabs(pwmL);
denden 0:ab144c574081 534 float aPwmR = fabs(pwmR);
denden 0:ab144c574081 535 float aPwmB = fabs(pwmB);
denden 0:ab144c574081 536 pwmMAX = (pwmMAX > aPwmR) ? pwmMAX : aPwmR;
denden 0:ab144c574081 537 pwmMAX = (pwmMAX > aPwmB) ? pwmMAX : aPwmB;
denden 0:ab144c574081 538 /*--- 求めた最大値で正規化 ---*/
denden 0:ab144c574081 539 pwmL = pwmL / pwmMAX * speed * IndividualValueL;
denden 0:ab144c574081 540 pwmR = pwmR / pwmMAX * speed * IndividualValueR;
denden 0:ab144c574081 541 pwmB = pwmB / pwmMAX * speed * IndividualValueB;
denden 0:ab144c574081 542
denden 0:ab144c574081 543 /*--- 値を反映 ---*/
denden 0:ab144c574081 544 *pPwmL = pwmL;
denden 0:ab144c574081 545 *pPwmR = pwmR;
denden 0:ab144c574081 546 *pPwmB = pwmB;
denden 0:ab144c574081 547 }
denden 0:ab144c574081 548 void PwmLRB2(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed)
denden 0:ab144c574081 549 {
denden 0:ab144c574081 550 float pwmL = vx * cos(Theta+PI*5/6)+ vy * sin(Theta+PI*5/6);
denden 0:ab144c574081 551 float pwmR = vx * cos(Theta+PI/6) + vy * sin(Theta+PI/6) ;
denden 0:ab144c574081 552 float pwmB = vx * cos(Theta-PI/2) + vy * sin(Theta-PI/2) ;
denden 0:ab144c574081 553 /*--- 最大値を求める ---*/
denden 0:ab144c574081 554 float pwmMAX = fabs(pwmL);
denden 0:ab144c574081 555 float aPwmR = fabs(pwmR);
denden 0:ab144c574081 556 float aPwmB = fabs(pwmB);
denden 0:ab144c574081 557 pwmMAX = (pwmMAX > aPwmR) ? pwmMAX : aPwmR;
denden 0:ab144c574081 558 pwmMAX = (pwmMAX > aPwmB) ? pwmMAX : aPwmB;
denden 0:ab144c574081 559 /*--- 求めた最大値で正規化 ---*/
denden 0:ab144c574081 560 pwmL = pwmL / pwmMAX * speed * IndividualValueL * 0.7 - 0.3*Omega;
denden 0:ab144c574081 561 pwmR = pwmR / pwmMAX * speed * IndividualValueR * 0.7 - 0.3*Omega;
denden 0:ab144c574081 562 pwmB = pwmB / pwmMAX * speed * IndividualValueB * 0.7 - 0.3*Omega;
denden 0:ab144c574081 563
denden 0:ab144c574081 564 /*--- 値を反映 ---*/
denden 0:ab144c574081 565 *pPwmL = pwmL;
denden 0:ab144c574081 566 *pPwmR = pwmR;
denden 0:ab144c574081 567 *pPwmB = pwmB;
denden 0:ab144c574081 568 }
denden 0:ab144c574081 569 void PwmLRB3(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed)
denden 0:ab144c574081 570 {
denden 0:ab144c574081 571 /*---
denden 0:ab144c574081 572
denden 0:ab144c574081 573 このサイトに載っていた方法の応用を使う (http://yukispanicworld.tumblr.com/post/106053191409)
denden 0:ab144c574081 574 - 旋回は、求めた値の割合の変化分を足し合わせることによって行う
denden 0:ab144c574081 575
denden 0:ab144c574081 576 ---*/
denden 0:ab144c574081 577 float pwmL = vx * cos(Theta+PI*5/6)+ vy * sin(Theta+PI*5/6);
denden 0:ab144c574081 578 float pwmR = vx * cos(Theta+PI/6) + vy * sin(Theta+PI/6) ;
denden 0:ab144c574081 579 float pwmB = vx * cos(Theta-PI/2) + vy * sin(Theta-PI/2) ;
denden 0:ab144c574081 580 /*--- 最大値を求める ---*/
denden 0:ab144c574081 581 float pwmMAX = fabs(pwmL);
denden 0:ab144c574081 582 float aPwmR = fabs(pwmR);
denden 0:ab144c574081 583 float aPwmB = fabs(pwmB);
denden 0:ab144c574081 584 pwmMAX = (pwmMAX > aPwmR) ? pwmMAX : aPwmR;
denden 0:ab144c574081 585 pwmMAX = (pwmMAX > aPwmB) ? pwmMAX : aPwmB;
denden 0:ab144c574081 586 /*--- 求めた最大値で正規化 ---*/
denden 0:ab144c574081 587 pwmL = pwmL / pwmMAX * speed;
denden 0:ab144c574081 588 pwmR = pwmR / pwmMAX * speed;
denden 0:ab144c574081 589 pwmB = pwmB / pwmMAX * speed;
denden 0:ab144c574081 590 /*--- 旋回を考慮する ---*/
denden 0:ab144c574081 591 float k = 0.07;
denden 0:ab144c574081 592 pwmL += pwmL * k*Omega;
denden 0:ab144c574081 593 pwmR += pwmR * k*Omega;
denden 0:ab144c574081 594 pwmB += pwmB * k*Omega;
denden 0:ab144c574081 595 /*--- 値を反映 ---*/
denden 0:ab144c574081 596 *pPwmL = pwmL;
denden 0:ab144c574081 597 *pPwmR = pwmR;
denden 0:ab144c574081 598 *pPwmB = pwmB;
denden 0:ab144c574081 599 }
denden 0:ab144c574081 600 void RegulationHeading(float* pPwmL, float* pPwmR, float* pPwmB)
denden 0:ab144c574081 601 {
denden 0:ab144c574081 602 float k = 1.0; // 調節用の係数
denden 0:ab144c574081 603 float pwmL = - Omega * k * IndividualValueL;
denden 0:ab144c574081 604 float pwmR = - Omega * k * IndividualValueR;
denden 0:ab144c574081 605 float pwmB = - Omega * k * IndividualValueB;
denden 0:ab144c574081 606
denden 0:ab144c574081 607 /*--- 値を反映 ---*/
denden 0:ab144c574081 608 *pPwmL = pwmL;
denden 0:ab144c574081 609 *pPwmR = pwmR;
denden 0:ab144c574081 610 *pPwmB = pwmB;
denden 0:ab144c574081 611 }
denden 0:ab144c574081 612 /*--- モータの計算と通信 ---*/
denden 0:ab144c574081 613 inline void Motor(float direction, float speed)
denden 0:ab144c574081 614 {
denden 0:ab144c574081 615 uint8_t rx_data;
denden 0:ab144c574081 616
denden 0:ab144c574081 617 /*--- 計算に必要となる値 ---
denden 0:ab144c574081 618 * 方向 ... direction (deg)
denden 0:ab144c574081 619 * 速度 ... speed (m/s)
denden 0:ab144c574081 620 * 機体の回転数 ... Omega (rad/s)
denden 0:ab144c574081 621 * 機体の半径 ... R (m)
denden 0:ab144c574081 622 * 機体の基準に対する傾き ... Theta (rad)
denden 0:ab144c574081 623 * pwmL(左), pwmB(後), pwmR(右)
denden 0:ab144c574081 624 --*/
denden 0:ab144c574081 625
denden 0:ab144c574081 626 // 変数宣言
denden 0:ab144c574081 627 float pwmL, pwmB, pwmR;
denden 0:ab144c574081 628
denden 0:ab144c574081 629
denden 0:ab144c574081 630
denden 0:ab144c574081 631 // 停止コマンドのときの処理
denden 0:ab144c574081 632 if (direction == 1000.0) {
denden 0:ab144c574081 633 RegulationHeading(&pwmL, &pwmR, &pwmB);
denden 0:ab144c574081 634 }
denden 0:ab144c574081 635 // 普通のときの処理
denden 0:ab144c574081 636 else {
denden 0:ab144c574081 637 // Theta = Angle * Byte2Rad;
denden 0:ab144c574081 638 direction *= Deg2Rad;
denden 0:ab144c574081 639 float vx = cos(direction);
denden 0:ab144c574081 640 float vy = sin(direction);
denden 0:ab144c574081 641
denden 0:ab144c574081 642 PwmLRB1(&pwmL, &pwmR, &pwmB, vx, vy, speed);
denden 0:ab144c574081 643 // PwmLRB2(&pwmL, &pwmR, &pwmB, vx, vy, speed);
denden 0:ab144c574081 644 // PwmLRB3(&pwmL, &pwmR, &pwmB, vx, vy, speed);
denden 0:ab144c574081 645 }
denden 0:ab144c574081 646
denden 0:ab144c574081 647 msgF(0, "pwmL", pwmL, false);
denden 0:ab144c574081 648 msgF(0, "pwmR", pwmR, false);
denden 0:ab144c574081 649 msgF(0, "pwmB", pwmB, false);
denden 0:ab144c574081 650
denden 0:ab144c574081 651 // 回転方向を決定　と　マイナスだったらプラスに変える
denden 0:ab144c574081 652 uint8_t data0 = 0x3F; // 0011 1111
denden 0:ab144c574081 653 if (pwmL!=0.0 && pwmR!=0.0 && pwmB!=0.0) {
denden 0:ab144c574081 654 // 左モータ
denden 0:ab144c574081 655 if (pwmL>0) {
denden 0:ab144c574081 656 data0 &= 0x37; // 0011 0111
denden 0:ab144c574081 657 }
denden 0:ab144c574081 658 else {
denden 0:ab144c574081 659 data0 &= 0x3B; // 0011 1011
denden 0:ab144c574081 660 pwmL *= -1.0;
denden 0:ab144c574081 661 }
denden 0:ab144c574081 662 if (pwmL>1.0) pwmL=1.0;
denden 0:ab144c574081 663
denden 0:ab144c574081 664 // 右モータ
denden 0:ab144c574081 665 if (pwmR>0) {
denden 0:ab144c574081 666 data0 &= 0x3D; // 0011 1101
denden 0:ab144c574081 667 }
denden 0:ab144c574081 668 else {
denden 0:ab144c574081 669 data0 &= 0x3E; // 0011 1110
denden 0:ab144c574081 670 pwmR *= -1.0;
denden 0:ab144c574081 671 }
denden 0:ab144c574081 672 if (pwmR>1.0) pwmR=1.0;
denden 0:ab144c574081 673
denden 0:ab144c574081 674 // 後モータ
denden 0:ab144c574081 675 if (pwmB>0) {
denden 0:ab144c574081 676 data0 &= 0x1F; // 0001 1111
denden 0:ab144c574081 677 }
denden 0:ab144c574081 678 else {
denden 0:ab144c574081 679 data0 &= 0x2F; // 0010 1111
denden 0:ab144c574081 680 pwmB *= -1.0;
denden 0:ab144c574081 681 }
denden 0:ab144c574081 682 if (pwmB>1.0) pwmB=1.0;
denden 0:ab144c574081 683 }
denden 0:ab144c574081 684 else data0 = 0x00;
denden 0:ab144c574081 685
denden 0:ab144c574081 686 uint8_t dataL = (uint8_t)(pwmL * 255.0); // 左モータの回転数
denden 0:ab144c574081 687 uint8_t dataR = (uint8_t)(pwmR * 255.0); // 右モータの回転数
denden 0:ab144c574081 688 uint8_t dataB = (uint8_t)(pwmB * 255.0); // 後モータの回転数
denden 0:ab144c574081 689 if (dataL == 0x00) {
denden 0:ab144c574081 690 data0 &= 0x33; // 0011 0011
denden 0:ab144c574081 691 data0 |= 0x0C;
denden 0:ab144c574081 692 dataL++;
denden 0:ab144c574081 693 }
denden 0:ab144c574081 694 if (dataR == 0x00) {
denden 0:ab144c574081 695 data0 &= 0x3C; // 0011 1100
denden 0:ab144c574081 696 data0 |= 0x03;
denden 0:ab144c574081 697 dataR++;
denden 0:ab144c574081 698 }
denden 0:ab144c574081 699 if (dataB == 0x00) {
denden 0:ab144c574081 700 data0 &= 0x0F; // 0000 1111
denden 0:ab144c574081 701 data0 |= 0x30;
denden 0:ab144c574081 702 dataB++;
denden 0:ab144c574081 703 }
denden 0:ab144c574081 704
denden 0:ab144c574081 705 // データ確認用の値を埋め込む
denden 0:ab144c574081 706 dataL = dataL & 0xFC | 0x01;
denden 0:ab144c574081 707 dataR = dataR & 0xFC | 0x02;
denden 0:ab144c574081 708 dataB = dataB & 0xFC | 0x03;
denden 0:ab144c574081 709
denden 0:ab144c574081 710 // デバッグのため、送信データを表示
denden 0:ab144c574081 711 // pc.printf(" ");
denden 0:ab144c574081 712 msgX(0, "data0", data0, false);
denden 0:ab144c574081 713 msgX(0, "dataL", dataL, false);
denden 0:ab144c574081 714 msgX(0, "dataR", dataR, false);
denden 0:ab144c574081 715 msgX(0, "dataB", dataB, true);
denden 0:ab144c574081 716
denden 0:ab144c574081 717 // データを送信
denden 0:ab144c574081 718 int error = 0;
denden 0:ab144c574081 719 int count = 0;
denden 0:ab144c574081 720 while (1) {
denden 0:ab144c574081 721 // 通信開始の合図
denden 0:ab144c574081 722 cs_Motor=0; wait_us(t_SS_SCLK);
denden 0:ab144c574081 723 rx_data=spi.write((uint8_t)(error%4)); wait_us(t_SS_SCLK);
denden 0:ab144c574081 724 cs_Motor=1;
denden 0:ab144c574081 725 if (rx_data!=0xAA) {
denden 0:ab144c574081 726 errorMsgHex(0, "start", rx_data);
denden 0:ab144c574081 727 // errorMsgHex(0, "start_data", (uint8_t)(error%4));
denden 0:ab144c574081 728 }
denden 0:ab144c574081 729 wait_us(SPI_WAIT_US);
denden 0:ab144c574081 730 // 回転方向のデータ
denden 0:ab144c574081 731 do {
denden 0:ab144c574081 732 cs_Motor=0; wait_us(t_SS_SCLK);
denden 0:ab144c574081 733 rx_data=spi.write(data0); wait_us(t_SS_SCLK);
denden 0:ab144c574081 734 cs_Motor=1;
denden 0:ab144c574081 735 count++;
denden 0:ab144c574081 736 } while(rx_data != 0x00 && count < 5);
denden 0:ab144c574081 737 if (rx_data!=0x00) {
denden 0:ab144c574081 738 errorMsgHex(0, "data0", rx_data);
denden 0:ab144c574081 739 led_Motor = 0;
denden 0:ab144c574081 740 error++;
denden 0:ab144c574081 741 continue;
denden 0:ab144c574081 742 } else led_Motor = 1;
denden 0:ab144c574081 743 msgD(0, "count", count);
denden 0:ab144c574081 744 count = 0;
denden 0:ab144c574081 745 wait_us(SPI_WAIT_US);
denden 0:ab144c574081 746 // MotorLのデータ
denden 0:ab144c574081 747 do {
denden 0:ab144c574081 748 cs_Motor=0; wait_us(t_SS_SCLK);
denden 0:ab144c574081 749 rx_data=spi.write(dataL); wait_us(t_SS_SCLK);
denden 0:ab144c574081 750 cs_Motor=1;
denden 0:ab144c574081 751 count++;
denden 0:ab144c574081 752 } while(rx_data != data0 && count < 5);
denden 0:ab144c574081 753 if (rx_data!=data0) {
denden 0:ab144c574081 754 errorMsgHex(0, "dataL", rx_data);
denden 0:ab144c574081 755 led_Motor = 0;
denden 0:ab144c574081 756 error++;
denden 0:ab144c574081 757 continue;
denden 0:ab144c574081 758 } else led_Motor = 1;
denden 0:ab144c574081 759 msgD(0, "count", count);
denden 0:ab144c574081 760 count = 0;
denden 0:ab144c574081 761 wait_us(SPI_WAIT_US);
denden 0:ab144c574081 762 // MotorRのデータ
denden 0:ab144c574081 763 do {
denden 0:ab144c574081 764 cs_Motor=0; wait_us(t_SS_SCLK);
denden 0:ab144c574081 765 rx_data=spi.write(dataR); wait_us(t_SS_SCLK);
denden 0:ab144c574081 766 cs_Motor=1;
denden 0:ab144c574081 767 count++;
denden 0:ab144c574081 768 } while(rx_data != dataL && count < 5);
denden 0:ab144c574081 769 if (rx_data!=dataL) {
denden 0:ab144c574081 770 errorMsgHex(0, "dataR", rx_data);
denden 0:ab144c574081 771 led_Motor = 0;
denden 0:ab144c574081 772 error++;
denden 0:ab144c574081 773 continue;
denden 0:ab144c574081 774 } else led_Motor = 1;
denden 0:ab144c574081 775 msgD(0, "count", count);
denden 0:ab144c574081 776 count = 0;
denden 0:ab144c574081 777 wait_us(SPI_WAIT_US);
denden 0:ab144c574081 778 // MotorBのデータ
denden 0:ab144c574081 779 do {
denden 0:ab144c574081 780 cs_Motor=0; wait_us(t_SS_SCLK);
denden 0:ab144c574081 781 rx_data=spi.write(dataB); wait_us(t_SS_SCLK);
denden 0:ab144c574081 782 cs_Motor=1;
denden 0:ab144c574081 783 count++;
denden 0:ab144c574081 784 } while(rx_data != dataR && count < 5);
denden 0:ab144c574081 785 if (rx_data!=dataR) {
denden 0:ab144c574081 786 errorMsgHex(0, "dataB", rx_data);
denden 0:ab144c574081 787 led_Motor = 0;
denden 0:ab144c574081 788 error++;
denden 0:ab144c574081 789 continue;
denden 0:ab144c574081 790 } else led_Motor = 1;
denden 0:ab144c574081 791 msgD(0, "count", count);
denden 0:ab144c574081 792 count = 0;
denden 0:ab144c574081 793 wait_us(SPI_WAIT_US);
denden 0:ab144c574081 794
denden 0:ab144c574081 795 break;
denden 0:ab144c574081 796 }
denden 0:ab144c574081 797
denden 0:ab144c574081 798 return;
denden 0:ab144c574081 799 }
denden 0:ab144c574081 800
denden 0:ab144c574081 801 /*--- ボールの方向から、ロボットの進行方向を決める関数 ---*/
denden 0:ab144c574081 802 inline float MoveDirection(int iBallDirection)
denden 0:ab144c574081 803 {
denden 0:ab144c574081 804 /*---
denden 0:ab144c574081 805
denden 0:ab144c574081 806 - ボールの方向から、ロボットの進行方向を求める関数
denden 0:ab144c574081 807 - -180 < iBallDirection < +180
denden 0:ab144c574081 808
denden 0:ab144c574081 809 ---*/
denden 0:ab144c574081 810
denden 0:ab144c574081 811 if (iBallDirection == 1000.0) return 1000.0;
denden 0:ab144c574081 812
denden 0:ab144c574081 813 float fMoveDirection = 0.0;
denden 0:ab144c574081 814 if (-30<iBallDirection && iBallDirection<30) fMoveDirection = 0.0;
denden 0:ab144c574081 815 else if (iBallDirection > 0) fMoveDirection = iBallDirection+60.0;
denden 0:ab144c574081 816 else if (iBallDirection < 0) fMoveDirection = iBallDirection-60.0;
denden 0:ab144c574081 817 else fMoveDirection = 180.0;
denden 0:ab144c574081 818
denden 0:ab144c574081 819 // 角度を -180~+180 に修正
denden 0:ab144c574081 820 if (fMoveDirection > 180.0){
denden 0:ab144c574081 821 return fMoveDirection - 360.0;
denden 0:ab144c574081 822 }
denden 0:ab144c574081 823 if (fMoveDirection < -180.0){
denden 0:ab144c574081 824 return fMoveDirection + 360.0;
denden 0:ab144c574081 825 }
denden 0:ab144c574081 826
denden 0:ab144c574081 827 return fMoveDirection;
denden 0:ab144c574081 828 }
denden 0:ab144c574081 829
denden 0:ab144c574081 830
denden 0:ab144c574081 831 /*--- SPI通信確認 ---*/
denden 0:ab144c574081 832 bool spiChecking()
denden 0:ab144c574081 833 {
denden 0:ab144c574081 834 int spicheck = 0;
denden 0:ab144c574081 835 uint8_t data = 0;
denden 0:ab144c574081 836 uint8_t missData1 = 0;
denden 0:ab144c574081 837 uint8_t missData2 = 0;
denden 0:ab144c574081 838 uint8_t missData3 = 0;
denden 0:ab144c574081 839 uint8_t missData4 = 0;
denden 0:ab144c574081 840 pc.printf("\nChecking SPI...\n");
denden 0:ab144c574081 841
denden 0:ab144c574081 842 #ifdef PING_RLB_
denden 0:ab144c574081 843 cs_PingRLB = 0; missData1 = spi.write(0x50); cs_PingRLB = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 844 cs_PingRLB = 0; missData2 = spi.write(0x50); cs_PingRLB = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 845 cs_PingRLB = 0; data = spi.write(0x50); cs_PingRLB = 1;
denden 0:ab144c574081 846 if (data != 0xAA) {
denden 0:ab144c574081 847 pc.printf("##Bad## mbed <-> LPC1114 <-> Ping(RLB)\n");
denden 0:ab144c574081 848 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
denden 0:ab144c574081 849 spicheck++;
denden 0:ab144c574081 850 }
denden 0:ab144c574081 851 #endif
denden 0:ab144c574081 852
denden 0:ab144c574081 853 #ifdef MOTOR_
denden 0:ab144c574081 854 led_Motor = 0;
denden 0:ab144c574081 855 // ちゃんと値が入っているかをチェック
denden 0:ab144c574081 856 cs_Motor = 0; missData1 = spi.write(0x51); cs_Motor = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 857 cs_Motor = 0; missData2 = spi.write(0x51); cs_Motor = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 858 cs_Motor = 0; missData3 = spi.write(0x51); cs_Motor = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 859 cs_Motor = 0; missData4 = spi.write(0x51); cs_Motor = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 860 cs_Motor = 0; data = spi.write(0x01); cs_Motor = 1;
denden 0:ab144c574081 861 pc.printf(" ReturnValue1 = %x\n ReturnValue2 = %x\n ReturnValue3 = %x\n ReturnValue4 = %x\n ReturnValue5 = %x\n", missData1, missData2, missData3, missData4, data);
denden 0:ab144c574081 862 // SPI通信のチェック
denden 0:ab144c574081 863 cs_Motor = 0; missData1 = spi.write(0x50); cs_Motor = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 864 cs_Motor = 0; missData2 = spi.write(0x50); cs_Motor = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 865 cs_Motor = 0; data = spi.write(0x01); cs_Motor = 1;
denden 0:ab144c574081 866 if (data != 0xAA) {
denden 0:ab144c574081 867 pc.printf("##Bad## mbed <-> CY8C29466 <-> Motor\n");
denden 0:ab144c574081 868 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
denden 0:ab144c574081 869 spicheck++;
denden 0:ab144c574081 870 }
denden 0:ab144c574081 871 else led_Motor=1;
denden 0:ab144c574081 872 #endif
denden 0:ab144c574081 873
denden 0:ab144c574081 874 #ifdef KICK_DIV_
denden 0:ab144c574081 875 cs_KickerDribbler = 0; missData1 = spi.write(0x50); cs_KickerDribbler = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 876 cs_KickerDribbler = 0; missData2 = spi.write(0x50); cs_KickerDribbler = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 877 cs_KickerDribbler = 0;
denden 0:ab144c574081 878 data = spi.write(0x01);
denden 0:ab144c574081 879 cs_KickerDribbler = 1;
denden 0:ab144c574081 880 if (data != 0xAA) {
denden 0:ab144c574081 881 pc.printf("##Bad## mbed <-> CY8C29466 <-> Kicker&Dribbler\n");
denden 0:ab144c574081 882 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
denden 0:ab144c574081 883 spicheck++;
denden 0:ab144c574081 884 }
denden 0:ab144c574081 885 #endif
denden 0:ab144c574081 886
denden 0:ab144c574081 887 #ifdef IR_FR_
denden 0:ab144c574081 888 cs_IRFrDistance = 0; missData1 = spi.write(0x50); cs_IRFrDistance = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 889 cs_IRFrDistance = 0; missData2 = spi.write(0x50); cs_IRFrDistance = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 890 cs_IRFrDistance = 0; data = spi.write(0x01); cs_IRFrDistance = 1;
denden 0:ab144c574081 891 if (data != 0xAA) {
denden 0:ab144c574081 892 pc.printf("##Bad## mbed <-> CY8C24123A <-> IR_FR_distance\n");
denden 0:ab144c574081 893 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
denden 0:ab144c574081 894 spicheck++;
denden 0:ab144c574081 895 }
denden 0:ab144c574081 896 #endif
denden 0:ab144c574081 897
denden 0:ab144c574081 898 #ifdef IR_BR_
denden 0:ab144c574081 899 cs_IRBrDistance = 0; missData1 = spi.write(0x50); cs_IRBrDistance = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 900 cs_IRBrDistance = 0; missData2 = spi.write(0x50); cs_IRBrDistance = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 901 cs_IRBrDistance = 0; data = spi.write(0x01); cs_IRBrDistance = 1;
denden 0:ab144c574081 902 if (data != 0xAA) {
denden 0:ab144c574081 903 pc.printf("##Bad## mbed <-> CY8C24123A <-> IR_BR_distance\n");
denden 0:ab144c574081 904 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
denden 0:ab144c574081 905 spicheck++;
denden 0:ab144c574081 906 }
denden 0:ab144c574081 907 #endif
denden 0:ab144c574081 908
denden 0:ab144c574081 909 #ifdef IR_BL_
denden 0:ab144c574081 910 cs_IRBlDistance = 0; missData1 = spi.write(0x50); cs_IRBlDistance = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 911 cs_IRBlDistance = 0; missData2 = spi.write(0x50); cs_IRBlDistance = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 912 cs_IRBlDistance = 0; data = spi.write(0x01); cs_IRBlDistance = 1;
denden 0:ab144c574081 913 if (data != 0xAA) {
denden 0:ab144c574081 914 pc.printf("##Bad## mbed <-> CY8C24123A <-> IR_BL_distance\n");
denden 0:ab144c574081 915 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
denden 0:ab144c574081 916 spicheck++;
denden 0:ab144c574081 917 }
denden 0:ab144c574081 918 #endif
denden 0:ab144c574081 919
denden 0:ab144c574081 920 #ifdef IR_FL_
denden 0:ab144c574081 921 cs_IRFlDistance = 0; missData1 = spi.write(0x50); cs_IRFlDistance = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 922 cs_IRFlDistance = 0; missData2 = spi.write(0x50); cs_IRFlDistance = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 923 cs_IRFlDistance = 0; data = spi.write(0x01); cs_IRFlDistance = 1;
denden 0:ab144c574081 924 if (data != 0xAA) {
denden 0:ab144c574081 925 pc.printf("##Bad## mbed <-> CY8C24123A <-> IR_FL_distance\n");
denden 0:ab144c574081 926 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
denden 0:ab144c574081 927 spicheck++;
denden 0:ab144c574081 928 }
denden 0:ab144c574081 929 #endif
denden 0:ab144c574081 930
denden 0:ab144c574081 931 #ifdef IR__
denden 0:ab144c574081 932 led_IR = 0;
denden 0:ab144c574081 933 cs_IR = 0; missData1 = spi.write(0x50); cs_IR = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 934 cs_IR = 0; missData2 = spi.write(0x50); cs_IR = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 935 cs_IR = 0; data = spi.write(0x01); cs_IR = 1;
denden 0:ab144c574081 936 if (data != 0xAA) {
denden 0:ab144c574081 937 pc.printf("##Bad## mbed <-> CY8C29466 <-> IR_direction\n");
denden 0:ab144c574081 938 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
denden 0:ab144c574081 939 spicheck++;
denden 0:ab144c574081 940 }
denden 0:ab144c574081 941 else led_IR=1;
denden 0:ab144c574081 942 #endif
denden 0:ab144c574081 943
denden 0:ab144c574081 944 #ifdef PING_F_
denden 0:ab144c574081 945 cs_PingF = 0; missData1 = spi.write(0x50); cs_PingF = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 946 cs_PingF = 0; missData2 = spi.write(0x50); cs_PingF = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 947 cs_PingF = 0; data = spi.write(0x01); cs_PingF = 1;
denden 0:ab144c574081 948 if (data != 0xAA) {
denden 0:ab144c574081 949 pc.printf("##Bad## mbed <-> LPC1114 <-> Ping(F)\n");
denden 0:ab144c574081 950 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
denden 0:ab144c574081 951 spicheck++;
denden 0:ab144c574081 952 }
denden 0:ab144c574081 953 #endif
denden 0:ab144c574081 954
denden 0:ab144c574081 955 #ifdef IMU_
denden 0:ab144c574081 956 led_IMU = 0;
denden 0:ab144c574081 957 int count = 5;
denden 0:ab144c574081 958 do {
denden 0:ab144c574081 959 cs_IMU = 0; data = spi.write(0x50); cs_IMU = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 960 pc.printf("data : %x\n", data);
denden 0:ab144c574081 961 count--;
denden 0:ab144c574081 962 } while ((data==0xFF || data==0x00) && count);
denden 0:ab144c574081 963
denden 0:ab144c574081 964 if (count < 1) {
denden 0:ab144c574081 965 spicheck++;
denden 0:ab144c574081 966 pc.printf("##Bad## mbed <-> LPC1114 <-> LSM9DS0\n");
denden 0:ab144c574081 967 }
denden 0:ab144c574081 968 else led_IMU=1;
denden 0:ab144c574081 969 pc.printf(" IMU : %d\n", data);
denden 0:ab144c574081 970 #endif
denden 0:ab144c574081 971
denden 0:ab144c574081 972 #ifdef DEBUG_
denden 0:ab144c574081 973 cs_Debug = 0; missData1 = spi.write(0x50); cs_Debug = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 974 cs_Debug = 0; missData2 = spi.write(0x50); cs_Debug = 1; wait_us(SPI_WAIT_US);
denden 0:ab144c574081 975 cs_Debug = 0; data = spi.write(0x01); cs_Debug = 1;
denden 0:ab144c574081 976 if (data != 0xAA) {
denden 0:ab144c574081 977 pc.printf("##Bad## mbed <-> LPC1114_DEBUG\n");
denden 0:ab144c574081 978 pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
denden 0:ab144c574081 979 spicheck++;
denden 0:ab144c574081 980 }
denden 0:ab144c574081 981 #endif
denden 0:ab144c574081 982
denden 0:ab144c574081 983 // 最後にメッセージを表示
denden 0:ab144c574081 984 if (spicheck) {
denden 0:ab144c574081 985 pc.printf("waring : %d\n", spicheck);
denden 0:ab144c574081 986 led4 = 0;
denden 0:ab144c574081 987 // while (!pc.readable()); // PCから指示があるまで待つ
denden 0:ab144c574081 988 // pc.getc();
denden 0:ab144c574081 989 return 0;
denden 0:ab144c574081 990 wait(2);
denden 0:ab144c574081 991 }
denden 0:ab144c574081 992 else {
denden 0:ab144c574081 993 pc.printf("All SPIdevices complete!!\n");
denden 0:ab144c574081 994 led4 = 1;
denden 0:ab144c574081 995 return 1;
denden 0:ab144c574081 996 wait(1);
denden 0:ab144c574081 997 }
denden 0:ab144c574081 998 pc.printf("\n");
denden 0:ab144c574081 999 }
denden 0:ab144c574081 1000
denden 0:ab144c574081 1001
denden 0:ab144c574081 1002 /*--- PCとの通信用関数 ---*/
denden 0:ab144c574081 1003 inline void ControlCommand(uint8_t command)
denden 0:ab144c574081 1004 {
denden 0:ab144c574081 1005 /*--- コマンド一覧 ---
denden 0:ab144c574081 1006 * 必要なparameter
denden 0:ab144c574081 1007 0x01 PID_Kp PID制御の比例
denden 0:ab144c574081 1008 0x02 PID_Ki PID制御の積分
denden 0:ab144c574081 1009 0x03 PID_Kd PID制御の微分
denden 0:ab144c574081 1010 0x04 Robo_R 機体の半径
denden 0:ab144c574081 1011
denden 0:ab144c574081 1012 * 必要なcommand
denden 0:ab144c574081 1013 0x10 modeStop 停止コマンド
denden 0:ab144c574081 1014 0x11 modeBattle1 戦闘モード1
denden 0:ab144c574081 1015 0x12 modeBattle2 戦闘モード2
denden 0:ab144c574081 1016 0x13 modeDebug デバッグモード
denden 0:ab144c574081 1017 0x21 goFlont 前進
denden 0:ab144c574081 1018 0x22 goBack 後退
denden 0:ab144c574081 1019 0x23 goLeft 左
denden 0:ab144c574081 1020 0x24 goRight 右
denden 0:ab144c574081 1021 0x25 turnCW 時計回りに一回転
denden 0:ab144c574081 1022 0x26 turnCCW 反時計回りに一回転
denden 0:ab144c574081 1023 0x31 checkSensor センサーチェック
denden 0:ab144c574081 1024 0x31 checkIMU IMUからの情報を画面に表示
denden 0:ab144c574081 1025 0xAA Reset リセット（SDカードから、値を再読み込み）
denden 0:ab144c574081 1026 ---*/
denden 0:ab144c574081 1027 float wait_time = 1.0;
denden 0:ab144c574081 1028 switch (command)
denden 0:ab144c574081 1029 {
denden 0:ab144c574081 1030 case 0xAA:
denden 0:ab144c574081 1031 break;
denden 0:ab144c574081 1032 case 0x13: // modeDebug
denden 0:ab144c574081 1033 spiChecking();
denden 0:ab144c574081 1034 wait(3);
denden 0:ab144c574081 1035 break;
denden 0:ab144c574081 1036 case 0x21: // goFlont
denden 0:ab144c574081 1037 Motor(0.0, 0.3);
denden 0:ab144c574081 1038 wait(wait_time);
denden 0:ab144c574081 1039 break;
denden 0:ab144c574081 1040 case 0x22: // goBack
denden 0:ab144c574081 1041 Motor(180.0, 0.3);
denden 0:ab144c574081 1042 wait(wait_time);
denden 0:ab144c574081 1043 break;
denden 0:ab144c574081 1044 case 0x23: // goLeft
denden 0:ab144c574081 1045 Motor(-90.0, 0.3);
denden 0:ab144c574081 1046 wait(wait_time);
denden 0:ab144c574081 1047 break;
denden 0:ab144c574081 1048 case 0x24: // goRight
denden 0:ab144c574081 1049 Motor(90.0, 0.3);
denden 0:ab144c574081 1050 wait(wait_time);
denden 0:ab144c574081 1051 break;
denden 0:ab144c574081 1052 default:
denden 0:ab144c574081 1053 break;
denden 0:ab144c574081 1054 }
denden 0:ab144c574081 1055 }
denden 0:ab144c574081 1056
denden 0:ab144c574081 1057
denden 0:ab144c574081 1058 #ifdef XBEE
denden 0:ab144c574081 1059 /*--- 送信用関数 ---*/
denden 0:ab144c574081 1060 void xbeeTx(uint8_t *address, uint8_t data) {
denden 0:ab144c574081 1061 int i=0;
denden 0:ab144c574081 1062
denden 0:ab144c574081 1063 xbee.putc(0x7E);
denden 0:ab144c574081 1064 xbee.putc(0x00); // フレーム長
denden 0:ab144c574081 1065 xbee.putc(0x0F);
denden 0:ab144c574081 1066 xbee.putc(0x10); // フレームタイプ
denden 0:ab144c574081 1067 xbee.putc(0x01); // フレームID
denden 0:ab144c574081 1068 for (int i=0; i<8; i++)
denden 0:ab144c574081 1069 xbee.putc(address[i]); // 62bit宛先アドレス
denden 0:ab144c574081 1070 xbee.putc(0xFF); // 16bit宛先ネットワークアドレス
denden 0:ab144c574081 1071 xbee.putc(0xFE);
denden 0:ab144c574081 1072 xbee.putc(0x00); // ブロードキャスト半径
denden 0:ab144c574081 1073 xbee.putc(0x00); // オプション
denden 0:ab144c574081 1074 xbee.putc(data); // RFデータ
denden 0:ab144c574081 1075 // チェックサムの計算
denden 0:ab144c574081 1076 uint8_t checksum = 0xFF - (0x10+0x01+0xFF+0xFE+data);
denden 0:ab144c574081 1077 for (i=0; i<8; i++) checksum -= address[i];
denden 0:ab144c574081 1078 xbee.putc(checksum); // チェックサム
denden 0:ab144c574081 1079
denden 0:ab144c574081 1080 // 送信ステータスの確認
denden 0:ab144c574081 1081 if (xbee.readable()) {
denden 0:ab144c574081 1082 if (xbee.getc() == 0x7E) {
denden 0:ab144c574081 1083 for (i=0; i<8; i++) xbee.getc();
denden 0:ab144c574081 1084 if (xbee.getc() == 0x00)
denden 0:ab144c574081 1085 // pc.printf("Send success!\n");
denden 0:ab144c574081 1086 else
denden 0:ab144c574081 1087 // pc.printf("Send missed.\n");
denden 0:ab144c574081 1088 for (i=9; i<10; i++) xbee.getc();
denden 0:ab144c574081 1089 }
denden 0:ab144c574081 1090 }
denden 0:ab144c574081 1091 }
denden 0:ab144c574081 1092 /*--- 受信用関数 ---*/
denden 0:ab144c574081 1093 xbeeAPI xbeeRx() {
denden 0:ab144c574081 1094 xbeeAPI returnApi; // 返り値用の変数
denden 0:ab144c574081 1095 returnApi.reset();
denden 0:ab144c574081 1096 if (xbee.readable() > 0) {
denden 0:ab144c574081 1097 // pc.printf("readable\n");
denden 0:ab144c574081 1098 if (xbee.getc() == 0x7E) {
denden 0:ab144c574081 1099 // pc.printf("startBit\n");
denden 0:ab144c574081 1100 int i=0;
denden 0:ab144c574081 1101 uint8_t buffer[18];
denden 0:ab144c574081 1102
denden 0:ab144c574081 1103 buffer[0] = 0x7E;
denden 0:ab144c574081 1104 for (i=1; i<18; i++) {
denden 0:ab144c574081 1105 buffer[i] = xbee.getc();
denden 0:ab144c574081 1106 }
denden 0:ab144c574081 1107 // 送信元アドレスを抽出
denden 0:ab144c574081 1108 for (i=0; i<8; i++) {
denden 0:ab144c574081 1109 returnApi.address[i] = buffer[i+4];
denden 0:ab144c574081 1110 }
denden 0:ab144c574081 1111 // 受信データを抽出
denden 0:ab144c574081 1112 returnApi.direction = buffer[15];
denden 0:ab144c574081 1113 returnApi.turning = buffer[16];
denden 0:ab144c574081 1114
denden 0:ab144c574081 1115 // データをPCに表示
denden 0:ab144c574081 1116 // for (i=0; i<8; i++) pc.printf("%x", returnApi.address[i]);
denden 0:ab144c574081 1117 // pc.printf("\n");
denden 0:ab144c574081 1118 // pc.printf("%x\n", returnApi.direction);
denden 0:ab144c574081 1119 // pc.printf("%x\n", returnApi.turning);
denden 0:ab144c574081 1120 }
denden 0:ab144c574081 1121 }
denden 0:ab144c574081 1122 return returnApi;
denden 0:ab144c574081 1123 }
denden 0:ab144c574081 1124 #endif
denden 0:ab144c574081 1125
denden 0:ab144c574081 1126 /*--- serialの通信チェック関数 ---*/
denden 0:ab144c574081 1127 inline void serialChecking()
denden 0:ab144c574081 1128 {
denden 0:ab144c574081 1129 if ( pc.readable() ) {
denden 0:ab144c574081 1130 uint8_t command = pc.getc();
denden 0:ab144c574081 1131 printf("command : %x\n", pc.getc());
denden 0:ab144c574081 1132 ControlCommand(command);
denden 0:ab144c574081 1133 }
denden 0:ab144c574081 1134 }
denden 0:ab144c574081 1135
denden 0:ab144c574081 1136
denden 0:ab144c574081 1137
denden 0:ab144c574081 1138
denden 0:ab144c574081 1139 int main()
denden 0:ab144c574081 1140 {
denden 0:ab144c574081 1141 /*--- 変数宣言 ---*/
denden 0:ab144c574081 1142 int direction = 0; // ボールのある方向
denden 0:ab144c574081 1143 float speed = 0.0; // ロボットの速度
denden 0:ab144c574081 1144
denden 0:ab144c574081 1145 /*--- 初期処理 ---*/
denden 0:ab144c574081 1146 // 音
denden 0:ab144c574081 1147 float mm[]={mC,mD,mE,mF,mG,mA,mB,mC*2}; // ドレミファソラシ
denden 0:ab144c574081 1148 sound.period(1.0/mm[0]);
denden 0:ab144c574081 1149 sound = 0.03;
denden 0:ab144c574081 1150 // Serialの設定
denden 0:ab144c574081 1151 pc.baud(115200);
denden 0:ab144c574081 1152 // SPIの初期設定
denden 0:ab144c574081 1153 spi.format(8, 3); // DataFlameのビット数(4~16)
denden 0:ab144c574081 1154 spi.frequency(1000000); // クロック周波数(デフォルト:1MHz)
denden 0:ab144c574081 1155 // ラインセンサに許可信号を
denden 0:ab144c574081 1156 linePermission=1;
denden 0:ab144c574081 1157 wait(1); // 各モジュールのセットアップが完了するまで待つ
denden 0:ab144c574081 1158
denden 0:ab144c574081 1159 // PIDライブラリの初期設定
denden 0:ab144c574081 1160 pidController.setInputLimits(-128.0, 127.0);
denden 0:ab144c574081 1161 pidController.setOutputLimits(-1.0, 1.0);
denden 0:ab144c574081 1162 pidController.setBias(0.0);
denden 0:ab144c574081 1163 pidController.setMode(AUTO_MODE);
denden 0:ab144c574081 1164 pidController.setSetPoint(0.0);
denden 0:ab144c574081 1165 // pidupdata.attach(&ImuRead, RATE);
denden 0:ab144c574081 1166
denden 0:ab144c574081 1167 // lineの設定
denden 0:ab144c574081 1168 // lineLeft.rise(&lineMove);
denden 0:ab144c574081 1169 // lineRight.rise(&lineMove);
denden 0:ab144c574081 1170
denden 0:ab144c574081 1171 // SPIの通信確認
denden 0:ab144c574081 1172 while ( !spiChecking() );
denden 0:ab144c574081 1173
denden 0:ab144c574081 1174
denden 0:ab144c574081 1175 /*--- スタートの合図を待つ ---*/
denden 0:ab144c574081 1176 kickerOuput = 1;
denden 0:ab144c574081 1177 // SWが押されるまで待つ
denden 0:ab144c574081 1178 sound.period(1.0/mm[1]);
denden 0:ab144c574081 1179 sound = 0.06;
denden 0:ab144c574081 1180 while(!SWcheck());
denden 0:ab144c574081 1181 // 音を止める
denden 0:ab144c574081 1182 sound = 0.00;
denden 0:ab144c574081 1183
denden 0:ab144c574081 1184
denden 0:ab144c574081 1185 /*--- メイン処理 ---*/
denden 0:ab144c574081 1186 while(true) {
denden 0:ab144c574081 1187
denden 0:ab144c574081 1188 #ifdef PROG_TIME
denden 0:ab144c574081 1189 t.start();
denden 0:ab144c574081 1190 #endif
denden 0:ab144c574081 1191
denden 0:ab144c574081 1192 /*--- pcとの通信があったかをCheck ---*/
denden 0:ab144c574081 1193 // serialChecking();
denden 0:ab144c574081 1194 /*--- ストップボタンをCheck ---*/
denden 0:ab144c574081 1195 if (SWcheck()) {
denden 0:ab144c574081 1196 Motor(0.0, 0.0);
denden 0:ab144c574081 1197 sound.period(1.0/mm[1]);
denden 0:ab144c574081 1198 sound = 0.06;
denden 0:ab144c574081 1199 while(!SWcheck());
denden 0:ab144c574081 1200 sound = 0.00;
denden 0:ab144c574081 1201 }
denden 0:ab144c574081 1202 lineCheck();
denden 0:ab144c574081 1203
denden 0:ab144c574081 1204 // PingRead();
denden 0:ab144c574081 1205
denden 0:ab144c574081 1206 /*--- 現在の方角を確認 ---*/
denden 0:ab144c574081 1207 // Kicker使用時は、IMUの値は参考にならないので無視する
denden 0:ab144c574081 1208 // if (kickerInput) {
denden 0:ab144c574081 1209 // kickerOuput = 1;
denden 0:ab144c574081 1210 // wait(0.8);
denden 0:ab144c574081 1211 // kickerOuput = 0;
denden 0:ab144c574081 1212 // } else {
denden 0:ab144c574081 1213 // ImuRead();
denden 0:ab144c574081 1214 // pidController.setProcessValue(Angle);
denden 0:ab144c574081 1215 // }
denden 0:ab144c574081 1216 ImuRead();
denden 0:ab144c574081 1217 pidController.setProcessValue(Angle);
denden 0:ab144c574081 1218 lineCheck();
denden 0:ab144c574081 1219
denden 0:ab144c574081 1220 /*--- ロボットから見たボールの方向を確認 ---*/
denden 0:ab144c574081 1221 direction = IRRead();
denden 0:ab144c574081 1222 lineCheck();
denden 0:ab144c574081 1223
denden 0:ab144c574081 1224 /*--- ボールの方向の絶対値を算出 ---*/
denden 0:ab144c574081 1225 if (direction != 1000.0) {
denden 0:ab144c574081 1226 direction += Angle;
denden 0:ab144c574081 1227 if (direction > 180.0){
denden 0:ab144c574081 1228 direction -= 360.0;
denden 0:ab144c574081 1229 }
denden 0:ab144c574081 1230 else if (direction < -180.0){
denden 0:ab144c574081 1231 direction += 360.0;
denden 0:ab144c574081 1232 }
denden 0:ab144c574081 1233 }
denden 0:ab144c574081 1234 lineCheck();
denden 0:ab144c574081 1235
denden 0:ab144c574081 1236 /*--- ロボットの進行方向を決定 ---*/
denden 0:ab144c574081 1237 moveDirection = MoveDirection(direction);
denden 0:ab144c574081 1238 // pc.printf("%f\n", moveDirection);
denden 0:ab144c574081 1239 lineCheck();
denden 0:ab144c574081 1240
denden 0:ab144c574081 1241 /*--- 修正のための角速度を算出 ---*/
denden 0:ab144c574081 1242 // GetOmega();
denden 0:ab144c574081 1243 Omega = pidController.compute();
denden 0:ab144c574081 1244 if (-5<Angle && Angle<5) {
denden 0:ab144c574081 1245 Omega = 0.0;
denden 0:ab144c574081 1246 }
denden 0:ab144c574081 1247 lineCheck();
denden 0:ab144c574081 1248
denden 0:ab144c574081 1249 /*--- 実際にロボットを動かす ---*/
denden 0:ab144c574081 1250 // moveDirection = 0.0;
denden 0:ab144c574081 1251 // moveDirection = 1000.0;
denden 0:ab144c574081 1252 Motor(moveDirection, 0.4);
denden 0:ab144c574081 1253 lineCheck();
denden 0:ab144c574081 1254
denden 0:ab144c574081 1255 Direction = moveDirection;
denden 0:ab144c574081 1256
denden 0:ab144c574081 1257
denden 0:ab144c574081 1258 #ifdef PROG_TIME
denden 0:ab144c574081 1259 t.stop();
denden 0:ab144c574081 1260 pc.printf("%f\n", t.read());
denden 0:ab144c574081 1261 t.reset();
denden 0:ab144c574081 1262 #endif
denden 0:ab144c574081 1263
denden 0:ab144c574081 1264 LoopCount++;
denden 0:ab144c574081 1265 }
denden 0:ab144c574081 1266 }
denden 0:ab144c574081 1267
denden 0:ab144c574081 1268
denden 0:ab144c574081 1269
denden 0:ab144c574081 1270
denden 0:ab144c574081 1271
denden 0:ab144c574081 1272
denden 0:ab144c574081 1273
denden 0:ab144c574081 1274
denden 0:ab144c574081 1275
denden 0:ab144c574081 1276
denden 0:ab144c574081 1277
denden 0:ab144c574081 1278