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