Naoya Muramatsu
6輪ロボットの制御プログラム。 6つのオムニホイール、2つの緑レーザー、4つの受光センサを持っている。 xbeeを使って、コントローラと通信を行う。 基本的にRedと同じ。
Diff: main.cpp
- Revision:
- 0:2df3dada985c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed May 04 12:50:58 2016 +0000
@@ -0,0 +1,316 @@
+// Robot
+// Blue
+
+#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 MyRemoteAddress[] = {0x00,0x13,0xa2,0x00,0x40,0x7c,0x04,0xbd}; // 見方コントローラのアドレス
+
+
+
+
+/*--- モーター制御用関数 ---*/
+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.71); CRmove(+0.71);
+ BLmove(-1.00); BRmove(-1.00);
+}
+void FRmove() {
+ FLmove(-1.00); FRmove(+1.00);
+ CLmove(+1.00); CRmove(+1.00);
+ BLmove(-1.00); BRmove(+1.00);
+}
+void FLmove() {
+ FLmove(+1.00); FRmove(-1.00);
+ CLmove(+1.00); CRmove(+1.00);
+ BLmove(+1.00); BRmove(-1.00);
+}
+void Lmove() {
+ FLmove(+1.00); FRmove(-1.00);
+ CLmove(-0.00); CRmove(-0.00);
+ BLmove(+1.00); BRmove(-1.00);
+}
+void Rmove() {
+ FLmove(-1.00); FRmove(+1.00);
+ CLmove(-0.00); CRmove(-0.00);
+ BLmove(-1.00); BRmove(+1.00);
+}
+void BBmove() {
+ FLmove(-1.00); FRmove(-1.00);
+ CLmove(-0.71); CRmove(-0.71);
+ BLmove(+1.00); BRmove(+1.00);
+}
+void BRmove() {
+ FLmove(-1.00); FRmove(+1.00);
+ 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(-0.70); FRmove(+0.70);
+ CLmove(+0.00); CRmove(+0.00);
+ BLmove(-1.00); BRmove(+1.00);
+}
+void Rturn() {
+ FLmove(+0.70); FRmove(-0.70);
+ CLmove(+0.00); CRmove(+0.00);
+ BLmove(+1.00); BRmove(-1.00);
+}
+
+
+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;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+