Naoya Muramatsu
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;
}