aigamozu / Mbed 2 deprecated GPSLOG_program

GPS精度実験

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

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

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

Who changed what in which revision?

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