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@49:a6ae71759d0f, 2017-08-04 (annotated)

Committer:
s1200058
Date:
Fri Aug 04 12:49:58 2017 +0000
Revision:
49:a6ae71759d0f
Parent:
45:a90c4301ee45 
for_OHARA

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