Naoya Muramatsu
/
4E_Rotot_Red_2014
6輪ロボットの制御プログラム。 6つのオムニホイール、2つの緑レーザー、4つの受光センサを持っている。 xbeeを使って、コントローラと通信を行う。
Diff: main.cpp
- Revision:
- 0:5e6d5dfcb386
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed May 04 12:48:26 2016 +0000 @@ -0,0 +1,316 @@ +// 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; +} + + + + + + + + + + + + + +