Naoya Muramatsu
/
4E_Rotot_Red_2014
6輪ロボットの制御プログラム。 6つのオムニホイール、2つの緑レーザー、4つの受光センサを持っている。 xbeeを使って、コントローラと通信を行う。
main.cpp
- Committer:
- denden
- Date:
- 2016-05-04
- Revision:
- 0:5e6d5dfcb386
File content as of revision 0:5e6d5dfcb386:
// Robot // Red #include "mbed.h" /* --- XBeeアドレス --- 平面アンテナ Red Zigbee Router API : 0013A200 407ABE67 Zigbee Router API : 0013A200 407ABCBB Blue Zigbee Coordinator API : 0013A200 407C04BD Zigbee Router API : 0013A200 407C04F9 */ Serial xbee(p28, p27); Serial pc(USBTX, USBRX); InterruptIn raserSensor1(p9); InterruptIn raserSensor2(p10); InterruptIn raserSensor3(p29); DigitalOut FR1(p11); DigitalOut FR2(p12); DigitalOut FL1(p13); DigitalOut FL2(p14); DigitalOut BR1(p15); DigitalOut BR2(p16); DigitalOut BL1(p17); DigitalOut BL2(p18); DigitalOut CR1(p5); DigitalOut CR2(p6); DigitalOut CL1(p7); DigitalOut CL2(p8); PwmOut FR(p25); PwmOut FL(p22); PwmOut BR(p24); PwmOut BL(p23); PwmOut CR(p26); PwmOut CL(p21); DigitalOut Raser1(p20); DigitalOut Raser2(p30); DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); /*--- グローバル変数宣言---*/ uint8_t MyRemoteAddress[] = {0x00,0x13,0xa2,0x00,0x40,0x7a,0xbe,0x67}; // 敵コントローラのアドレス // uint8_t EnRemoteAddress[] = {0x00,0x13,0xa2,0x00,0x40,0x7c,0x04,0xf9}; // 敵コントローラのアドレス /*--- モーター制御用関数 ---*/ void FRmove (float vec) {FR = abs(vec); FR1 = (unsigned char)(abs)(vec+0.9); FR2 = (unsigned char)(abs)(vec-0.9);} void FLmove (float vec) {FL = abs(vec); FL1 = (unsigned char)(abs)(vec+0.9); FL2 = (unsigned char)(abs)(vec-0.9);} void CRmove (float vec) {CR = abs(vec); CR1 = (unsigned char)(abs)(vec+0.9); CR2 = (unsigned char)(abs)(vec-0.9);} void CLmove (float vec) {CL = abs(vec); CL1 = (unsigned char)(abs)(vec-0.9); CL2 = (unsigned char)(abs)(vec+0.9);} void BRmove (float vec) {BR = abs(vec); BR1 = (unsigned char)(abs)(vec+0.9); BR2 = (unsigned char)(abs)(vec-0.9);} void BLmove (float vec) {BL = abs(vec); BL1 = (unsigned char)(abs)(vec+0.9); BL2 = (unsigned char)(abs)(vec-0.9);} /*--- 進行方向制御用変数 ---*/ void FFmove() { FLmove(+1.00); FRmove(+1.00); CLmove(+0.61); CRmove(+0.71); BLmove(-1.00); BRmove(-1.00); } void FRmove() { FLmove(-0.60); FRmove(+0.60); CLmove(+1.00); CRmove(+1.00); BLmove(-1.00); BRmove(+1.00); } void FLmove() { FLmove(+0.60); FRmove(-0.60); CLmove(+1.00); CRmove(+1.00); BLmove(+1.00); BRmove(-1.00); } void Lmove() { FLmove(+0.70); FRmove(-0.70); CLmove(+0.00); CRmove(+0.00); BLmove(+1.00); BRmove(-1.00); } void Rmove() { FLmove(-0.60); FRmove(+0.60); CLmove(+0.00); CRmove(+0.00); BLmove(-1.00); BRmove(+1.00); } void BBmove() { FLmove(-1.00); FRmove(-1.00); CLmove(-0.61); CRmove(-0.71); BLmove(+1.00); BRmove(+1.00); } void BRmove() { FLmove(-0.60); FRmove(+0.60); CLmove(-1.00); CRmove(-1.00); BLmove(-1.00); BRmove(+1.00); } void BLmove() { FLmove(+0.70); FRmove(-0.70); CLmove(-1.00); CRmove(-1.00); BLmove(+1.00); BRmove(-1.00); } void Lturn() { FLmove(+1.00); FRmove(-1.00); CLmove(-1.00); CRmove(+1.00); BLmove(+0.50); BRmove(-0.50); } void Rturn() { FLmove(-1.00); FRmove(+1.00); CLmove(+1.00); CRmove(-1.00); BLmove(-0.50); BRmove(+0.50); } void Stop() { FR1 = 1; FR2 = 1; FL1 = 1; FL2 = 1; CR1 = 1; CR2 = 1; CL1 = 1; CL2 = 1; BR1 = 1; BR2 = 1; BL1 = 1; BL2 = 1; } /*--- データフレーム用クラス ---*/ class xbeeAPI { public: uint8_t address[8]; uint8_t direction; uint8_t turning; void reset() { for (int i=0; i<8; i++) address[i]=0; direction = 0; turning = 0; } }; /*--- 送信用関数 ---*/ void xbeeTx(uint8_t *address, uint8_t data) { int i=0; xbee.putc(0x7E); xbee.putc(0x00); // フレーム長 xbee.putc(0x0F); xbee.putc(0x10); // フレームタイプ xbee.putc(0x01); // フレームID for (int i=0; i<8; i++) xbee.putc(address[i]); // 62bit宛先アドレス xbee.putc(0xFF); // 16bit宛先ネットワークアドレス xbee.putc(0xFE); xbee.putc(0x00); // ブロードキャスト半径 xbee.putc(0x00); // オプション xbee.putc(data); // RFデータ // チェックサムの計算 uint8_t checksum = 0xFF - (0x10+0x01+0xFF+0xFE+data); for (i=0; i<8; i++) checksum -= address[i]; xbee.putc(checksum); // チェックサム // 送信ステータスの確認 if (xbee.readable()) { if (xbee.getc() == 0x7E) { for (i=0; i<8; i++) xbee.getc(); if (xbee.getc() == 0x00) pc.printf("Send success!\n"); else pc.printf("Send missed.\n"); for (i=9; i<10; i++) xbee.getc(); } } } /*--- 受信用関数 ---*/ xbeeAPI xbeeRx() { xbeeAPI returnApi; // 返り値用の変数 returnApi.reset(); if (xbee.readable() > 0) { pc.printf("readable\n"); if (xbee.getc() == 0x7E) { pc.printf("startBit\n"); int i=0; uint8_t buffer[18]; buffer[0] = 0x7E; for (i=1; i<18; i++) { buffer[i] = xbee.getc(); } // 送信元アドレスを抽出 for (i=0; i<8; i++) { returnApi.address[i] = buffer[i+4]; } // 受信データを抽出 returnApi.direction = buffer[15]; returnApi.turning = buffer[16]; // データをPCに表示 for (i=0; i<8; i++) pc.printf("%x", returnApi.address[i]); pc.printf("\n"); pc.printf("%x\n", returnApi.direction); pc.printf("%x\n", returnApi.turning); } } return returnApi; } /*--- レーザー発射 ---*/ void Raser() { Raser1 = 1; Raser2 = 1; wait(1); Raser1 = 0; Raser2 = 0; } /*--- レーザー被弾時割り込み ---*/ // 注)タクトスイッチに、チャタリング防止のためのコンデンサフィルタを付ける void damage() { // 見方コントローラに連絡 led1 = 1; xbeeTx(MyRemoteAddress, 'D'); wait(1); led1 = 0; } int main() { /*--- 変数宣言 ---*/ int i, j, k; xbeeAPI rxData; // コントローラからのデータ受信用変数 /*--- 初期設定 ---*/ // ビットレートの設定 xbee.baud(9600); pc.baud(9600); // 割り込みの設定 raserSensor1.rise(&damage); raserSensor2.rise(&damage); raserSensor3.rise(&damage); // 初期値の設定 Raser1=0; Raser2=0; led1=0; led2=0; led3=0; led4=0; /*--- 繰り返し処理 ---*/ while(1) { // 受信 rxData = xbeeRx(); // チェック if (rxData.address[7] != MyRemoteAddress[7]) continue; // 命令確認 & 実行 switch (rxData.direction) { case 'w': FFmove(); break; case 'q': FLmove(); break; case 'e': FRmove(); break; case 'a': Lmove(); break; case 'd': Rmove(); break; case 'x': BBmove(); break; case 'z': BLmove(); break; case 'c': BRmove(); break; case 'o': Lturn(); break; case 'p': Rturn(); break; case 's': Stop(); break; case 'r': Raser(); // レーザー発射! rxData.direction = 0x00; break; } switch (rxData.turning) { case 0: } // wait(0.5); } return 0; }