aigamozu / Mbed 2 deprecated GPSLOG_program

GPS精度実験

Dependencies:   ADXL345 AigamozuControlPackets_2016 HMC5843 ITG3200 MBed_Adafruit-GPS-Library2 XBee2 agzIDLIST_2016 mbed

main.cpp.orig@3:693ea9476763, 2016-10-14 (annotated)

Committer:
s1200058
Date:
Fri Oct 14 07:58:59 2016 +0000
Revision:
3:693ea9476763
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1200058 3:693ea9476763 1 /**********************************************/
s1200058 3:693ea9476763 2 //
s1200058 3:693ea9476763 3 //
s1200058 3:693ea9476763 4 //
s1200058 3:693ea9476763 5 // Program name: Aigamozu ROBOT
s1200058 3:693ea9476763 6 // Author: Mineta Kizuku
s1200058 3:693ea9476763 7 //
s1200058 3:693ea9476763 8 //
s1200058 3:693ea9476763 9 //
s1200058 3:693ea9476763 10 /**********************************************/
s1200058 3:693ea9476763 11
s1200058 3:693ea9476763 12 /**********************************************/
s1200058 3:693ea9476763 13 //更新情報
s1200058 3:693ea9476763 14 //2015/05/11
s1200058 3:693ea9476763 15 //ロボットプログラムの作成
s1200058 3:693ea9476763 16 //
s1200058 3:693ea9476763 17 //2015/05/13
s1200058 3:693ea9476763 18 //カルマンフィルタの共分散の値を0.0001以下にならないようにした
s1200058 3:693ea9476763 19 //共分散の値を10進数に変換するようにした
s1200058 3:693ea9476763 20 //
s1200058 3:693ea9476763 21 //2015/05/13 Yokokawa
s1200058 3:693ea9476763 22 //何回目のGPSでとられたGPSか確認するようにしました。
s1200058 3:693ea9476763 23 //
s1200058 3:693ea9476763 24 //2015/05/15
s1200058 3:693ea9476763 25 //プログラムcreateReceiveStatusCommand()にて
s1200058 3:693ea9476763 26 //Aigamozu_collect_dataにinかoutかを送るためにstate関連をいじったので必要に応じて直してください。
s1200058 3:693ea9476763 27 //
s1200058 3:693ea9476763 28 //2015/05//17
s1200058 3:693ea9476763 29 //Send_Status関数内を変更：基地局への送信データのみ現在のモードを入れるパケットの部分に内外判定の結果を入れるようにした
s1200058 3:693ea9476763 30 //基地局以外には現在のモードを送信するようにしてある
s1200058 3:693ea9476763 31 //要確認！！！！
s1200058 3:693ea9476763 32 //
s1200058 3:693ea9476763 33 //2015/05/17
s1200058 3:693ea9476763 34 //Get_GPS()の中身longitudeの範囲138〜140に変更
s1200058 3:693ea9476763 35 //
s1200058 3:693ea9476763 36 //2015/05/19
s1200058 3:693ea9476763 37 //autoモードのとき、三十秒前進・三秒右に転回に変更した。
s1200058 3:693ea9476763 38 //
s1200058 3:693ea9476763 39 //2015/05/24
s1200058 3:693ea9476763 40 //Kalmanフィルターを十進数で計算するようにした。
s1200058 3:693ea9476763 41 //Kalmanフィルターの計算式を変更した。
s1200058 3:693ea9476763 42 //set_kalmanを追加した。
s1200058 3:693ea9476763 43 //
s1200058 3:693ea9476763 44 /**********************************************/
s1200058 3:693ea9476763 45
s1200058 3:693ea9476763 46 #include "mbed.h"
s1200058 3:693ea9476763 47 #include "XBee.h"
s1200058 3:693ea9476763 48 #include "MBed_Adafruit_GPS.h"
s1200058 3:693ea9476763 49 #include "AigamozuControlPackets.h"
s1200058 3:693ea9476763 50 #include "agzIDLIST.h"
s1200058 3:693ea9476763 51 #include "aigamozuSetting.h"
s1200058 3:693ea9476763 52 #include "Kalman.h"
s1200058 3:693ea9476763 53 #include "math.h"
s1200058 3:693ea9476763 54
s1200058 3:693ea9476763 55 #define SIGMA_MIN 0.0001
s1200058 3:693ea9476763 56 #define KALMAN_START_TIME 5
s1200058 3:693ea9476763 57
s1200058 3:693ea9476763 58 //************ID Number*****************
s1200058 3:693ea9476763 59 //Robot ID: 'A' ~ 'Z'
s1200058 3:693ea9476763 60 //Base ID: 'a' ~ 'z'
s1200058 3:693ea9476763 61 //manager ID: '0'(数字のゼロ)
s1200058 3:693ea9476763 62 //
s1200058 3:693ea9476763 63 const char MyID = 'B';
s1200058 3:693ea9476763 64 const char Collect_Number = 'a';
s1200058 3:693ea9476763 65 //************ID Number*****************
s1200058 3:693ea9476763 66
s1200058 3:693ea9476763 67 /////////////////////////////////////////
s1200058 3:693ea9476763 68 //
s1200058 3:693ea9476763 69 //Pin Setting
s1200058 3:693ea9476763 70 //
s1200058 3:693ea9476763 71 /////////////////////////////////////////
s1200058 3:693ea9476763 72 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
s1200058 3:693ea9476763 73
s1200058 3:693ea9476763 74
s1200058 3:693ea9476763 75 /////////////////////////////////////////
s1200058 3:693ea9476763 76 //
s1200058 3:693ea9476763 77 //Connection Setting
s1200058 3:693ea9476763 78 //
s1200058 3:693ea9476763 79 /////////////////////////////////////////
s1200058 3:693ea9476763 80
s1200058 3:693ea9476763 81 //Serial Connect Setting: PC <--> mbed
s1200058 3:693ea9476763 82 Serial pc(USBTX, USBRX);
s1200058 3:693ea9476763 83
s1200058 3:693ea9476763 84 //Serial Connect Setting: GPS <--> mbed
s1200058 3:693ea9476763 85 Serial * gps_Serial;
s1200058 3:693ea9476763 86
s1200058 3:693ea9476763 87 //Serial Connect Setting: XBEE <--> mbed
s1200058 3:693ea9476763 88 XBee xbee(p13,p14);
s1200058 3:693ea9476763 89 ZBRxResponse zbRx = ZBRxResponse();
s1200058 3:693ea9476763 90
s1200058 3:693ea9476763 91 //set up GPS module
s1200058 3:693ea9476763 92
s1200058 3:693ea9476763 93 //set up AigamozuControlPackets library
s1200058 3:693ea9476763 94 AigamozuControlPackets agz(agz_motorShield);
s1200058 3:693ea9476763 95
s1200058 3:693ea9476763 96
s1200058 3:693ea9476763 97 /////////////////////////////////////////
s1200058 3:693ea9476763 98 //
s1200058 3:693ea9476763 99 //For Kalman data
s1200058 3:693ea9476763 100 //
s1200058 3:693ea9476763 101 /////////////////////////////////////////
s1200058 3:693ea9476763 102 #define FIRST_S1 0.0001
s1200058 3:693ea9476763 103 #define FIRST_S2 0.000001
s1200058 3:693ea9476763 104 #define COUNTER_MAX 10000
s1200058 3:693ea9476763 105 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
s1200058 3:693ea9476763 106 double s2x_cur=FIRST_S1,s2x_prev=FIRST_S1,s2y_cur=FIRST_S1,s2y_prev=FIRST_S1;//緯度経度のの時刻tと時刻t-1での共分散
s1200058 3:693ea9476763 107 double s2_R=FIRST_S2;//GPSセンサの分散
s1200058 3:693ea9476763 108 double Kx=0,Ky=0;//カルマンゲイン
s1200058 3:693ea9476763 109 double zx,zy;//観測値
s1200058 3:693ea9476763 110 void Kalman(double Latitude,double Longitude);
s1200058 3:693ea9476763 111 int change = 0;
s1200058 3:693ea9476763 112
s1200058 3:693ea9476763 113 /*
s1200058 3:693ea9476763 114 LocalFileSystem local("local"); // マウントポイントを定義（ディレクトリパスになる）
s1200058 3:693ea9476763 115 FILE *fp;
s1200058 3:693ea9476763 116 char filename[16] = "/local/out0.txt";
s1200058 3:693ea9476763 117 */
s1200058 3:693ea9476763 118 /////////////////////////////////////////
s1200058 3:693ea9476763 119 //
s1200058 3:693ea9476763 120 //Plus Speed
s1200058 3:693ea9476763 121 //
s1200058 3:693ea9476763 122 //MNUAL_MODEの時にスピードを変える
s1200058 3:693ea9476763 123 /////////////////////////////////////////
s1200058 3:693ea9476763 124 void Plus_Speed(uint8_t *packetdata)
s1200058 3:693ea9476763 125 {
s1200058 3:693ea9476763 126
s1200058 3:693ea9476763 127 if(agz.nowMode == MANUAL_MODE) {
s1200058 3:693ea9476763 128 agz.changeSpeed(packetdata);
s1200058 3:693ea9476763 129 }
s1200058 3:693ea9476763 130
s1200058 3:693ea9476763 131 }
s1200058 3:693ea9476763 132
s1200058 3:693ea9476763 133
s1200058 3:693ea9476763 134 /////////////////////////////////////////
s1200058 3:693ea9476763 135 //
s1200058 3:693ea9476763 136 //Send Status
s1200058 3:693ea9476763 137 //
s1200058 3:693ea9476763 138 //リクエストがきたとき、自分の位置情報などを返信する
s1200058 3:693ea9476763 139 /////////////////////////////////////////
s1200058 3:693ea9476763 140 void Send_Status(char SenderIDc)
s1200058 3:693ea9476763 141 {
s1200058 3:693ea9476763 142 XBeeAddress64 send_Address;
s1200058 3:693ea9476763 143 if(SenderIDc == '0') {
s1200058 3:693ea9476763 144 send_Address = manager_Address;
s1200058 3:693ea9476763 145 agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
s1200058 3:693ea9476763 146 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1200058 3:693ea9476763 147 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1200058 3:693ea9476763 148 agz.get_agzCov_lati(),agz.get_agzCov_longi());
s1200058 3:693ea9476763 149 }
s1200058 3:693ea9476763 150 if(SenderIDc >= 'A' && SenderIDc <= 'Z') {
s1200058 3:693ea9476763 151 send_Address = robot_Address[SenderIDc - 'A'];
s1200058 3:693ea9476763 152 //Create GPS Infomation Packet
s1200058 3:693ea9476763 153 agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
s1200058 3:693ea9476763 154 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1200058 3:693ea9476763 155 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1200058 3:693ea9476763 156 agz.get_agzCov_lati(),agz.get_agzCov_longi());
s1200058 3:693ea9476763 157 }
s1200058 3:693ea9476763 158 if(SenderIDc >= 'a' && SenderIDc <= 'z') {
s1200058 3:693ea9476763 159 send_Address = base_Address[SenderIDc - 'a'];
s1200058 3:693ea9476763 160
s1200058 3:693ea9476763 161 agz.createReceiveStatusCommand(MyID,SenderIDc, (int)agz.gpsAuto(),
s1200058 3:693ea9476763 162 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1200058 3:693ea9476763 163 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1200058 3:693ea9476763 164 agz.get_agzCov_lati(),agz.get_agzCov_longi());
s1200058 3:693ea9476763 165 }
s1200058 3:693ea9476763 166 //send normal data
s1200058 3:693ea9476763 167
s1200058 3:693ea9476763 168 /*
s1200058 3:693ea9476763 169 //debug***************************************************
s1200058 3:693ea9476763 170 printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
s1200058 3:693ea9476763 171 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1200058 3:693ea9476763 172 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1200058 3:693ea9476763 173 agz.get_agzCov_lati(),agz.get_agzCov_longi()
s1200058 3:693ea9476763 174 );
s1200058 3:693ea9476763 175 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
s1200058 3:693ea9476763 176 printf("\n");
s1200058 3:693ea9476763 177 //debug end***************************************************
s1200058 3:693ea9476763 178 */
s1200058 3:693ea9476763 179 //Select Destination
s1200058 3:693ea9476763 180 ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
s1200058 3:693ea9476763 181 //Send -> Base
s1200058 3:693ea9476763 182 xbee.send(tx64request);
s1200058 3:693ea9476763 183
s1200058 3:693ea9476763 184 }
s1200058 3:693ea9476763 185
s1200058 3:693ea9476763 186 /////////////////////////////////////////
s1200058 3:693ea9476763 187 //
s1200058 3:693ea9476763 188 //Get GPS function
s1200058 3:693ea9476763 189 //
s1200058 3:693ea9476763 190 /////////////////////////////////////////
s1200058 3:693ea9476763 191
s1200058 3:693ea9476763 192 void Get_GPS(Adafruit_GPS *myGPS)
s1200058 3:693ea9476763 193 {
s1200058 3:693ea9476763 194 static int flag = 0;
s1200058 3:693ea9476763 195 // static int save_counter = 0;
s1200058 3:693ea9476763 196
s1200058 3:693ea9476763 197 if (myGPS->fix) {
s1200058 3:693ea9476763 198
s1200058 3:693ea9476763 199 agz.nowStatus = GPS_AVAIL;
s1200058 3:693ea9476763 200
s1200058 3:693ea9476763 201 /* if(save_counter < 10){
s1200058 3:693ea9476763 202 fp = fopen(filename, "a");
s1200058 3:693ea9476763 203 fprintf(fp, "%d %.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
s1200058 3:693ea9476763 204 change, agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1200058 3:693ea9476763 205 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1200058 3:693ea9476763 206 agz.get_agzCov_lati(),agz.get_agzCov_longi());
s1200058 3:693ea9476763 207 fclose(fp);
s1200058 3:693ea9476763 208
s1200058 3:693ea9476763 209 if((flag - 16) % 500 == 0){
s1200058 3:693ea9476763 210 filename[10]++;
s1200058 3:693ea9476763 211 save_counter++;
s1200058 3:693ea9476763 212 }
s1200058 3:693ea9476763 213 }
s1200058 3:693ea9476763 214 */
s1200058 3:693ea9476763 215
s1200058 3:693ea9476763 216 } else agz.nowStatus = GPS_UNAVAIL;
s1200058 3:693ea9476763 217
s1200058 3:693ea9476763 218 }
s1200058 3:693ea9476763 219
s1200058 3:693ea9476763 220 /////////////////////////////////////////
s1200058 3:693ea9476763 221 //
s1200058 3:693ea9476763 222 //New Mode
s1200058 3:693ea9476763 223 //
s1200058 3:693ea9476763 224 /////////////////////////////////////////
s1200058 3:693ea9476763 225
s1200058 3:693ea9476763 226 void New_Mode(uint8_t *packetdata)
s1200058 3:693ea9476763 227 {
s1200058 3:693ea9476763 228
s1200058 3:693ea9476763 229 //bool result;
s1200058 3:693ea9476763 230 agz.changeMode(packetdata);
s1200058 3:693ea9476763 231
s1200058 3:693ea9476763 232 }
s1200058 3:693ea9476763 233
s1200058 3:693ea9476763 234 /////////////////////////////////////////
s1200058 3:693ea9476763 235 //
s1200058 3:693ea9476763 236 //Get Status
s1200058 3:693ea9476763 237 //
s1200058 3:693ea9476763 238 /////////////////////////////////////////
s1200058 3:693ea9476763 239 void Get_Status(char SenderIDc,uint8_t *packetdata)
s1200058 3:693ea9476763 240 {
s1200058 3:693ea9476763 241 int id = 0;
s1200058 3:693ea9476763 242
s1200058 3:693ea9476763 243 //printf("Get data\n");
s1200058 3:693ea9476763 244 agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
s1200058 3:693ea9476763 245 agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
s1200058 3:693ea9476763 246 agz.set_agzCov_from_packet(&packetdata[45],&packetdata[53]);
s1200058 3:693ea9476763 247
s1200058 3:693ea9476763 248
s1200058 3:693ea9476763 249 printf("%d, %f, %f\n",
s1200058 3:693ea9476763 250 SenderIDc, agz.get_basePoint_lati(id),agz.get_basePoint_longi(id)
s1200058 3:693ea9476763 251 );
s1200058 3:693ea9476763 252
s1200058 3:693ea9476763 253
s1200058 3:693ea9476763 254 }
s1200058 3:693ea9476763 255
s1200058 3:693ea9476763 256
s1200058 3:693ea9476763 257 /////////////////////////////////////////
s1200058 3:693ea9476763 258 //
s1200058 3:693ea9476763 259 //Send_Request
s1200058 3:693ea9476763 260 //
s1200058 3:693ea9476763 261 /////////////////////////////////////////
s1200058 3:693ea9476763 262 void Send_Request(int number)
s1200058 3:693ea9476763 263 {
s1200058 3:693ea9476763 264
s1200058 3:693ea9476763 265 agz.createRequestCommand(MyID, number);
s1200058 3:693ea9476763 266 //Select Destination!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
s1200058 3:693ea9476763 267 //基地局の場合
s1200058 3:693ea9476763 268 if('a' <= number && number <= 'z'){
s1200058 3:693ea9476763 269 number = number - 'a';
s1200058 3:693ea9476763 270 //printf("Send b:%d\n",number);
s1200058 3:693ea9476763 271 ZBTxRequest tx64request(base_Address[number],agz.packetData,agz.getPacketLength());
s1200058 3:693ea9476763 272 //Send
s1200058 3:693ea9476763 273 xbee.send(tx64request);
s1200058 3:693ea9476763 274 }
s1200058 3:693ea9476763 275 //ロボットの場合
s1200058 3:693ea9476763 276 if('A' <= number && number <= 'Z'){
s1200058 3:693ea9476763 277 number = number - 'A';
s1200058 3:693ea9476763 278 //printf("Send r:%d\n",MyID);
s1200058 3:693ea9476763 279 ZBTxRequest tx64request(robot_Address[number],agz.packetData,agz.getPacketLength());
s1200058 3:693ea9476763 280 //Send
s1200058 3:693ea9476763 281 xbee.send(tx64request);
s1200058 3:693ea9476763 282 }
s1200058 3:693ea9476763 283
s1200058 3:693ea9476763 284 }
s1200058 3:693ea9476763 285
s1200058 3:693ea9476763 286
s1200058 3:693ea9476763 287 void print_gps(int count)
s1200058 3:693ea9476763 288 {
s1200058 3:693ea9476763 289
s1200058 3:693ea9476763 290 printf("%d times:\n", count);
s1200058 3:693ea9476763 291 printf("%f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
s1200058 3:693ea9476763 292
s1200058 3:693ea9476763 293 }
s1200058 3:693ea9476763 294
s1200058 3:693ea9476763 295
s1200058 3:693ea9476763 296 /////////////////////////////////////////
s1200058 3:693ea9476763 297 //
s1200058 3:693ea9476763 298 //Kalman Processing
s1200058 3:693ea9476763 299 //
s1200058 3:693ea9476763 300 /////////////////////////////////////////
s1200058 3:693ea9476763 301 void calc_Kalman()
s1200058 3:693ea9476763 302 {
s1200058 3:693ea9476763 303 //calc Kalman gain
s1200058 3:693ea9476763 304 Kx = s2x_prev/(s2x_prev+s2_R);
s1200058 3:693ea9476763 305 Ky = s2y_prev/(s2y_prev+s2_R);
s1200058 3:693ea9476763 306 //estimate
s1200058 3:693ea9476763 307 x_cur = x_prev + Kx*(zx-x_prev);
s1200058 3:693ea9476763 308 y_cur = y_prev + Ky*(zy-y_prev);
s1200058 3:693ea9476763 309 //calc sigma
s1200058 3:693ea9476763 310 s2x_cur = s2x_prev-Kx*s2x_prev;
s1200058 3:693ea9476763 311 s2y_cur = s2y_prev-Ky*s2y_prev;
s1200058 3:693ea9476763 312
s1200058 3:693ea9476763 313 }
s1200058 3:693ea9476763 314
s1200058 3:693ea9476763 315 void Kalman(double Latitude,double Longitude)
s1200058 3:693ea9476763 316 {
s1200058 3:693ea9476763 317
s1200058 3:693ea9476763 318 zx = Latitude;
s1200058 3:693ea9476763 319 zy = Longitude;
s1200058 3:693ea9476763 320
s1200058 3:693ea9476763 321 calc_Kalman();
s1200058 3:693ea9476763 322
s1200058 3:693ea9476763 323 //更新
s1200058 3:693ea9476763 324 x_prev = x_cur;
s1200058 3:693ea9476763 325 y_prev = y_cur;
s1200058 3:693ea9476763 326 s2x_prev = s2x_cur;
s1200058 3:693ea9476763 327 s2y_prev = s2y_cur;
s1200058 3:693ea9476763 328
s1200058 3:693ea9476763 329 //agzPontKalmanとagzCovに格納する
s1200058 3:693ea9476763 330 agz.set_agzPointKalman_lati(x_cur);
s1200058 3:693ea9476763 331 agz.set_agzPointKalman_longi(y_cur);
s1200058 3:693ea9476763 332 agz.set_agzCov(s2x_cur,s2y_cur);
s1200058 3:693ea9476763 333
s1200058 3:693ea9476763 334 }
s1200058 3:693ea9476763 335
s1200058 3:693ea9476763 336 /////////////////////////////////////////
s1200058 3:693ea9476763 337 //
s1200058 3:693ea9476763 338 //Main Processing
s1200058 3:693ea9476763 339 //
s1200058 3:693ea9476763 340 /////////////////////////////////////////
s1200058 3:693ea9476763 341 int main()
s1200058 3:693ea9476763 342 {
s1200058 3:693ea9476763 343 //start up time
s1200058 3:693ea9476763 344 wait(3);
s1200058 3:693ea9476763 345 //set pc frequency to 57600bps
s1200058 3:693ea9476763 346 pc.baud(PC_BAUD_RATE);
s1200058 3:693ea9476763 347 //set xbee frequency to 57600bps
s1200058 3:693ea9476763 348 xbee.begin(XBEE_BAUD_RATE);
s1200058 3:693ea9476763 349
s1200058 3:693ea9476763 350
s1200058 3:693ea9476763 351 //GPS setting
s1200058 3:693ea9476763 352 gps_Serial = new Serial(p28,p27);
s1200058 3:693ea9476763 353 Adafruit_GPS myGPS(gps_Serial);
s1200058 3:693ea9476763 354 Timer refresh_Timer;
s1200058 3:693ea9476763 355 Timer auto_Timer;
s1200058 3:693ea9476763 356 // const int straight = 8000, turning = 12000, wait = 10000;
s1200058 3:693ea9476763 357
s1200058 3:693ea9476763 358 myGPS.begin(GPS_BAUD_RATE);
s1200058 3:693ea9476763 359
s1200058 3:693ea9476763 360 Timer collect_Timer;
s1200058 3:693ea9476763 361 const int collect_Time = 1000;
s1200058 3:693ea9476763 362
s1200058 3:693ea9476763 363 char SenderIDc;
s1200058 3:693ea9476763 364 //GPS Send Command
s1200058 3:693ea9476763 365 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
s1200058 3:693ea9476763 366 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
s1200058 3:693ea9476763 367 myGPS.sendCommand(PGCMD_ANTENNA);
s1200058 3:693ea9476763 368 wait_ms(2000);
s1200058 3:693ea9476763 369
s1200058 3:693ea9476763 370 //interrupt start
s1200058 3:693ea9476763 371 refresh_Timer.start();
s1200058 3:693ea9476763 372 auto_Timer.start();
s1200058 3:693ea9476763 373 agz.Move_Timer.start();
s1200058 3:693ea9476763 374 collect_Timer.start();
s1200058 3:693ea9476763 375 agz.Automove_Timer.start();
s1200058 3:693ea9476763 376 printf("start\n");
s1200058 3:693ea9476763 377
s1200058 3:693ea9476763 378 agz.auto_count = 0;
s1200058 3:693ea9476763 379
s1200058 3:693ea9476763 380 while (true) {
s1200058 3:693ea9476763 381
s1200058 3:693ea9476763 382 //Check Xbee Buffer Available
s1200058 3:693ea9476763 383 xbee.readPacket();
s1200058 3:693ea9476763 384
s1200058 3:693ea9476763 385 if (xbee.getResponse().isAvailable()) {
s1200058 3:693ea9476763 386 xbee.getResponse().getZBRxResponse(zbRx);
s1200058 3:693ea9476763 387 uint8_t *buf = zbRx.getFrameData();
s1200058 3:693ea9476763 388
s1200058 3:693ea9476763 389 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
s1200058 3:693ea9476763 390 xbee.getResponse().getZBRxResponse(zbRx);
s1200058 3:693ea9476763 391 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
s1200058 3:693ea9476763 392 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
s1200058 3:693ea9476763 393 SenderIDc = buf1[5];//送信元のIDを取得する
s1200058 3:693ea9476763 394 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
s1200058 3:693ea9476763 395
s1200058 3:693ea9476763 396 //Check Command Type
s1200058 3:693ea9476763 397 switch(Command_type) {
s1200058 3:693ea9476763 398 //Get Request command
s1200058 3:693ea9476763 399 case MANUAL: {
s1200058 3:693ea9476763 400 Plus_Speed(buf);
s1200058 3:693ea9476763 401 break;
s1200058 3:693ea9476763 402 }
s1200058 3:693ea9476763 403 case STATUS_REQUEST: {
s1200058 3:693ea9476763 404 Send_Status(SenderIDc);
s1200058 3:693ea9476763 405 //printf("%c\n", SenderIDc);
s1200058 3:693ea9476763 406 break;
s1200058 3:693ea9476763 407 }
s1200058 3:693ea9476763 408 case CHANGE_MODE: {
s1200058 3:693ea9476763 409 New_Mode(buf);
s1200058 3:693ea9476763 410 break;
s1200058 3:693ea9476763 411 }
s1200058 3:693ea9476763 412 case RECEIVE_STATUS: {
s1200058 3:693ea9476763 413 //printf("GET");
s1200058 3:693ea9476763 414 Get_Status(SenderIDc,buf1);
s1200058 3:693ea9476763 415 break;
s1200058 3:693ea9476763 416 }
s1200058 3:693ea9476763 417 default: {
s1200058 3:693ea9476763 418 break;
s1200058 3:693ea9476763 419 }
s1200058 3:693ea9476763 420 }//endswitch
s1200058 3:693ea9476763 421 }//endifZB_RX_RESPONSE
s1200058 3:693ea9476763 422 }//endifisAvailable
s1200058 3:693ea9476763 423
s1200058 3:693ea9476763 424 myGPS.read();
s1200058 3:693ea9476763 425 //recive gps module
s1200058 3:693ea9476763 426 //check if we recieved a new message from GPS, if so, attempt to parse it,
s1200058 3:693ea9476763 427 if ( myGPS.newNMEAreceived() ) {
s1200058 3:693ea9476763 428 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
s1200058 3:693ea9476763 429 continue;
s1200058 3:693ea9476763 430 }else{
s1200058 3:693ea9476763 431
s1200058 3:693ea9476763 432 }
s1200058 3:693ea9476763 433 if(strstr(myGPS.lastNMEA(), "$GPGSA")){
s1200058 3:693ea9476763 434 //myGPS.GPGSAdata=myGPS.lastNMEA();
s1200058 3:693ea9476763 435 //printf("%s",myGPS.GPGSAdata);
s1200058 3:693ea9476763 436 }
s1200058 3:693ea9476763 437 if(strstr(myGPS.lastNMEA(), "$GPGGA")){
s1200058 3:693ea9476763 438 //myGPS.GPGGAdata=myGPS.lastNMEA();
s1200058 3:693ea9476763 439 //printf("%s",myGPS.GPGGAdata);
s1200058 3:693ea9476763 440 }
s1200058 3:693ea9476763 441 if(strstr(myGPS.lastNMEA(), "$GPRMC")){
s1200058 3:693ea9476763 442 //myGPS.GPRMCdata=myGPS.lastNMEA();
s1200058 3:693ea9476763 443 //printf("%s",myGPS.GPRMCdata);
s1200058 3:693ea9476763 444 }
s1200058 3:693ea9476763 445 if(strstr(myGPS.lastNMEA(), "$GPGSV")){
s1200058 3:693ea9476763 446 //myGPS.GPRMCdata=myGPS.lastNMEA();
s1200058 3:693ea9476763 447 //printf("%s",myGPS.GPRMCdata);
s1200058 3:693ea9476763 448 }
s1200058 3:693ea9476763 449 }
s1200058 3:693ea9476763 450
s1200058 3:693ea9476763 451
s1200058 3:693ea9476763 452 //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
s1200058 3:693ea9476763 453 if(refresh_Timer.read_ms() >= 3000 && myGPS.print_ok == 1){
s1200058 3:693ea9476763 454 printf("%s\n",myGPS.GPGGAdata);
s1200058 3:693ea9476763 455 printf("%s\n",myGPS.GPGSAdata);
s1200058 3:693ea9476763 456 printf("%s\n",myGPS.GPRMCdata);
s1200058 3:693ea9476763 457 printf("%s\n",myGPS.GPGSVdataA);
s1200058 3:693ea9476763 458 printf("%s\n",myGPS.GPGSVdataB);
s1200058 3:693ea9476763 459 printf("%s\n",myGPS.GPGSVdataC);
s1200058 3:693ea9476763 460 printf("%s\n",myGPS.GPGSVdataD);
s1200058 3:693ea9476763 461 refresh_Timer.reset();
s1200058 3:693ea9476763 462 }
s1200058 3:693ea9476763 463
s1200058 3:693ea9476763 464
s1200058 3:693ea9476763 465 }
s1200058 3:693ea9476763 466
s1200058 3:693ea9476763 467 }