aigamozu / Mbed 2 deprecated Aigamozu_Robot_ver1_2_1

commentの追加 by Yokokawa

Dependencies:   ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed

Fork of Aigamozu_Robot_ver1_2 by aigamozu

main.cpp@8:447211fa24dd, 2015-05-12 (annotated)

Committer:
s1200058
Date:
Tue May 12 16:31:47 2015 +0000
Revision:
8:447211fa24dd
Parent:
6:4abf031b5ccc 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:eee5e3d906ce 1 /**********************************************/
kityann 0:eee5e3d906ce 2 //
kityann 0:eee5e3d906ce 3 //
kityann 0:eee5e3d906ce 4 //
kityann 0:eee5e3d906ce 5 // Program name: Aigamozu BASE
kityann 0:eee5e3d906ce 6 // Author: Mineta Kizuku
kityann 0:eee5e3d906ce 7 //
kityann 0:eee5e3d906ce 8 //
kityann 0:eee5e3d906ce 9 /**********************************************/
kityann 0:eee5e3d906ce 10
kityann 0:eee5e3d906ce 11 /**********************************************/
kityann 0:eee5e3d906ce 12 //更新情報
kityann 0:eee5e3d906ce 13 //2015/05/11
kityann 0:eee5e3d906ce 14 //ベースプログラムの作成
kityann 0:eee5e3d906ce 15 //
s1200058 8:447211fa24dd 16 //2015/05/12
s1200058 8:447211fa24dd 17 //ロボットプログラムへの変更
kityann 0:eee5e3d906ce 18 //
s1200058 8:447211fa24dd 19 //main()内でAutoModeのときでかつ2000ms経った時にGPSを取得する。
s1200058 8:447211fa24dd 20 //auto_Move()の作成
s1200058 8:447211fa24dd 21 ////GPSをセットする。
s1200058 8:447211fa24dd 22 ////gpsAutoを使いInAreaかOutAreaの判定。
s1200058 8:447211fa24dd 23 ////GPSをプリントする。
s1200058 8:447211fa24dd 24 //
s1200058 8:447211fa24dd 25 //AigamozuControlPackets内
s1200058 8:447211fa24dd 26 ////GPSの設定
s1200058 8:447211fa24dd 27 ////get_KAlman, set_autoGPS, set_KalmanGPS系の関数の作成。
s1200058 8:447211fa24dd 28 ////set_GPS系関数内
s1200058 8:447211fa24dd 29 ////呼び出すごとに+=0.0001等を設定
kityann 0:eee5e3d906ce 30 //
kityann 0:eee5e3d906ce 31 /**********************************************/
kityann 0:eee5e3d906ce 32
kityann 0:eee5e3d906ce 33 #include "mbed.h"
kityann 0:eee5e3d906ce 34 #include "XBee.h"
kityann 0:eee5e3d906ce 35 #include "MBed_Adafruit_GPS.h"
kityann 0:eee5e3d906ce 36 #include "AigamozuControlPackets.h"
kityann 0:eee5e3d906ce 37 #include "agzIDLIST.h"
kityann 0:eee5e3d906ce 38 #include "aigamozuSetting.h"
kityann 0:eee5e3d906ce 39 #include "agz_common.h"
kityann 0:eee5e3d906ce 40 #include "Kalman.h"
kityann 0:eee5e3d906ce 41
kityann 0:eee5e3d906ce 42 //************ID Number*****************
kityann 0:eee5e3d906ce 43 const char MyID = 'D';
kityann 0:eee5e3d906ce 44 //************ID Number*****************
kityann 0:eee5e3d906ce 45
kityann 0:eee5e3d906ce 46 /////////////////////////////////////////
kityann 0:eee5e3d906ce 47 //
kityann 0:eee5e3d906ce 48 //Pin Setting
kityann 0:eee5e3d906ce 49 //
kityann 0:eee5e3d906ce 50 /////////////////////////////////////////
kityann 0:eee5e3d906ce 51 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:eee5e3d906ce 52
kityann 0:eee5e3d906ce 53
kityann 0:eee5e3d906ce 54 /////////////////////////////////////////
kityann 0:eee5e3d906ce 55 //
kityann 0:eee5e3d906ce 56 //Connection Setting
kityann 0:eee5e3d906ce 57 //
kityann 0:eee5e3d906ce 58 /////////////////////////////////////////
kityann 0:eee5e3d906ce 59
kityann 0:eee5e3d906ce 60 //Serial Connect Setting: PC <--> mbed
kityann 0:eee5e3d906ce 61 Serial pc(USBTX, USBRX);
kityann 0:eee5e3d906ce 62
kityann 0:eee5e3d906ce 63 //Serial Connect Setting: GPS <--> mbed
kityann 0:eee5e3d906ce 64 Serial * gps_Serial;
kityann 0:eee5e3d906ce 65
kityann 0:eee5e3d906ce 66 //Serial Connect Setting: XBEE <--> mbed
kityann 0:eee5e3d906ce 67 XBee xbee(p13,p14);
kityann 0:eee5e3d906ce 68 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:eee5e3d906ce 69
kityann 0:eee5e3d906ce 70 //set up GPS module
kityann 0:eee5e3d906ce 71
kityann 0:eee5e3d906ce 72 //set up AigamozuControlPackets library
kityann 0:eee5e3d906ce 73 AigamozuControlPackets agz(agz_motorShield);
kityann 0:eee5e3d906ce 74
kityann 0:eee5e3d906ce 75
kityann 0:eee5e3d906ce 76 /////////////////////////////////////////
kityann 0:eee5e3d906ce 77 //
kityann 0:eee5e3d906ce 78 //For Kalman data
kityann 0:eee5e3d906ce 79 //
kityann 0:eee5e3d906ce 80 /////////////////////////////////////////
kityann 0:eee5e3d906ce 81 double sigmaGPS[2][2] = {{250,0},{0,250}};
kityann 0:eee5e3d906ce 82 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
kityann 0:eee5e3d906ce 83 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
kityann 0:eee5e3d906ce 84 double y[2],x[2][2]={0};
kityann 0:eee5e3d906ce 85 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS);
kityann 0:eee5e3d906ce 86
kityann 0:eee5e3d906ce 87 /////////////////////////////////////////
kityann 0:eee5e3d906ce 88 //
kityann 0:eee5e3d906ce 89 //Address List
kityann 0:eee5e3d906ce 90 //
kityann 0:eee5e3d906ce 91 /////////////////////////////////////////
kityann 0:eee5e3d906ce 92 XBeeAddress64 base_Address[BaseNumber] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L),
kityann 0:eee5e3d906ce 93 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L),
kityann 0:eee5e3d906ce 94 XBeeAddress64(BASE5_32H,BASE5_32L)};
kityann 0:eee5e3d906ce 95 XBeeAddress64 robot_Address[RobotNumber] = {XBeeAddress64(ROBOT1_32H,ROBOT1_32L), XBeeAddress64(ROBOT1_32H,ROBOT1_32L)};
kityann 0:eee5e3d906ce 96 XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L);
kityann 0:eee5e3d906ce 97
kityann 0:eee5e3d906ce 98
s1200058 1:d10d72b1d29b 99
kityann 0:eee5e3d906ce 100 /////////////////////////////////////////
kityann 0:eee5e3d906ce 101 //
kityann 0:eee5e3d906ce 102 //Send_Status
kityann 0:eee5e3d906ce 103 //
kityann 0:eee5e3d906ce 104 //リクエストがきたとき、自分の位置情報などを返信する
kityann 0:eee5e3d906ce 105 /////////////////////////////////////////
kityann 0:eee5e3d906ce 106 void Send_Status(char SenderIDc){
kityann 0:eee5e3d906ce 107 XBeeAddress64 send_Address;
kityann 0:eee5e3d906ce 108 if(SenderIDc == '0'){
kityann 0:eee5e3d906ce 109 send_Address = manager_Address;
kityann 0:eee5e3d906ce 110 }
kityann 0:eee5e3d906ce 111 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
kityann 0:eee5e3d906ce 112 send_Address = robot_Address[SenderIDc - 'A'];
kityann 0:eee5e3d906ce 113 }
kityann 0:eee5e3d906ce 114 if(SenderIDc >= 'a' && SenderIDc <= 'z'){
kityann 0:eee5e3d906ce 115 send_Address = base_Address[SenderIDc - 'a'];
kityann 0:eee5e3d906ce 116 }
kityann 0:eee5e3d906ce 117 //send normal data
kityann 0:eee5e3d906ce 118 //Create GPS Infomation Packet
s1200058 1:d10d72b1d29b 119 // agz.createReceiveStatusCommand(MyID,SenderIDc,agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1200058 1:d10d72b1d29b 120 // agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1200058 1:d10d72b1d29b 121 // agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 0:eee5e3d906ce 122 //Select Destination
s1200058 1:d10d72b1d29b 123 // ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
kityann 0:eee5e3d906ce 124 //Send -> Base
s1200058 1:d10d72b1d29b 125 // xbee.send(tx64request);
kityann 0:eee5e3d906ce 126 }
kityann 0:eee5e3d906ce 127
kityann 0:eee5e3d906ce 128 /////////////////////////////////////////
kityann 0:eee5e3d906ce 129 //
kityann 0:eee5e3d906ce 130 //Get GPS function
kityann 0:eee5e3d906ce 131 //
kityann 0:eee5e3d906ce 132 /////////////////////////////////////////
kityann 0:eee5e3d906ce 133
kityann 0:eee5e3d906ce 134 void Get_GPS(Adafruit_GPS *myGPS){
kityann 0:eee5e3d906ce 135 static bool flag = true;
kityann 0:eee5e3d906ce 136
kityann 0:eee5e3d906ce 137 if (myGPS->fix) {
kityann 0:eee5e3d906ce 138 agz.nowStatus = GPS_AVAIL;
kityann 0:eee5e3d906ce 139
kityann 0:eee5e3d906ce 140 if(flag){//初期値設定
kityann 0:eee5e3d906ce 141 if(myGPS->longitudeH>=138 && myGPS->longitudeH<=141 && myGPS->latitudeH>=36 && myGPS->latitudeH<=38){
kityann 0:eee5e3d906ce 142 flag = false;
kityann 0:eee5e3d906ce 143 x[0][0]=(double)myGPS->latitudeL;
kityann 0:eee5e3d906ce 144 x[0][1]=(double)myGPS->longitudeL;
kityann 0:eee5e3d906ce 145 }
kityann 0:eee5e3d906ce 146 }
kityann 0:eee5e3d906ce 147
kityann 0:eee5e3d906ce 148 if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
kityann 0:eee5e3d906ce 149 return;
kityann 0:eee5e3d906ce 150 }
s1200058 1:d10d72b1d29b 151
kityann 0:eee5e3d906ce 152 //Kalman Filter
kityann 0:eee5e3d906ce 153 Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
kityann 0:eee5e3d906ce 154
kityann 0:eee5e3d906ce 155 agz.reNewRobotPoint(myGPS->longitudeH,myGPS->longitudeL,myGPS->latitudeH,myGPS->latitudeL);
kityann 0:eee5e3d906ce 156 agz.reNewRobotPointKalman(myGPS->longitudeKH,myGPS->longitudeKL,myGPS->latitudeKH,myGPS->latitudeKL);
kityann 0:eee5e3d906ce 157 }
kityann 0:eee5e3d906ce 158 else agz.nowStatus = GPS_UNAVAIL;
kityann 0:eee5e3d906ce 159
kityann 0:eee5e3d906ce 160 }
kityann 0:eee5e3d906ce 161
kityann 0:eee5e3d906ce 162
kityann 0:eee5e3d906ce 163 /////////////////////////////////////////
kityann 0:eee5e3d906ce 164 //
kityann 0:eee5e3d906ce 165 //Kalman Processing
kityann 0:eee5e3d906ce 166 //
kityann 0:eee5e3d906ce 167 /////////////////////////////////////////
kityann 0:eee5e3d906ce 168
kityann 0:eee5e3d906ce 169 void get_K(){
kityann 0:eee5e3d906ce 170 double temp[2][2]={
kityann 0:eee5e3d906ce 171 {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
kityann 0:eee5e3d906ce 172 {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
kityann 0:eee5e3d906ce 173 };
kityann 0:eee5e3d906ce 174 double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
kityann 0:eee5e3d906ce 175 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
kityann 0:eee5e3d906ce 176 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
kityann 0:eee5e3d906ce 177 }
kityann 0:eee5e3d906ce 178
kityann 0:eee5e3d906ce 179
kityann 0:eee5e3d906ce 180 void get_x(){
kityann 0:eee5e3d906ce 181 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
kityann 0:eee5e3d906ce 182 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
kityann 0:eee5e3d906ce 183 }
kityann 0:eee5e3d906ce 184
kityann 0:eee5e3d906ce 185
kityann 0:eee5e3d906ce 186 void get_sigma(){
kityann 0:eee5e3d906ce 187 double temp[2][2];
kityann 0:eee5e3d906ce 188 for(int i=0;i<2;i++) {
kityann 0:eee5e3d906ce 189 for(int j=0;j<2;j++) {
kityann 0:eee5e3d906ce 190 for(int k=0;k<2;k++) {
kityann 0:eee5e3d906ce 191 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
kityann 0:eee5e3d906ce 192 }
kityann 0:eee5e3d906ce 193 }
kityann 0:eee5e3d906ce 194 }
kityann 0:eee5e3d906ce 195 for(int i = 0;i < 2;i++){
kityann 0:eee5e3d906ce 196 for(int j = 0;j < 2;j++){
kityann 0:eee5e3d906ce 197 sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
kityann 0:eee5e3d906ce 198 }
kityann 0:eee5e3d906ce 199 }
kityann 0:eee5e3d906ce 200 }
kityann 0:eee5e3d906ce 201
kityann 0:eee5e3d906ce 202 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
kityann 0:eee5e3d906ce 203 y[0] = Latitude;
kityann 0:eee5e3d906ce 204 y[1] = Longitude;
kityann 0:eee5e3d906ce 205 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
kityann 0:eee5e3d906ce 206 get_K();
kityann 0:eee5e3d906ce 207 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
kityann 0:eee5e3d906ce 208 get_x();
kityann 0:eee5e3d906ce 209 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
kityann 0:eee5e3d906ce 210 get_sigma();
kityann 0:eee5e3d906ce 211
kityann 0:eee5e3d906ce 212
kityann 0:eee5e3d906ce 213 //kousinn
kityann 0:eee5e3d906ce 214 for(int i = 0;i < 2;i++){
kityann 0:eee5e3d906ce 215 for(int j = 0;j < 2;j++){
kityann 0:eee5e3d906ce 216 K[0][i][j]=K[1][i][j];
kityann 0:eee5e3d906ce 217 x[0][i]=x[1][i];
kityann 0:eee5e3d906ce 218 sigma[0][i][j]=sigma[1][i][j];
kityann 0:eee5e3d906ce 219 }
kityann 0:eee5e3d906ce 220 }
kityann 0:eee5e3d906ce 221
kityann 0:eee5e3d906ce 222 myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
kityann 0:eee5e3d906ce 223 myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
kityann 0:eee5e3d906ce 224 myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
kityann 0:eee5e3d906ce 225 myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
kityann 0:eee5e3d906ce 226
kityann 0:eee5e3d906ce 227 agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
kityann 0:eee5e3d906ce 228 }
kityann 0:eee5e3d906ce 229
s1200058 1:d10d72b1d29b 230 void auto_Move(){
s1200058 1:d10d72b1d29b 231
s1200058 1:d10d72b1d29b 232 bool result;
s1200058 1:d10d72b1d29b 233 int i;
s1200058 1:d10d72b1d29b 234
s1200058 1:d10d72b1d29b 235 result = agz.gpsAuto();
s1200058 1:d10d72b1d29b 236 agz.set_agzAutoGPS();
s1200058 1:d10d72b1d29b 237 agz.set_agzKalmanGPS();
s1200058 1:d10d72b1d29b 238
s1200058 1:d10d72b1d29b 239 if(result == true){
s1200058 1:d10d72b1d29b 240 printf("Out Area\n");
s1200058 1:d10d72b1d29b 241 }
s1200058 1:d10d72b1d29b 242 else if(result == false){
s1200058 1:d10d72b1d29b 243 printf("In Area\n");
s1200058 1:d10d72b1d29b 244 }
s1200058 1:d10d72b1d29b 245 for(i = 0; i < 4; i++){
s1200058 1:d10d72b1d29b 246 printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i));
s1200058 1:d10d72b1d29b 247 }
s1200058 1:d10d72b1d29b 248 printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
s1200058 1:d10d72b1d29b 249
s1200058 1:d10d72b1d29b 250 }
kityann 0:eee5e3d906ce 251
kityann 0:eee5e3d906ce 252 /////////////////////////////////////////
kityann 0:eee5e3d906ce 253 //
kityann 0:eee5e3d906ce 254 //Main Processing
kityann 0:eee5e3d906ce 255 //
kityann 0:eee5e3d906ce 256 /////////////////////////////////////////
kityann 0:eee5e3d906ce 257 int main() {
kityann 0:eee5e3d906ce 258 //start up time
kityann 0:eee5e3d906ce 259 wait(3);
kityann 0:eee5e3d906ce 260 //set pc frequency to 57600bps
kityann 0:eee5e3d906ce 261 pc.baud(PC_BAUD_RATE);
kityann 0:eee5e3d906ce 262 //set xbee frequency to 57600bps
kityann 0:eee5e3d906ce 263 xbee.begin(XBEE_BAUD_RATE);
kityann 0:eee5e3d906ce 264
kityann 0:eee5e3d906ce 265
kityann 0:eee5e3d906ce 266 //GPS setting
kityann 0:eee5e3d906ce 267 gps_Serial = new Serial(p28,p27);
kityann 0:eee5e3d906ce 268 Adafruit_GPS myGPS(gps_Serial);
kityann 0:eee5e3d906ce 269 Timer refresh_Timer;
kityann 0:eee5e3d906ce 270 const int refresh_Time = 2000; //refresh time in ms
kityann 0:eee5e3d906ce 271 myGPS.begin(GPS_BAUD_RATE);
kityann 0:eee5e3d906ce 272
kityann 0:eee5e3d906ce 273 char SenderIDc;
s1200058 1:d10d72b1d29b 274
s1200058 1:d10d72b1d29b 275 Timer auto_Timer;
s1200058 1:d10d72b1d29b 276 const int auto_Time = 2000;
s1200058 6:4abf031b5ccc 277 agz.nowMode = AUTO_MODE;
s1200058 1:d10d72b1d29b 278
kityann 0:eee5e3d906ce 279 //GPS Send Command
kityann 0:eee5e3d906ce 280 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:eee5e3d906ce 281 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:eee5e3d906ce 282 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:eee5e3d906ce 283
kityann 0:eee5e3d906ce 284 wait(2);
kityann 0:eee5e3d906ce 285
kityann 0:eee5e3d906ce 286 //interrupt start
kityann 0:eee5e3d906ce 287 refresh_Timer.start();
s1200058 1:d10d72b1d29b 288 auto_Timer.start();
kityann 0:eee5e3d906ce 289
kityann 0:eee5e3d906ce 290 while (true) {
kityann 0:eee5e3d906ce 291
kityann 0:eee5e3d906ce 292 //Check Xbee Buffer Available
kityann 0:eee5e3d906ce 293 xbee.readPacket();
kityann 0:eee5e3d906ce 294
kityann 0:eee5e3d906ce 295 if (xbee.getResponse().isAvailable()) {
kityann 0:eee5e3d906ce 296 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:eee5e3d906ce 297 uint8_t *buf = zbRx.getFrameData();
kityann 0:eee5e3d906ce 298
kityann 0:eee5e3d906ce 299 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:eee5e3d906ce 300 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:eee5e3d906ce 301 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
kityann 0:eee5e3d906ce 302 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
kityann 0:eee5e3d906ce 303 SenderIDc = buf1[5];//送信元のIDを取得する
kityann 0:eee5e3d906ce 304 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
kityann 0:eee5e3d906ce 305
kityann 0:eee5e3d906ce 306 //Check Command Type
kityann 0:eee5e3d906ce 307 switch(Command_type){
kityann 0:eee5e3d906ce 308 //Get Request command
kityann 0:eee5e3d906ce 309 case STATUS_REQUEST:{
kityann 0:eee5e3d906ce 310 Send_Status(SenderIDc);
kityann 0:eee5e3d906ce 311 break;
kityann 0:eee5e3d906ce 312 }
kityann 0:eee5e3d906ce 313 default:{
kityann 0:eee5e3d906ce 314 break;
kityann 0:eee5e3d906ce 315 }
kityann 0:eee5e3d906ce 316 }//endswitch
kityann 0:eee5e3d906ce 317 }//endifZB_RX_RESPONSE
kityann 0:eee5e3d906ce 318 }//endifisAvailable
s1200058 1:d10d72b1d29b 319
s1200058 1:d10d72b1d29b 320
kityann 0:eee5e3d906ce 321
kityann 0:eee5e3d906ce 322 myGPS.read();
kityann 0:eee5e3d906ce 323 //recive gps module
kityann 0:eee5e3d906ce 324 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:eee5e3d906ce 325 if ( myGPS.newNMEAreceived() ) {
kityann 0:eee5e3d906ce 326 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:eee5e3d906ce 327 continue;
kityann 0:eee5e3d906ce 328 }
kityann 0:eee5e3d906ce 329 }
kityann 0:eee5e3d906ce 330
kityann 0:eee5e3d906ce 331
kityann 0:eee5e3d906ce 332 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:eee5e3d906ce 333 refresh_Timer.reset();
kityann 0:eee5e3d906ce 334 Get_GPS(&myGPS);
kityann 0:eee5e3d906ce 335
kityann 0:eee5e3d906ce 336 }
s1200058 1:d10d72b1d29b 337
s1200058 1:d10d72b1d29b 338 if(agz.nowMode == AUTO_MODE && auto_Timer.read_ms() >= auto_Time){
s1200058 1:d10d72b1d29b 339 auto_Timer.reset();
s1200058 1:d10d72b1d29b 340 auto_Move();
s1200058 1:d10d72b1d29b 341 }
s1200058 1:d10d72b1d29b 342
kityann 0:eee5e3d906ce 343 }
kityann 0:eee5e3d906ce 344 }