Naoya Muramatsu / Mbed 2 deprecated 4E_Rotot_Red_2014

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

Dependencies:   mbed

main.cpp@0:5e6d5dfcb386, 2016-05-04 (annotated)

Committer:
denden
Date:
Wed May 04 12:48:26 2016 +0000
Revision:
0:5e6d5dfcb386
publish

Who changed what in which revision?

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