aigamozu / Mbed 2 deprecated Aigamozu_Robot_templete

展示会用に簡単にしています

Dependencies:   ADXL345 AigamozuControlPackets_展示会 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed

Fork of Aigamozu_Robot_March by Mami Yokokawa

main.cpp@48:303e2ed5e062, 2017-04-24 (annotated)

Committer:
s1200058
Date:
Mon Apr 24 09:03:49 2017 +0000
Revision:
48:303e2ed5e062
Parent:
47:626572030a78 
for 3rd group

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:daab5accfd83 1 /**********************************************/
kityann 0:daab5accfd83 2 //
kityann 0:daab5accfd83 3 //
kityann 0:daab5accfd83 4 //
kityann 0:daab5accfd83 5 // Program name: Aigamozu ROBOT
kityann 0:daab5accfd83 6 // Author: Mineta Kizuku
s1200058 38:5cd6d4964f65 7 // Yokokawa
s1200058 21:76e7f36df4a9 8 //
kityann 0:daab5accfd83 9 //
kityann 0:daab5accfd83 10 /**********************************************/
kityann 0:daab5accfd83 11
kityann 0:daab5accfd83 12 /**********************************************/
kityann 0:daab5accfd83 13 //更新情報
s1200058 38:5cd6d4964f65 14 //展示会用プログラム
s1200058 38:5cd6d4964f65 15 //中身をすっきりさせました。
s1200058 46:f1e87c375e02 16 //Automodeの際のプログラムはautoMode()の中身を変更すれば大丈夫です。
kityann 0:daab5accfd83 17 /**********************************************/
kityann 0:daab5accfd83 18
kityann 0:daab5accfd83 19 #include "mbed.h"
kityann 0:daab5accfd83 20 #include "XBee.h"
kityann 0:daab5accfd83 21 #include "MBed_Adafruit_GPS.h"
kityann 0:daab5accfd83 22 #include "AigamozuControlPackets.h"
kityann 0:daab5accfd83 23 #include "agzIDLIST.h"
kityann 0:daab5accfd83 24 #include "aigamozuSetting.h"
s1200058 34:9c3192c00e41 25 #include "math.h"
kityann 0:daab5accfd83 26
s1200058 47:626572030a78 27 //************ID Number*****************
s1200058 47:626572030a78 28 const char MyID = 'e';
s1200058 48:303e2ed5e062 29 const char SenderIDc = 'i';
s1200058 47:626572030a78 30 //************ID Number*****************
s1200058 47:626572030a78 31
kityann 0:daab5accfd83 32 /////////////////////////////////////////
kityann 0:daab5accfd83 33 //
kityann 0:daab5accfd83 34 //Pin Setting
kityann 0:daab5accfd83 35 //
kityann 0:daab5accfd83 36 /////////////////////////////////////////
s1200058 45:a90c4301ee45 37 //Serial Connect Setting: mbed <--> motor driver
kityann 0:daab5accfd83 38 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:daab5accfd83 39
kityann 0:daab5accfd83 40
kityann 0:daab5accfd83 41 /////////////////////////////////////////
kityann 0:daab5accfd83 42 //
kityann 0:daab5accfd83 43 //Connection Setting
kityann 0:daab5accfd83 44 //
kityann 0:daab5accfd83 45 /////////////////////////////////////////
kityann 0:daab5accfd83 46
kityann 0:daab5accfd83 47 //Serial Connect Setting: PC <--> mbed
kityann 0:daab5accfd83 48 Serial pc(USBTX, USBRX);
kityann 0:daab5accfd83 49
kityann 0:daab5accfd83 50 //Serial Connect Setting: GPS <--> mbed
kityann 0:daab5accfd83 51 Serial * gps_Serial;
kityann 0:daab5accfd83 52
kityann 0:daab5accfd83 53 //Serial Connect Setting: XBEE <--> mbed
kityann 0:daab5accfd83 54 XBee xbee(p13,p14);
kityann 0:daab5accfd83 55 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:daab5accfd83 56
kityann 0:daab5accfd83 57 //set up AigamozuControlPackets library
kityann 0:daab5accfd83 58 AigamozuControlPackets agz(agz_motorShield);
kityann 0:daab5accfd83 59
kityann 0:daab5accfd83 60
s1200058 2:886fac7f4399 61 /////////////////////////////////////////
s1200058 2:886fac7f4399 62 //
s1200058 2:886fac7f4399 63 //Plus Speed
s1200058 2:886fac7f4399 64 //
s1200058 45:a90c4301ee45 65 //ロボットを操縦する
s1200058 2:886fac7f4399 66 /////////////////////////////////////////
s1200058 45:a90c4301ee45 67 void plusSpeed(uint8_t *packetdata){
s1200058 2:886fac7f4399 68
s1200058 2:886fac7f4399 69 if(agz.nowMode == MANUAL_MODE){
s1200058 2:886fac7f4399 70 agz.changeSpeed(packetdata);
s1200058 2:886fac7f4399 71 }
s1200058 2:886fac7f4399 72
s1200058 2:886fac7f4399 73 }
kityann 0:daab5accfd83 74
kityann 24:698d4e920d33 75
s1200058 2:886fac7f4399 76
s1200058 2:886fac7f4399 77 /////////////////////////////////////////
s1200058 2:886fac7f4399 78 //
s1200058 2:886fac7f4399 79 //New Mode
s1200058 2:886fac7f4399 80 //
s1200058 45:a90c4301ee45 81 //ロボットのモードを変更する
s1200058 2:886fac7f4399 82 /////////////////////////////////////////
s1200058 45:a90c4301ee45 83 void newMode(uint8_t *packetdata){
s1200058 2:886fac7f4399 84
s1200058 19:a806105ba365 85 //bool result;
s1200058 20:eee8ac8d1788 86 agz.changeMode(packetdata);
s1200058 2:886fac7f4399 87
s1200058 2:886fac7f4399 88 }
kityann 1:b2b950b916ce 89
kityann 0:daab5accfd83 90
kityann 0:daab5accfd83 91 /////////////////////////////////////////
kityann 0:daab5accfd83 92 //
s1200058 47:626572030a78 93 //Send_Change_Mode
s1200058 47:626572030a78 94 //
s1200058 47:626572030a78 95 //マニュアルモードのコマンドを送る
s1200058 47:626572030a78 96 /////////////////////////////////////////
s1200058 47:626572030a78 97 void Send_Change_Mode(char c){
s1200058 47:626572030a78 98
s1200058 47:626572030a78 99 XBeeAddress64 send_Address;
s1200058 47:626572030a78 100 if(SenderIDc == '0'){
s1200058 47:626572030a78 101 send_Address = manager_Address;
s1200058 47:626572030a78 102 }
s1200058 47:626572030a78 103 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
s1200058 47:626572030a78 104 send_Address = robot_Address[SenderIDc - 'A'];
s1200058 47:626572030a78 105 }
s1200058 47:626572030a78 106 if(SenderIDc >= 'a' && SenderIDc <= 'z'){
s1200058 47:626572030a78 107 send_Address = base_Address[SenderIDc - 'a'];
s1200058 47:626572030a78 108 }
s1200058 47:626572030a78 109
s1200058 47:626572030a78 110 //send normal data
s1200058 47:626572030a78 111 int cmode=0;
s1200058 47:626572030a78 112 if(c == 'n'){
s1200058 47:626572030a78 113 cmode = 0;
s1200058 47:626572030a78 114 pc.printf("stand by mode\n\r");
s1200058 47:626572030a78 115 }
s1200058 47:626572030a78 116 else if(c == 'm'){
s1200058 47:626572030a78 117 cmode = 1;
s1200058 47:626572030a78 118 pc.printf("manual mode\n\r");
s1200058 47:626572030a78 119 }
s1200058 47:626572030a78 120 else if(c == 'b'){
s1200058 47:626572030a78 121 cmode = 2;
s1200058 47:626572030a78 122 pc.printf("auto mode\n\r");
s1200058 47:626572030a78 123 }
s1200058 47:626572030a78 124
s1200058 47:626572030a78 125 //Create GPS Infomation Packet
s1200058 47:626572030a78 126 agz.createChangeModeCommand(MyID,SenderIDc,cmode);
s1200058 47:626572030a78 127
s1200058 47:626572030a78 128 //debug
s1200058 47:626572030a78 129 printf("RF data:");
s1200058 47:626572030a78 130 for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; i++){
s1200058 47:626572030a78 131 printf("%x ", agz.packetData[i]);
s1200058 47:626572030a78 132 }
s1200058 47:626572030a78 133 printf("\n\r");
s1200058 47:626572030a78 134
s1200058 47:626572030a78 135 //Select Destination
s1200058 47:626572030a78 136 ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
s1200058 47:626572030a78 137 //Send -> Base
s1200058 47:626572030a78 138 xbee.send(tx64request);
s1200058 47:626572030a78 139 }
s1200058 47:626572030a78 140
s1200058 47:626572030a78 141
s1200058 47:626572030a78 142 /////////////////////////////////////////
s1200058 47:626572030a78 143 //
kityann 0:daab5accfd83 144 //Main Processing
kityann 0:daab5accfd83 145 //
kityann 0:daab5accfd83 146 /////////////////////////////////////////
kityann 0:daab5accfd83 147 int main() {
kityann 0:daab5accfd83 148 //start up time
kityann 0:daab5accfd83 149 wait(3);
kityann 0:daab5accfd83 150 //set pc frequency to 57600bps
kityann 0:daab5accfd83 151 pc.baud(PC_BAUD_RATE);
kityann 0:daab5accfd83 152 //set xbee frequency to 57600bps
kityann 0:daab5accfd83 153 xbee.begin(XBEE_BAUD_RATE);
kityann 0:daab5accfd83 154
kityann 0:daab5accfd83 155 //GPS setting
kityann 0:daab5accfd83 156 gps_Serial = new Serial(p28,p27);
kityann 0:daab5accfd83 157 Adafruit_GPS myGPS(gps_Serial);
s1200058 45:a90c4301ee45 158
s1200058 45:a90c4301ee45 159 //2secごとの更新用Timer
s1200058 45:a90c4301ee45 160 Timer autoTimer;
s1200058 47:626572030a78 161 const int autoTime = 200; //refresh time in ms
s1200058 45:a90c4301ee45 162
s1200058 45:a90c4301ee45 163 //interrupt start
s1200058 45:a90c4301ee45 164 autoTimer.start();
s1200058 45:a90c4301ee45 165 agz.moveTimer.start();
kityann 0:daab5accfd83 166
s1200058 29:524684a1198f 167 wait_ms(2000);
kityann 1:b2b950b916ce 168
s1200058 45:a90c4301ee45 169 printf("start\n");
s1200058 45:a90c4301ee45 170
kityann 0:daab5accfd83 171 while (true) {
kityann 0:daab5accfd83 172
kityann 0:daab5accfd83 173 //Check Xbee Buffer Available
kityann 0:daab5accfd83 174 xbee.readPacket();
s1200058 45:a90c4301ee45 175
s1200058 45:a90c4301ee45 176 //Xbee受信があった場合
kityann 0:daab5accfd83 177 if (xbee.getResponse().isAvailable()) {
kityann 0:daab5accfd83 178 xbee.getResponse().getZBRxResponse(zbRx);
s1200058 45:a90c4301ee45 179 //frame dataの格納
kityann 0:daab5accfd83 180 uint8_t *buf = zbRx.getFrameData();
s1200058 45:a90c4301ee45 181
kityann 0:daab5accfd83 182 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:daab5accfd83 183 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 184 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
s1200058 21:76e7f36df4a9 185 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
kityann 0:daab5accfd83 186
kityann 0:daab5accfd83 187 //Check Command Type
kityann 0:daab5accfd83 188 switch(Command_type){
kityann 0:daab5accfd83 189 //Get Request command
s1200058 45:a90c4301ee45 190 case MANUAL:{ //マニュアルモードパケット
s1200058 45:a90c4301ee45 191 plusSpeed(buf); //ロボットの操縦
s1200058 2:886fac7f4399 192 break;
s1200058 2:886fac7f4399 193 }
s1200058 45:a90c4301ee45 194 case STATUS_REQUEST:{ //ステータスリクエストパケット
kityann 0:daab5accfd83 195 break;
kityann 0:daab5accfd83 196 }
s1200058 45:a90c4301ee45 197 case CHANGE_MODE:{ //モード変更パケット
s1200058 45:a90c4301ee45 198 newMode(buf); //モードの変更
s1200058 2:886fac7f4399 199 break;
s1200058 2:886fac7f4399 200 }
s1200058 45:a90c4301ee45 201 case RECEIVE_STATUS:{ //レシーブステータスパケット
kityann 1:b2b950b916ce 202 break;
kityann 1:b2b950b916ce 203 }
kityann 0:daab5accfd83 204 default:{
kityann 0:daab5accfd83 205 break;
kityann 0:daab5accfd83 206 }
kityann 0:daab5accfd83 207 }//endswitch
kityann 0:daab5accfd83 208 }//endifZB_RX_RESPONSE
kityann 0:daab5accfd83 209 }//endifisAvailable
s1200058 38:5cd6d4964f65 210
s1200058 46:f1e87c375e02 211 //ロボットがオートモードのとき、シーケンス動作する
s1200058 47:626572030a78 212 // if(agz.nowMode == AUTO_GPS_MODE && autoTimer.read_ms() >= autoTime){
s1200058 47:626572030a78 213 // autoTimer.reset();
s1200058 47:626572030a78 214 // agz.autoMove();
s1200058 47:626572030a78 215 // }
s1200058 47:626572030a78 216 //
s1200058 47:626572030a78 217
s1200058 47:626572030a78 218 if(autoTimer.read_ms() >= autoTime && pc.readable()){
s1200058 47:626572030a78 219 autoTimer.reset();
s1200058 47:626572030a78 220 char c = pc.getc();
s1200058 47:626572030a78 221
s1200058 47:626572030a78 222 if(c == 'm' || c == 'n' || c == 'b'){
s1200058 47:626572030a78 223 //m:manual-mode,n:stand-by-mode,b:auto-gps-mode
s1200058 47:626572030a78 224 Send_Change_Mode(c);
s1200058 47:626572030a78 225 //pc.printf("%c\n\r", c);
s1200058 47:626572030a78 226 }
s1200058 29:524684a1198f 227 }
s1200058 29:524684a1198f 228 }
s1200058 34:9c3192c00e41 229
kityann 0:daab5accfd83 230 }