Naoya Muramatsu / Mbed 2 deprecated 4E_Robot_Blue_2014

6輪ロボットの制御プログラム。 6つのオムニホイール、2つの緑レーザー、4つの受光センサを持っている。 xbeeを使って、コントローラと通信を行う。 基本的にRedと同じ。

Dependencies:   mbed

main.cpp@0:2df3dada985c, 2016-05-04 (annotated)

Committer:
denden
Date:
Wed May 04 12:50:58 2016 +0000
Revision:
0:2df3dada985c
publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
denden 0:2df3dada985c 1 // Robot
denden 0:2df3dada985c 2 // Blue
denden 0:2df3dada985c 3
denden 0:2df3dada985c 4 #include "mbed.h"
denden 0:2df3dada985c 5
denden 0:2df3dada985c 6 /*
denden 0:2df3dada985c 7 --- XBeeアドレス ---
denden 0:2df3dada985c 8 平面アンテナ
denden 0:2df3dada985c 9 Red
denden 0:2df3dada985c 10 Zigbee Router API : 0013A200 407ABE67
denden 0:2df3dada985c 11 Zigbee Router API : 0013A200 407ABCBB
denden 0:2df3dada985c 12 Blue
denden 0:2df3dada985c 13 Zigbee Coordinator API : 0013A200 407C04BD
denden 0:2df3dada985c 14 Zigbee Router API : 0013A200 407C04F9
denden 0:2df3dada985c 15 */
denden 0:2df3dada985c 16
denden 0:2df3dada985c 17 Serial xbee(p28, p27);
denden 0:2df3dada985c 18 Serial pc(USBTX, USBRX);
denden 0:2df3dada985c 19
denden 0:2df3dada985c 20 InterruptIn raserSensor1(p9);
denden 0:2df3dada985c 21 InterruptIn raserSensor2(p10);
denden 0:2df3dada985c 22 InterruptIn raserSensor3(p29);
denden 0:2df3dada985c 23
denden 0:2df3dada985c 24 DigitalOut FR1(p11);
denden 0:2df3dada985c 25 DigitalOut FR2(p12);
denden 0:2df3dada985c 26 DigitalOut FL1(p13);
denden 0:2df3dada985c 27 DigitalOut FL2(p14);
denden 0:2df3dada985c 28 DigitalOut BR1(p15);
denden 0:2df3dada985c 29 DigitalOut BR2(p16);
denden 0:2df3dada985c 30 DigitalOut BL1(p17);
denden 0:2df3dada985c 31 DigitalOut BL2(p18);
denden 0:2df3dada985c 32 DigitalOut CR1(p5);
denden 0:2df3dada985c 33 DigitalOut CR2(p6);
denden 0:2df3dada985c 34 DigitalOut CL1(p7);
denden 0:2df3dada985c 35 DigitalOut CL2(p8);
denden 0:2df3dada985c 36
denden 0:2df3dada985c 37 PwmOut FR(p25);
denden 0:2df3dada985c 38 PwmOut FL(p22);
denden 0:2df3dada985c 39 PwmOut BR(p24);
denden 0:2df3dada985c 40 PwmOut BL(p23);
denden 0:2df3dada985c 41 PwmOut CR(p26);
denden 0:2df3dada985c 42 PwmOut CL(p21);
denden 0:2df3dada985c 43
denden 0:2df3dada985c 44 DigitalOut Raser1(p20);
denden 0:2df3dada985c 45 DigitalOut Raser2(p30);
denden 0:2df3dada985c 46
denden 0:2df3dada985c 47 DigitalOut led1(LED1);
denden 0:2df3dada985c 48 DigitalOut led2(LED2);
denden 0:2df3dada985c 49 DigitalOut led3(LED3);
denden 0:2df3dada985c 50 DigitalOut led4(LED4);
denden 0:2df3dada985c 51
denden 0:2df3dada985c 52
denden 0:2df3dada985c 53 /*--- グローバル変数宣言---*/
denden 0:2df3dada985c 54 // uint8_t MyRemoteAddress[] = {0x00,0x13,0xa2,0x00,0x40,0x7a,0xbe,0x67}; // 見方コントローラのアドレス
denden 0:2df3dada985c 55
denden 0:2df3dada985c 56 uint8_t MyRemoteAddress[] = {0x00,0x13,0xa2,0x00,0x40,0x7c,0x04,0xbd}; // 見方コントローラのアドレス
denden 0:2df3dada985c 57
denden 0:2df3dada985c 58
denden 0:2df3dada985c 59
denden 0:2df3dada985c 60
denden 0:2df3dada985c 61 /*--- モーター制御用関数 ---*/
denden 0:2df3dada985c 62 void FRmove (float vec) {FR = abs(vec); FR1 = (unsigned char)(abs)(vec-0.9); FR2 = (unsigned char)(abs)(vec+0.9);}
denden 0:2df3dada985c 63 void FLmove (float vec) {FL = abs(vec); FL1 = (unsigned char)(abs)(vec+0.9); FL2 = (unsigned char)(abs)(vec-0.9);}
denden 0:2df3dada985c 64 void CRmove (float vec) {CR = abs(vec); CR1 = (unsigned char)(abs)(vec-0.9); CR2 = (unsigned char)(abs)(vec+0.9);}
denden 0:2df3dada985c 65 void CLmove (float vec) {CL = abs(vec); CL1 = (unsigned char)(abs)(vec-0.9); CL2 = (unsigned char)(abs)(vec+0.9);}
denden 0:2df3dada985c 66 void BRmove (float vec) {BR = abs(vec); BR1 = (unsigned char)(abs)(vec+0.9); BR2 = (unsigned char)(abs)(vec-0.9);}
denden 0:2df3dada985c 67 void BLmove (float vec) {BL = abs(vec); BL1 = (unsigned char)(abs)(vec+0.9); BL2 = (unsigned char)(abs)(vec-0.9);}
denden 0:2df3dada985c 68
denden 0:2df3dada985c 69
denden 0:2df3dada985c 70 /*--- 進行方向制御用変数 ---*/
denden 0:2df3dada985c 71 void FFmove() {
denden 0:2df3dada985c 72 FLmove(+1.00); FRmove(+1.00);
denden 0:2df3dada985c 73 CLmove(+0.71); CRmove(+0.71);
denden 0:2df3dada985c 74 BLmove(-1.00); BRmove(-1.00);
denden 0:2df3dada985c 75 }
denden 0:2df3dada985c 76 void FRmove() {
denden 0:2df3dada985c 77 FLmove(-1.00); FRmove(+1.00);
denden 0:2df3dada985c 78 CLmove(+1.00); CRmove(+1.00);
denden 0:2df3dada985c 79 BLmove(-1.00); BRmove(+1.00);
denden 0:2df3dada985c 80 }
denden 0:2df3dada985c 81 void FLmove() {
denden 0:2df3dada985c 82 FLmove(+1.00); FRmove(-1.00);
denden 0:2df3dada985c 83 CLmove(+1.00); CRmove(+1.00);
denden 0:2df3dada985c 84 BLmove(+1.00); BRmove(-1.00);
denden 0:2df3dada985c 85 }
denden 0:2df3dada985c 86 void Lmove() {
denden 0:2df3dada985c 87 FLmove(+1.00); FRmove(-1.00);
denden 0:2df3dada985c 88 CLmove(-0.00); CRmove(-0.00);
denden 0:2df3dada985c 89 BLmove(+1.00); BRmove(-1.00);
denden 0:2df3dada985c 90 }
denden 0:2df3dada985c 91 void Rmove() {
denden 0:2df3dada985c 92 FLmove(-1.00); FRmove(+1.00);
denden 0:2df3dada985c 93 CLmove(-0.00); CRmove(-0.00);
denden 0:2df3dada985c 94 BLmove(-1.00); BRmove(+1.00);
denden 0:2df3dada985c 95 }
denden 0:2df3dada985c 96 void BBmove() {
denden 0:2df3dada985c 97 FLmove(-1.00); FRmove(-1.00);
denden 0:2df3dada985c 98 CLmove(-0.71); CRmove(-0.71);
denden 0:2df3dada985c 99 BLmove(+1.00); BRmove(+1.00);
denden 0:2df3dada985c 100 }
denden 0:2df3dada985c 101 void BRmove() {
denden 0:2df3dada985c 102 FLmove(-1.00); FRmove(+1.00);
denden 0:2df3dada985c 103 CLmove(-1.00); CRmove(-1.00);
denden 0:2df3dada985c 104 BLmove(-1.00); BRmove(+1.00);
denden 0:2df3dada985c 105 }
denden 0:2df3dada985c 106 void BLmove() {
denden 0:2df3dada985c 107 FLmove(+0.70); FRmove(-0.70);
denden 0:2df3dada985c 108 CLmove(-1.00); CRmove(-1.00);
denden 0:2df3dada985c 109 BLmove(+1.00); BRmove(-1.00);
denden 0:2df3dada985c 110 }
denden 0:2df3dada985c 111
denden 0:2df3dada985c 112 void Lturn() {
denden 0:2df3dada985c 113 FLmove(-0.70); FRmove(+0.70);
denden 0:2df3dada985c 114 CLmove(+0.00); CRmove(+0.00);
denden 0:2df3dada985c 115 BLmove(-1.00); BRmove(+1.00);
denden 0:2df3dada985c 116 }
denden 0:2df3dada985c 117 void Rturn() {
denden 0:2df3dada985c 118 FLmove(+0.70); FRmove(-0.70);
denden 0:2df3dada985c 119 CLmove(+0.00); CRmove(+0.00);
denden 0:2df3dada985c 120 BLmove(+1.00); BRmove(-1.00);
denden 0:2df3dada985c 121 }
denden 0:2df3dada985c 122
denden 0:2df3dada985c 123
denden 0:2df3dada985c 124 void Stop() {
denden 0:2df3dada985c 125 FR1 = 1; FR2 = 1;
denden 0:2df3dada985c 126 FL1 = 1; FL2 = 1;
denden 0:2df3dada985c 127 CR1 = 1; CR2 = 1;
denden 0:2df3dada985c 128 CL1 = 1; CL2 = 1;
denden 0:2df3dada985c 129 BR1 = 1; BR2 = 1;
denden 0:2df3dada985c 130 BL1 = 1; BL2 = 1;
denden 0:2df3dada985c 131 }
denden 0:2df3dada985c 132
denden 0:2df3dada985c 133
denden 0:2df3dada985c 134 /*--- データフレーム用クラス ---*/
denden 0:2df3dada985c 135 class xbeeAPI {
denden 0:2df3dada985c 136 public:
denden 0:2df3dada985c 137 uint8_t address[8];
denden 0:2df3dada985c 138 uint8_t direction;
denden 0:2df3dada985c 139 uint8_t turning;
denden 0:2df3dada985c 140 void reset() {
denden 0:2df3dada985c 141 for (int i=0; i<8; i++) address[i]=0;
denden 0:2df3dada985c 142 direction = 0;
denden 0:2df3dada985c 143 turning = 0;
denden 0:2df3dada985c 144 }
denden 0:2df3dada985c 145 };
denden 0:2df3dada985c 146
denden 0:2df3dada985c 147 /*--- 送信用関数 ---*/
denden 0:2df3dada985c 148 void xbeeTx(uint8_t *address, uint8_t data) {
denden 0:2df3dada985c 149 int i=0;
denden 0:2df3dada985c 150
denden 0:2df3dada985c 151 xbee.putc(0x7E);
denden 0:2df3dada985c 152 xbee.putc(0x00); // フレーム長
denden 0:2df3dada985c 153 xbee.putc(0x0F);
denden 0:2df3dada985c 154 xbee.putc(0x10); // フレームタイプ
denden 0:2df3dada985c 155 xbee.putc(0x01); // フレームID
denden 0:2df3dada985c 156 for (int i=0; i<8; i++)
denden 0:2df3dada985c 157 xbee.putc(address[i]); // 62bit宛先アドレス
denden 0:2df3dada985c 158 xbee.putc(0xFF); // 16bit宛先ネットワークアドレス
denden 0:2df3dada985c 159 xbee.putc(0xFE);
denden 0:2df3dada985c 160 xbee.putc(0x00); // ブロードキャスト半径
denden 0:2df3dada985c 161 xbee.putc(0x00); // オプション
denden 0:2df3dada985c 162 xbee.putc(data); // RFデータ
denden 0:2df3dada985c 163 // チェックサムの計算
denden 0:2df3dada985c 164 uint8_t checksum = 0xFF - (0x10+0x01+0xFF+0xFE+data);
denden 0:2df3dada985c 165 for (i=0; i<8; i++) checksum -= address[i];
denden 0:2df3dada985c 166 xbee.putc(checksum); // チェックサム
denden 0:2df3dada985c 167
denden 0:2df3dada985c 168 // 送信ステータスの確認
denden 0:2df3dada985c 169 if (xbee.readable()) {
denden 0:2df3dada985c 170 if (xbee.getc() == 0x7E) {
denden 0:2df3dada985c 171 for (i=0; i<8; i++) xbee.getc();
denden 0:2df3dada985c 172 if (xbee.getc() == 0x00)
denden 0:2df3dada985c 173 pc.printf("Send success!\n");
denden 0:2df3dada985c 174 else
denden 0:2df3dada985c 175 pc.printf("Send missed.\n");
denden 0:2df3dada985c 176 for (i=9; i<10; i++) xbee.getc();
denden 0:2df3dada985c 177 }
denden 0:2df3dada985c 178 }
denden 0:2df3dada985c 179 }
denden 0:2df3dada985c 180
denden 0:2df3dada985c 181
denden 0:2df3dada985c 182 /*--- 受信用関数 ---*/
denden 0:2df3dada985c 183 xbeeAPI xbeeRx() {
denden 0:2df3dada985c 184 xbeeAPI returnApi; // 返り値用の変数
denden 0:2df3dada985c 185 returnApi.reset();
denden 0:2df3dada985c 186 if (xbee.readable() > 0) {
denden 0:2df3dada985c 187 pc.printf("readable\n");
denden 0:2df3dada985c 188 if (xbee.getc() == 0x7E) {
denden 0:2df3dada985c 189 pc.printf("startBit\n");
denden 0:2df3dada985c 190 int i=0;
denden 0:2df3dada985c 191 uint8_t buffer[18];
denden 0:2df3dada985c 192
denden 0:2df3dada985c 193 buffer[0] = 0x7E;
denden 0:2df3dada985c 194 for (i=1; i<18; i++) {
denden 0:2df3dada985c 195 buffer[i] = xbee.getc();
denden 0:2df3dada985c 196 }
denden 0:2df3dada985c 197 // 送信元アドレスを抽出
denden 0:2df3dada985c 198 for (i=0; i<8; i++) {
denden 0:2df3dada985c 199 returnApi.address[i] = buffer[i+4];
denden 0:2df3dada985c 200 }
denden 0:2df3dada985c 201 // 受信データを抽出
denden 0:2df3dada985c 202 returnApi.direction = buffer[15];
denden 0:2df3dada985c 203 returnApi.turning = buffer[16];
denden 0:2df3dada985c 204
denden 0:2df3dada985c 205 // データをPCに表示
denden 0:2df3dada985c 206 for (i=0; i<8; i++) pc.printf("%x", returnApi.address[i]);
denden 0:2df3dada985c 207 pc.printf("\n");
denden 0:2df3dada985c 208 pc.printf("%x\n", returnApi.direction);
denden 0:2df3dada985c 209 pc.printf("%x\n", returnApi.turning);
denden 0:2df3dada985c 210 }
denden 0:2df3dada985c 211 }
denden 0:2df3dada985c 212 return returnApi;
denden 0:2df3dada985c 213 }
denden 0:2df3dada985c 214
denden 0:2df3dada985c 215
denden 0:2df3dada985c 216 /*--- レーザー発射 ---*/
denden 0:2df3dada985c 217 void Raser() {
denden 0:2df3dada985c 218 Raser1 = 1;
denden 0:2df3dada985c 219 Raser2 = 1;
denden 0:2df3dada985c 220 wait(1);
denden 0:2df3dada985c 221 Raser1 = 0;
denden 0:2df3dada985c 222 Raser2 = 0;
denden 0:2df3dada985c 223 }
denden 0:2df3dada985c 224
denden 0:2df3dada985c 225
denden 0:2df3dada985c 226 /*--- レーザー被弾時割り込み ---*/
denden 0:2df3dada985c 227 // 注）タクトスイッチに、チャタリング防止のためのコンデンサフィルタを付ける
denden 0:2df3dada985c 228 void damage() {
denden 0:2df3dada985c 229 // 見方コントローラに連絡
denden 0:2df3dada985c 230 led1 = 1;
denden 0:2df3dada985c 231 xbeeTx(MyRemoteAddress, 'D');
denden 0:2df3dada985c 232 wait(1);
denden 0:2df3dada985c 233 led1 = 0;
denden 0:2df3dada985c 234 }
denden 0:2df3dada985c 235
denden 0:2df3dada985c 236
denden 0:2df3dada985c 237
denden 0:2df3dada985c 238
denden 0:2df3dada985c 239
denden 0:2df3dada985c 240
denden 0:2df3dada985c 241 int main() {
denden 0:2df3dada985c 242
denden 0:2df3dada985c 243 /*--- 変数宣言 ---*/
denden 0:2df3dada985c 244 int i, j, k;
denden 0:2df3dada985c 245 xbeeAPI rxData; // コントローラからのデータ受信用変数
denden 0:2df3dada985c 246
denden 0:2df3dada985c 247 /*--- 初期設定 ---*/
denden 0:2df3dada985c 248 // ビットレートの設定
denden 0:2df3dada985c 249 xbee.baud(9600);
denden 0:2df3dada985c 250 pc.baud(9600);
denden 0:2df3dada985c 251 // 割り込みの設定
denden 0:2df3dada985c 252 raserSensor1.rise(&damage);
denden 0:2df3dada985c 253 raserSensor2.rise(&damage);
denden 0:2df3dada985c 254 raserSensor3.rise(&damage);
denden 0:2df3dada985c 255 // 初期値の設定
denden 0:2df3dada985c 256 Raser1=0; Raser2=0;
denden 0:2df3dada985c 257 led1=0; led2=0; led3=0; led4=0;
denden 0:2df3dada985c 258
denden 0:2df3dada985c 259
denden 0:2df3dada985c 260
denden 0:2df3dada985c 261 /*--- 繰り返し処理 ---*/
denden 0:2df3dada985c 262 while(1) {
denden 0:2df3dada985c 263 // 受信
denden 0:2df3dada985c 264 rxData = xbeeRx();
denden 0:2df3dada985c 265
denden 0:2df3dada985c 266 // チェック
denden 0:2df3dada985c 267 if (rxData.address[7] != MyRemoteAddress[7]) continue;
denden 0:2df3dada985c 268 // 命令確認 & 実行
denden 0:2df3dada985c 269 switch (rxData.direction) {
denden 0:2df3dada985c 270 case 'w': FFmove(); break;
denden 0:2df3dada985c 271 case 'q': FLmove(); break;
denden 0:2df3dada985c 272 case 'e': FRmove(); break;
denden 0:2df3dada985c 273
denden 0:2df3dada985c 274 case 'a': Lmove(); break;
denden 0:2df3dada985c 275 case 'd': Rmove(); break;
denden 0:2df3dada985c 276
denden 0:2df3dada985c 277 case 'x': BBmove(); break;
denden 0:2df3dada985c 278 case 'z': BLmove(); break;
denden 0:2df3dada985c 279 case 'c': BRmove(); break;
denden 0:2df3dada985c 280
denden 0:2df3dada985c 281 case 'o': Lturn(); break;
denden 0:2df3dada985c 282 case 'p': Rturn(); break;
denden 0:2df3dada985c 283
denden 0:2df3dada985c 284 case 's': Stop(); break;
denden 0:2df3dada985c 285
denden 0:2df3dada985c 286 case 'r':
denden 0:2df3dada985c 287 Raser(); // レーザー発射！
denden 0:2df3dada985c 288 rxData.direction = 0x00;
denden 0:2df3dada985c 289 break;
denden 0:2df3dada985c 290 }
denden 0:2df3dada985c 291 switch (rxData.turning) {
denden 0:2df3dada985c 292 case 0:
denden 0:2df3dada985c 293
denden 0:2df3dada985c 294 }
denden 0:2df3dada985c 295
denden 0:2df3dada985c 296
denden 0:2df3dada985c 297 // wait(0.5);
denden 0:2df3dada985c 298
denden 0:2df3dada985c 299 }
denden 0:2df3dada985c 300
denden 0:2df3dada985c 301
denden 0:2df3dada985c 302 return 0;
denden 0:2df3dada985c 303 }
denden 0:2df3dada985c 304
denden 0:2df3dada985c 305
denden 0:2df3dada985c 306
denden 0:2df3dada985c 307
denden 0:2df3dada985c 308
denden 0:2df3dada985c 309
denden 0:2df3dada985c 310
denden 0:2df3dada985c 311
denden 0:2df3dada985c 312
denden 0:2df3dada985c 313
denden 0:2df3dada985c 314
denden 0:2df3dada985c 315
denden 0:2df3dada985c 316