aigamozu / Mbed 2 deprecated aigamozu_auto_ver3_3_2_Nakazawa

20150511, Nakazawa

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

Fork of aigamozu_auto_ver3_3_2 by aigamozu

main.cpp@7:209c07cc2b80, 2015-05-11 (annotated)

Committer:
s1210160
Date:
Mon May 11 13:23:52 2015 +0000
Revision:
7:209c07cc2b80
Parent:
4:480f4eb9bcbc 
20150511, Nakazawa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:68d27eee9a6c 1 /**********************************************/
kityann 0:68d27eee9a6c 2 //
kityann 0:68d27eee9a6c 3 //
kityann 0:68d27eee9a6c 4 //
kityann 0:68d27eee9a6c 5 // Program name: Aigamozu Robot Control
kityann 0:68d27eee9a6c 6 // Author: Atsunori Maruyama
kityann 0:68d27eee9a6c 7 // Ver -> 1.3
kityann 0:68d27eee9a6c 8 // Day -> 2014/06/09
kityann 0:68d27eee9a6c 9 //
kityann 0:68d27eee9a6c 10 //
kityann 0:68d27eee9a6c 11 /**********************************************/
s1210160 7:209c07cc2b80 12 //
s1210160 7:209c07cc2b80 13 // 中澤遥菜
s1210160 7:209c07cc2b80 14 // 変更点：メイン関数の中の処理を関数化しました。
s1210160 7:209c07cc2b80 15 // setting(), interrupt_start(VNH5019 motorShield),
s1210160 7:209c07cc2b80 16 // GPS_Setting(int refresh_Time, int collect_Time),
s1210160 7:209c07cc2b80 17 // GPS_SendCommand(), check_CommandType(utf8_t command_type),
s1210160 7:209c07cc2b80 18 // LatLong_afterFiltering(double longitude, double latitude, double x0, double x1),
s1210160 7:209c07cc2b80 19 // send_GPSdata(), get_GPSdata(), getKalman()
s1210160 7:209c07cc2b80 20 //
s1210160 7:209c07cc2b80 21 /**********************************************/
kityann 0:68d27eee9a6c 22
kityann 0:68d27eee9a6c 23 #include "mbed.h"
kityann 0:68d27eee9a6c 24 #include "XBee.h"
kityann 0:68d27eee9a6c 25 #include "MBed_Adafruit_GPS.h"
kityann 0:68d27eee9a6c 26 #include "AigamozuControlPackets.h"
kityann 0:68d27eee9a6c 27 #include "agzIDLIST.h"
kityann 0:68d27eee9a6c 28 #include "aigamozuSetting.h"
kityann 0:68d27eee9a6c 29 #include "agz_common.h"
kityann 0:68d27eee9a6c 30 #include "Kalman.h"
kityann 0:68d27eee9a6c 31
kityann 0:68d27eee9a6c 32 /////////////////////////////////////////
kityann 0:68d27eee9a6c 33 //
kityann 0:68d27eee9a6c 34 //Connection Setting
kityann 0:68d27eee9a6c 35 //
kityann 0:68d27eee9a6c 36 /////////////////////////////////////////
kityann 0:68d27eee9a6c 37
kityann 0:68d27eee9a6c 38 //Serial Connect Setting: PC <--> mbed
kityann 0:68d27eee9a6c 39 Serial pc(USBTX, USBRX);
kityann 0:68d27eee9a6c 40
kityann 0:68d27eee9a6c 41 //Serial Connect Setting: GPS <--> mbed
kityann 0:68d27eee9a6c 42 Serial * gps_Serial;
kityann 0:68d27eee9a6c 43
kityann 0:68d27eee9a6c 44 //Serial Connect Setting: XBEE <--> mbed
kityann 0:68d27eee9a6c 45 XBee xbee(p13,p14);
kityann 0:68d27eee9a6c 46 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:68d27eee9a6c 47 XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
kityann 0:68d27eee9a6c 48
s1200058 4:480f4eb9bcbc 49 AGZ_ROBOT robot[5];
s1200058 4:480f4eb9bcbc 50
s1200058 4:480f4eb9bcbc 51 char Base_ID = 'A';
kityann 0:68d27eee9a6c 52
kityann 0:68d27eee9a6c 53 /////////////////////////////////////////
kityann 0:68d27eee9a6c 54 //
kityann 0:68d27eee9a6c 55 //Pin Setting
kityann 0:68d27eee9a6c 56 //
kityann 0:68d27eee9a6c 57 /////////////////////////////////////////
kityann 0:68d27eee9a6c 58 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:68d27eee9a6c 59
kityann 0:68d27eee9a6c 60
kityann 0:68d27eee9a6c 61
kityann 0:68d27eee9a6c 62 /////////////////////////////////////////
kityann 0:68d27eee9a6c 63 //
kityann 0:68d27eee9a6c 64 //Kalman Processing
kityann 0:68d27eee9a6c 65 //
kityann 0:68d27eee9a6c 66 /////////////////////////////////////////
kityann 0:68d27eee9a6c 67
kityann 0:68d27eee9a6c 68
kityann 0:68d27eee9a6c 69 double sigmaGPS[2][2] = {{250,0},{0,250}};
kityann 0:68d27eee9a6c 70 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
kityann 0:68d27eee9a6c 71 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
kityann 0:68d27eee9a6c 72 double y[2],x[2][2]={0};
kityann 0:68d27eee9a6c 73
kityann 0:68d27eee9a6c 74
kityann 0:68d27eee9a6c 75 void get_K(){
kityann 0:68d27eee9a6c 76 double temp[2][2]={
kityann 0:68d27eee9a6c 77 {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
kityann 0:68d27eee9a6c 78 {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
kityann 0:68d27eee9a6c 79 };
kityann 0:68d27eee9a6c 80 double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
kityann 0:68d27eee9a6c 81 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
kityann 0:68d27eee9a6c 82 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
kityann 0:68d27eee9a6c 83 }
kityann 0:68d27eee9a6c 84
kityann 0:68d27eee9a6c 85
kityann 0:68d27eee9a6c 86 void get_x(){
kityann 0:68d27eee9a6c 87 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
kityann 0:68d27eee9a6c 88 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
kityann 0:68d27eee9a6c 89 }
kityann 0:68d27eee9a6c 90
kityann 0:68d27eee9a6c 91
kityann 0:68d27eee9a6c 92 void get_sigma(){
kityann 0:68d27eee9a6c 93 double temp[2][2];
kityann 0:68d27eee9a6c 94 for(int i=0;i<2;i++) {
kityann 0:68d27eee9a6c 95 for(int j=0;j<2;j++) {
kityann 0:68d27eee9a6c 96 for(int k=0;k<2;k++) {
kityann 0:68d27eee9a6c 97 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
kityann 0:68d27eee9a6c 98 }
kityann 0:68d27eee9a6c 99 }
kityann 0:68d27eee9a6c 100 }
kityann 0:68d27eee9a6c 101 for(int i = 0;i < 2;i++){
kityann 0:68d27eee9a6c 102 for(int j = 0;j < 2;j++){
kityann 0:68d27eee9a6c 103 sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
kityann 0:68d27eee9a6c 104 }
kityann 0:68d27eee9a6c 105 }
kityann 0:68d27eee9a6c 106 }
kityann 0:68d27eee9a6c 107
kityann 0:68d27eee9a6c 108 void Kalman(double Latitude,double Longitude){
kityann 0:68d27eee9a6c 109 y[0] = Latitude;
kityann 0:68d27eee9a6c 110 y[1] = Longitude;
kityann 0:68d27eee9a6c 111 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
kityann 0:68d27eee9a6c 112 get_K();
kityann 0:68d27eee9a6c 113 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
kityann 0:68d27eee9a6c 114 get_x();
kityann 0:68d27eee9a6c 115 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
kityann 0:68d27eee9a6c 116 get_sigma();
kityann 0:68d27eee9a6c 117 }
kityann 0:68d27eee9a6c 118
kityann 0:68d27eee9a6c 119 /////////////////////////////////////////
kityann 0:68d27eee9a6c 120 //
s1210160 7:209c07cc2b80 121 //setting
kityann 0:68d27eee9a6c 122 //
kityann 0:68d27eee9a6c 123 /////////////////////////////////////////
s1210160 7:209c07cc2b80 124 void setting(){
s1210160 7:209c07cc2b80 125 //start up time
kityann 0:68d27eee9a6c 126 wait(3);
kityann 0:68d27eee9a6c 127 //set pc frequency to 57600bps
kityann 0:68d27eee9a6c 128 pc.baud(PC_BAUD_RATE);
kityann 0:68d27eee9a6c 129 //set xbee frequency to 57600bps
kityann 0:68d27eee9a6c 130 xbee.begin(XBEE_BAUD_RATE);
s1210160 7:209c07cc2b80 131 }
s1210160 7:209c07cc2b80 132
s1210160 7:209c07cc2b80 133 /////////////////////////////////////////
s1210160 7:209c07cc2b80 134 //
s1210160 7:209c07cc2b80 135 //interrupt start
s1210160 7:209c07cc2b80 136 //
s1210160 7:209c07cc2b80 137 /////////////////////////////////////////
s1210160 7:209c07cc2b80 138 void interrupt_start(VNH5019 motorShield, Timer refresh){
s1210160 7:209c07cc2b80 139 AigamozuControlPackets agz(motorShield);
s1210160 7:209c07cc2b80 140 refresh.start();
kityann 0:68d27eee9a6c 141
s1210160 7:209c07cc2b80 142 printf("start\n");
s1210160 7:209c07cc2b80 143 }
s1210160 7:209c07cc2b80 144
s1210160 7:209c07cc2b80 145 /////////////////////////////////////////
s1210160 7:209c07cc2b80 146 //
s1210160 7:209c07cc2b80 147 //GPS Setting
s1210160 7:209c07cc2b80 148 //
s1210160 7:209c07cc2b80 149 /////////////////////////////////////////
s1210160 7:209c07cc2b80 150 XBeeAddress64 collect_Address[5];
s1210160 7:209c07cc2b80 151 XBeeAddress64 robot_Address
s1210160 7:209c07cc2b80 152
s1210160 7:209c07cc2b80 153 void GPS_Setting(Timer refresh_Timer, Timer collect_Timer){
kityann 0:68d27eee9a6c 154 gps_Serial = new Serial(p28,p27);
kityann 0:68d27eee9a6c 155 Adafruit_GPS myGPS(gps_Serial);
kityann 0:68d27eee9a6c 156 Timer refresh_Timer;
s1210160 7:209c07cc2b80 157 const int refresh_Time = refresh_Timer; //refresh time in ms
kityann 0:68d27eee9a6c 158 myGPS.begin(GPS_BAUD_RATE);
kityann 0:68d27eee9a6c 159
kityann 0:68d27eee9a6c 160 Timer collect_Timer;
s1210160 7:209c07cc2b80 161 const int collect_Time = collect_Timer; //when we collect 4 GPS point, we use
s1210160 7:209c07cc2b80 162 collect_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L),
s1200058 1:d351cf39a6e3 163 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L), XBeeAddress64(BASE5_32H,BASE5_32L)};
s1210160 7:209c07cc2b80 164 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
s1210160 7:209c07cc2b80 165 }
s1210160 7:209c07cc2b80 166
s1210160 7:209c07cc2b80 167 /////////////////////////////////////////
s1210160 7:209c07cc2b80 168 //
s1210160 7:209c07cc2b80 169 //GPS Send Command
s1210160 7:209c07cc2b80 170 //
s1210160 7:209c07cc2b80 171 /////////////////////////////////////////
s1210160 7:209c07cc2b80 172 void GPS_SendCommand(){
kityann 0:68d27eee9a6c 173 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:68d27eee9a6c 174 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:68d27eee9a6c 175 myGPS.sendCommand(PGCMD_ANTENNA);
s1210160 7:209c07cc2b80 176 }
kityann 0:68d27eee9a6c 177
s1210160 7:209c07cc2b80 178 /////////////////////////////////////////
s1210160 7:209c07cc2b80 179 //
s1210160 7:209c07cc2b80 180 //Check Command Type
s1210160 7:209c07cc2b80 181 //
s1210160 7:209c07cc2b80 182 /////////////////////////////////////////
s1210160 7:209c07cc2b80 183 void check_CommandType(utf8_t command_type, AigamozuControlPackets ai(motorShield), ){
s1210160 7:209c07cc2b80 184 switch(command_type){
kityann 0:68d27eee9a6c 185
kityann 0:68d27eee9a6c 186 //CommandType -> ChanegeMode
kityann 0:68d27eee9a6c 187 case CHANGE_MODE :{
kityann 0:68d27eee9a6c 188 agz.changeMode(buf);
kityann 0:68d27eee9a6c 189 break;
kityann 0:68d27eee9a6c 190 }
kityann 0:68d27eee9a6c 191
kityann 0:68d27eee9a6c 192 //CommandType -> Manual
kityann 0:68d27eee9a6c 193 case MANUAL:{
kityann 0:68d27eee9a6c 194 //Check now Mode
kityann 0:68d27eee9a6c 195 if(agz.nowMode == MANUAL_MODE){
kityann 0:68d27eee9a6c 196 agz.changeSpeed(buf);
kityann 0:68d27eee9a6c 197 }
kityann 0:68d27eee9a6c 198 break;
kityann 0:68d27eee9a6c 199 }
kityann 0:68d27eee9a6c 200
kityann 0:68d27eee9a6c 201 //CommandType -> Send Status
kityann 0:68d27eee9a6c 202 case STATUS_REQUEST:{
s1210160 7:209c07cc2b80 203
kityann 0:68d27eee9a6c 204 //send normal data
kityann 0:68d27eee9a6c 205 //Create GPS Infomation Packet
s1200058 4:480f4eb9bcbc 206 agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:68d27eee9a6c 207 //Select Destination
s1210160 7:209c07cc2b80 208 ZBTxRequest tx64request2(collect_Address[id],ai.packetData,ai.getPacketLength());
s1200058 4:480f4eb9bcbc 209 //Send -> Base
s1200058 4:480f4eb9bcbc 210 xbee.send(tx64request);
s1200058 4:480f4eb9bcbc 211
s1200058 4:480f4eb9bcbc 212 //send Kalman data
s1200058 4:480f4eb9bcbc 213 agz.createReceiveStatusCommandwithKalman(Base_ID,'a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
s1200058 4:480f4eb9bcbc 214 //Select Destination
s1200058 4:480f4eb9bcbc 215 ZBTxRequest tx64request3(collect_Address[id],agz.packetData,agz.getPacketLength());
s1200058 4:480f4eb9bcbc 216 //Send -> Base
s1200058 4:480f4eb9bcbc 217 xbee.send(tx64request);
kityann 0:68d27eee9a6c 218 break;
kityann 0:68d27eee9a6c 219
kityann 0:68d27eee9a6c 220 }
s1210160 7:209c07cc2b80 221
kityann 0:68d27eee9a6c 222 default:{
kityann 0:68d27eee9a6c 223 break;
kityann 0:68d27eee9a6c 224 }
kityann 0:68d27eee9a6c 225 }
kityann 0:68d27eee9a6c 226 }
kityann 0:68d27eee9a6c 227
s1210160 7:209c07cc2b80 228 /////////////////////////////////////////
s1210160 7:209c07cc2b80 229 //
s1210160 7:209c07cc2b80 230 //latitude and longitude after filtering
s1210160 7:209c07cc2b80 231 //
s1210160 7:209c07cc2b80 232 /////////////////////////////////////////
s1210160 7:209c07cc2b80 233 void LatLong_afterFiltering(double longitude, double latitude, double x0, double x1){
s1210160 7:209c07cc2b80 234 myGPS.longitudeKH=longitude;//longitude after filtering
s1210160 7:209c07cc2b80 235 myGPS.latitudeKH=latitude;//latitude after filtering
s1210160 7:209c07cc2b80 236 myGPS.longitudeKL=(long)x0;//longitude after filtering
s1210160 7:209c07cc2b80 237 myGPS.latitudeKL=(long)x1;//latitude after filtering
s1210160 7:209c07cc2b80 238 }
s1210160 7:209c07cc2b80 239
s1210160 7:209c07cc2b80 240 /////////////////////////////////////////
s1210160 7:209c07cc2b80 241 //
s1210160 7:209c07cc2b80 242 //send_GPSdata()
s1210160 7:209c07cc2b80 243 //
s1210160 7:209c07cc2b80 244 /////////////////////////////////////////
s1210160 7:209c07cc2b80 245 void send_GPSdata(){
s1210160 7:209c07cc2b80 246 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
s1210160 7:209c07cc2b80 247 xbee.getResponse().getZBRxResponse(zbRx);
s1210160 7:209c07cc2b80 248 uint8_t *buf = zbRx.getFrameData();
s1210160 7:209c07cc2b80 249 uint8_t *buf1 = &buf[11];
s1210160 7:209c07cc2b80 250 id = buf1[5] - 'A';+
s1210160 7:209c07cc2b80 251
s1210160 7:209c07cc2b80 252 //Check Command Type
s1210160 7:209c07cc2b80 253 check_CommandType(agz.checkCommnadType(buf));
kityann 0:68d27eee9a6c 254
s1210160 7:209c07cc2b80 255 }
s1210160 7:209c07cc2b80 256 }
s1210160 7:209c07cc2b80 257
s1210160 7:209c07cc2b80 258 /////////////////////////////////////////
s1210160 7:209c07cc2b80 259 //
s1210160 7:209c07cc2b80 260 //get_GPSdata
s1210160 7:209c07cc2b80 261 //
s1210160 7:209c07cc2b80 262 /////////////////////////////////////////
s1210160 7:209c07cc2b80 263 void get_GPSdata(){
s1210160 7:209c07cc2b80 264 if(flag){
s1210160 7:209c07cc2b80 265 if(myGPS.longitudeH==139 && myGPS.latitudeH==37){
s1210160 7:209c07cc2b80 266 flag = false;
s1210160 7:209c07cc2b80 267 x[0][0]=(double)myGPS.longitudeL;
s1210160 7:209c07cc2b80 268 x[0][1]=(double)myGPS.latitudeL;
s1210160 7:209c07cc2b80 269 }
s1210160 7:209c07cc2b80 270 }
s1210160 7:209c07cc2b80 271 if(myGPS.longitudeH!=139 || myGPS.latitudeH!=37) continue;
s1210160 7:209c07cc2b80 272 }
s1210160 7:209c07cc2b80 273
s1210160 7:209c07cc2b80 274 /////////////////////////////////////////
s1210160 7:209c07cc2b80 275 //
s1210160 7:209c07cc2b80 276 //get_Kalman
s1210160 7:209c07cc2b80 277 //
s1210160 7:209c07cc2b80 278 /////////////////////////////////////////
s1210160 7:209c07cc2b80 279 void get_Kalman(){
s1210160 7:209c07cc2b80 280 //Kalman Filter
s1210160 7:209c07cc2b80 281 Kalman(myGPS.longitudeL,myGPS.latitudeL);
s1210160 7:209c07cc2b80 282 //kousinn
s1210160 7:209c07cc2b80 283 for(int i = 0;i < 2;i++){
s1210160 7:209c07cc2b80 284 for(int j = 0;j < 2;j++){
s1210160 7:209c07cc2b80 285 K[0][i][j]=K[1][i][j];
s1210160 7:209c07cc2b80 286 x[0][i]=x[1][i];
s1210160 7:209c07cc2b80 287 sigma[0][i][j]=sigma[1][i][j];
s1210160 7:209c07cc2b80 288 }
s1210160 7:209c07cc2b80 289 }
s1210160 7:209c07cc2b80 290
s1210160 7:209c07cc2b80 291 LatLong_afterFiltering(myGPS.longitudeH, myGPS.latitudeH, x[1][0], x[1][1]);
s1210160 7:209c07cc2b80 292 }
s1210160 7:209c07cc2b80 293 /////////////////////////////////////////
s1210160 7:209c07cc2b80 294 //
s1210160 7:209c07cc2b80 295 //Main Processing
s1210160 7:209c07cc2b80 296 //
s1210160 7:209c07cc2b80 297 /////////////////////////////////////////
s1210160 7:209c07cc2b80 298 int main() {
s1210160 7:209c07cc2b80 299 setting();
s1210160 7:209c07cc2b80 300
s1210160 7:209c07cc2b80 301 //GPS setting
s1210160 7:209c07cc2b80 302 GPS_Setting(2000, 200);
s1210160 7:209c07cc2b80 303 int collect_flag = 0;
s1210160 7:209c07cc2b80 304 char collect_state = 'a';
s1210160 7:209c07cc2b80 305 int id;
s1210160 7:209c07cc2b80 306 bool flag = true;
s1210160 7:209c07cc2b80 307
s1210160 7:209c07cc2b80 308 //GPS Send Command
s1210160 7:209c07cc2b80 309 GPS_SendCommand();
s1210160 7:209c07cc2b80 310
s1210160 7:209c07cc2b80 311 wait(2);
s1210160 7:209c07cc2b80 312
s1210160 7:209c07cc2b80 313 //interrupt start
s1210160 7:209c07cc2b80 314 interrupt_start(agz_motorShield, refreshTime);
s1210160 7:209c07cc2b80 315
s1210160 7:209c07cc2b80 316 while (true) {
s1210160 7:209c07cc2b80 317
s1210160 7:209c07cc2b80 318 //Check Xbee Buffer Available
s1210160 7:209c07cc2b80 319 xbee.readPacket();
s1210160 7:209c07cc2b80 320 if (xbee.getResponse().isAvailable()) {
s1210160 7:209c07cc2b80 321 send_GPSdata();
s1210160 7:209c07cc2b80 322 }
s1210160 7:209c07cc2b80 323
kityann 0:68d27eee9a6c 324 myGPS.read();
kityann 0:68d27eee9a6c 325 //recive gps module
kityann 0:68d27eee9a6c 326 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:68d27eee9a6c 327 if ( myGPS.newNMEAreceived() ) {
kityann 0:68d27eee9a6c 328 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:68d27eee9a6c 329 continue;
kityann 0:68d27eee9a6c 330 }
kityann 0:68d27eee9a6c 331 }
kityann 0:68d27eee9a6c 332 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:68d27eee9a6c 333 refresh_Timer.reset();
kityann 0:68d27eee9a6c 334 if (myGPS.fix) {
kityann 0:68d27eee9a6c 335 agz.nowStatus = GPS_AVAIL;
kityann 0:68d27eee9a6c 336
s1210160 7:209c07cc2b80 337 get_GPSdata();
kityann 0:68d27eee9a6c 338
s1210160 7:209c07cc2b80 339 get_Kalman();
kityann 0:68d27eee9a6c 340
kityann 0:68d27eee9a6c 341 agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:68d27eee9a6c 342 agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
kityann 0:68d27eee9a6c 343 //printf("state = %d\n",agz.nowMode);
kityann 0:68d27eee9a6c 344 //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:68d27eee9a6c 345 }
kityann 0:68d27eee9a6c 346 else agz.nowStatus = GPS_UNAVAIL;
kityann 0:68d27eee9a6c 347
kityann 0:68d27eee9a6c 348 }
kityann 0:68d27eee9a6c 349
s1210160 7:209c07cc2b80 350
kityann 0:68d27eee9a6c 351 }
kityann 0:68d27eee9a6c 352 }