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@46:f1e87c375e02, 2017-03-31 (annotated)

Committer:
s1200058
Date:
Fri Mar 31 06:27:01 2017 +0000
Revision:
46:f1e87c375e02
Parent:
45:a90c4301ee45
Child:
47:626572030a78
???

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
kityann 0:daab5accfd83 27 /////////////////////////////////////////
kityann 0:daab5accfd83 28 //
kityann 0:daab5accfd83 29 //Pin Setting
kityann 0:daab5accfd83 30 //
kityann 0:daab5accfd83 31 /////////////////////////////////////////
s1200058 45:a90c4301ee45 32 //Serial Connect Setting: mbed <--> motor driver
kityann 0:daab5accfd83 33 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:daab5accfd83 34
kityann 0:daab5accfd83 35
kityann 0:daab5accfd83 36 /////////////////////////////////////////
kityann 0:daab5accfd83 37 //
kityann 0:daab5accfd83 38 //Connection Setting
kityann 0:daab5accfd83 39 //
kityann 0:daab5accfd83 40 /////////////////////////////////////////
kityann 0:daab5accfd83 41
kityann 0:daab5accfd83 42 //Serial Connect Setting: PC <--> mbed
kityann 0:daab5accfd83 43 Serial pc(USBTX, USBRX);
kityann 0:daab5accfd83 44
kityann 0:daab5accfd83 45 //Serial Connect Setting: GPS <--> mbed
kityann 0:daab5accfd83 46 Serial * gps_Serial;
kityann 0:daab5accfd83 47
kityann 0:daab5accfd83 48 //Serial Connect Setting: XBEE <--> mbed
kityann 0:daab5accfd83 49 XBee xbee(p13,p14);
kityann 0:daab5accfd83 50 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:daab5accfd83 51
kityann 0:daab5accfd83 52 //set up AigamozuControlPackets library
kityann 0:daab5accfd83 53 AigamozuControlPackets agz(agz_motorShield);
kityann 0:daab5accfd83 54
kityann 0:daab5accfd83 55
s1200058 2:886fac7f4399 56 /////////////////////////////////////////
s1200058 2:886fac7f4399 57 //
s1200058 2:886fac7f4399 58 //Plus Speed
s1200058 2:886fac7f4399 59 //
s1200058 45:a90c4301ee45 60 //ロボットを操縦する
s1200058 2:886fac7f4399 61 /////////////////////////////////////////
s1200058 45:a90c4301ee45 62 void plusSpeed(uint8_t *packetdata){
s1200058 2:886fac7f4399 63
s1200058 2:886fac7f4399 64 if(agz.nowMode == MANUAL_MODE){
s1200058 2:886fac7f4399 65 agz.changeSpeed(packetdata);
s1200058 2:886fac7f4399 66 }
s1200058 2:886fac7f4399 67
s1200058 2:886fac7f4399 68 }
kityann 0:daab5accfd83 69
kityann 24:698d4e920d33 70
s1200058 2:886fac7f4399 71
s1200058 2:886fac7f4399 72 /////////////////////////////////////////
s1200058 2:886fac7f4399 73 //
s1200058 2:886fac7f4399 74 //New Mode
s1200058 2:886fac7f4399 75 //
s1200058 45:a90c4301ee45 76 //ロボットのモードを変更する
s1200058 2:886fac7f4399 77 /////////////////////////////////////////
s1200058 45:a90c4301ee45 78 void newMode(uint8_t *packetdata){
s1200058 2:886fac7f4399 79
s1200058 19:a806105ba365 80 //bool result;
s1200058 20:eee8ac8d1788 81 agz.changeMode(packetdata);
s1200058 2:886fac7f4399 82
s1200058 2:886fac7f4399 83 }
kityann 1:b2b950b916ce 84
kityann 0:daab5accfd83 85
kityann 0:daab5accfd83 86 /////////////////////////////////////////
kityann 0:daab5accfd83 87 //
kityann 0:daab5accfd83 88 //Main Processing
kityann 0:daab5accfd83 89 //
kityann 0:daab5accfd83 90 /////////////////////////////////////////
kityann 0:daab5accfd83 91 int main() {
kityann 0:daab5accfd83 92 //start up time
kityann 0:daab5accfd83 93 wait(3);
kityann 0:daab5accfd83 94 //set pc frequency to 57600bps
kityann 0:daab5accfd83 95 pc.baud(PC_BAUD_RATE);
kityann 0:daab5accfd83 96 //set xbee frequency to 57600bps
kityann 0:daab5accfd83 97 xbee.begin(XBEE_BAUD_RATE);
kityann 0:daab5accfd83 98
kityann 0:daab5accfd83 99 //GPS setting
kityann 0:daab5accfd83 100 gps_Serial = new Serial(p28,p27);
kityann 0:daab5accfd83 101 Adafruit_GPS myGPS(gps_Serial);
s1200058 45:a90c4301ee45 102
s1200058 45:a90c4301ee45 103 //2secごとの更新用Timer
s1200058 45:a90c4301ee45 104 Timer autoTimer;
s1200058 45:a90c4301ee45 105 const int autoTime = 2000; //refresh time in ms
s1200058 45:a90c4301ee45 106
s1200058 45:a90c4301ee45 107 //interrupt start
s1200058 45:a90c4301ee45 108 autoTimer.start();
s1200058 45:a90c4301ee45 109 agz.moveTimer.start();
kityann 0:daab5accfd83 110
s1200058 29:524684a1198f 111 wait_ms(2000);
kityann 1:b2b950b916ce 112
s1200058 45:a90c4301ee45 113 printf("start\n");
s1200058 45:a90c4301ee45 114
kityann 0:daab5accfd83 115 while (true) {
kityann 0:daab5accfd83 116
kityann 0:daab5accfd83 117 //Check Xbee Buffer Available
kityann 0:daab5accfd83 118 xbee.readPacket();
s1200058 45:a90c4301ee45 119
s1200058 45:a90c4301ee45 120 //Xbee受信があった場合
kityann 0:daab5accfd83 121 if (xbee.getResponse().isAvailable()) {
kityann 0:daab5accfd83 122 xbee.getResponse().getZBRxResponse(zbRx);
s1200058 45:a90c4301ee45 123 //frame dataの格納
kityann 0:daab5accfd83 124 uint8_t *buf = zbRx.getFrameData();
s1200058 45:a90c4301ee45 125
kityann 0:daab5accfd83 126 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:daab5accfd83 127 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 128 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
s1200058 21:76e7f36df4a9 129 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
kityann 0:daab5accfd83 130
kityann 0:daab5accfd83 131 //Check Command Type
kityann 0:daab5accfd83 132 switch(Command_type){
kityann 0:daab5accfd83 133 //Get Request command
s1200058 45:a90c4301ee45 134 case MANUAL:{ //マニュアルモードパケット
s1200058 45:a90c4301ee45 135 plusSpeed(buf); //ロボットの操縦
s1200058 2:886fac7f4399 136 break;
s1200058 2:886fac7f4399 137 }
s1200058 45:a90c4301ee45 138 case STATUS_REQUEST:{ //ステータスリクエストパケット
kityann 0:daab5accfd83 139 break;
kityann 0:daab5accfd83 140 }
s1200058 45:a90c4301ee45 141 case CHANGE_MODE:{ //モード変更パケット
s1200058 45:a90c4301ee45 142 newMode(buf); //モードの変更
s1200058 2:886fac7f4399 143 break;
s1200058 2:886fac7f4399 144 }
s1200058 45:a90c4301ee45 145 case RECEIVE_STATUS:{ //レシーブステータスパケット
kityann 1:b2b950b916ce 146 break;
kityann 1:b2b950b916ce 147 }
kityann 0:daab5accfd83 148 default:{
kityann 0:daab5accfd83 149 break;
kityann 0:daab5accfd83 150 }
kityann 0:daab5accfd83 151 }//endswitch
kityann 0:daab5accfd83 152 }//endifZB_RX_RESPONSE
kityann 0:daab5accfd83 153 }//endifisAvailable
s1200058 38:5cd6d4964f65 154
s1200058 46:f1e87c375e02 155 //ロボットがオートモードのとき、シーケンス動作する
s1200058 45:a90c4301ee45 156 if(agz.nowMode == AUTO_GPS_MODE && autoTimer.read_ms() >= autoTime){
s1200058 45:a90c4301ee45 157 autoTimer.reset();
s1200058 45:a90c4301ee45 158 agz.autoMove();
s1200058 29:524684a1198f 159 }
s1200058 29:524684a1198f 160
s1200058 29:524684a1198f 161 }
s1200058 34:9c3192c00e41 162
kityann 0:daab5accfd83 163 }