aigamozu / Mbed 2 deprecated Aigamozu_Robot_ver3_3

2015/05/17

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

Fork of Aigamozu_Robot_ver3_2 by aigamozu

main.cpp@20:eee8ac8d1788, 2015-05-16 (annotated)

Committer:
s1200058
Date:
Sat May 16 14:08:27 2015 +0000
Revision:
20:eee8ac8d1788
Parent:
19:a806105ba365
Child:
21:76e7f36df4a9
2015/05/16

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
kityann 0:daab5accfd83 7 //
kityann 0:daab5accfd83 8 //
kityann 0:daab5accfd83 9 /**********************************************/
kityann 0:daab5accfd83 10
kityann 0:daab5accfd83 11 /**********************************************/
kityann 0:daab5accfd83 12 //更新情報
kityann 0:daab5accfd83 13 //2015/05/11
kityann 0:daab5accfd83 14 //ロボットプログラムの作成
kityann 0:daab5accfd83 15 //
kityann 4:f36986ceb73d 16 //2015/05/13
kityann 4:f36986ceb73d 17 //カルマンフィルタの共分散の値を0.0001以下にならないようにした
kityann 4:f36986ceb73d 18 //共分散の値を10進数に変換するようにした
kityann 0:daab5accfd83 19 //
s1200058 6:13f212b75e71 20 //2015/05/13
s1200058 6:13f212b75e71 21 //何回目のGPSでとられたGPSか確認するようにしました。
s1200058 6:13f212b75e71 22 //
s1200058 17:e563cf8e6bcf 23 //2015/05/15
s1200058 17:e563cf8e6bcf 24 //プログラムcreateReceiveStatusCommand()にて
s1200058 17:e563cf8e6bcf 25 //Aigamozu_collect_dataにinかoutかを送るためにstate関連をいじったので必要に応じて直してください。
s1200058 17:e563cf8e6bcf 26 //
kityann 0:daab5accfd83 27 /**********************************************/
kityann 0:daab5accfd83 28
kityann 0:daab5accfd83 29 #include "mbed.h"
kityann 0:daab5accfd83 30 #include "XBee.h"
kityann 0:daab5accfd83 31 #include "MBed_Adafruit_GPS.h"
kityann 0:daab5accfd83 32 #include "AigamozuControlPackets.h"
kityann 0:daab5accfd83 33 #include "agzIDLIST.h"
kityann 0:daab5accfd83 34 #include "aigamozuSetting.h"
kityann 0:daab5accfd83 35 #include "Kalman.h"
kityann 0:daab5accfd83 36
kityann 4:f36986ceb73d 37 #define SIGMA_MIN 0.0001
kityann 4:f36986ceb73d 38
kityann 0:daab5accfd83 39 //************ID Number*****************
kityann 0:daab5accfd83 40 //Robot ID: 'A' ~ 'Z'
kityann 0:daab5accfd83 41 //Base ID: 'a' ~ 'a'
kityann 0:daab5accfd83 42 //manager ID: '0'(数字のゼロ)
kityann 0:daab5accfd83 43 //
s1200058 20:eee8ac8d1788 44 const char MyID = 'B';
kityann 0:daab5accfd83 45 //************ID Number*****************
kityann 0:daab5accfd83 46
kityann 0:daab5accfd83 47 /////////////////////////////////////////
kityann 0:daab5accfd83 48 //
kityann 0:daab5accfd83 49 //Pin Setting
kityann 0:daab5accfd83 50 //
kityann 0:daab5accfd83 51 /////////////////////////////////////////
kityann 0:daab5accfd83 52 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:daab5accfd83 53
kityann 0:daab5accfd83 54
kityann 0:daab5accfd83 55 /////////////////////////////////////////
kityann 0:daab5accfd83 56 //
kityann 0:daab5accfd83 57 //Connection Setting
kityann 0:daab5accfd83 58 //
kityann 0:daab5accfd83 59 /////////////////////////////////////////
kityann 0:daab5accfd83 60
kityann 0:daab5accfd83 61 //Serial Connect Setting: PC <--> mbed
kityann 0:daab5accfd83 62 Serial pc(USBTX, USBRX);
kityann 0:daab5accfd83 63
kityann 0:daab5accfd83 64 //Serial Connect Setting: GPS <--> mbed
kityann 0:daab5accfd83 65 Serial * gps_Serial;
kityann 0:daab5accfd83 66
kityann 0:daab5accfd83 67 //Serial Connect Setting: XBEE <--> mbed
kityann 0:daab5accfd83 68 XBee xbee(p13,p14);
kityann 0:daab5accfd83 69 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:daab5accfd83 70
kityann 0:daab5accfd83 71 //set up GPS module
kityann 0:daab5accfd83 72
kityann 0:daab5accfd83 73 //set up AigamozuControlPackets library
kityann 0:daab5accfd83 74 AigamozuControlPackets agz(agz_motorShield);
kityann 0:daab5accfd83 75
kityann 0:daab5accfd83 76
kityann 0:daab5accfd83 77 /////////////////////////////////////////
kityann 0:daab5accfd83 78 //
kityann 0:daab5accfd83 79 //For Kalman data
kityann 0:daab5accfd83 80 //
kityann 0:daab5accfd83 81 /////////////////////////////////////////
kityann 0:daab5accfd83 82 double sigmaGPS[2][2] = {{250,0},{0,250}};
kityann 0:daab5accfd83 83 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
kityann 0:daab5accfd83 84 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
kityann 0:daab5accfd83 85 double y[2],x[2][2]={0};
kityann 0:daab5accfd83 86 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS);
kityann 0:daab5accfd83 87
kityann 0:daab5accfd83 88
s1200058 2:886fac7f4399 89 /////////////////////////////////////////
s1200058 2:886fac7f4399 90 //
s1200058 2:886fac7f4399 91 //Plus Speed
s1200058 2:886fac7f4399 92 //
s1200058 2:886fac7f4399 93 //MNUAL_MODEの時にスピードを変える
s1200058 2:886fac7f4399 94 /////////////////////////////////////////
s1200058 2:886fac7f4399 95 void Plus_Speed(uint8_t *packetdata){
s1200058 2:886fac7f4399 96
s1200058 2:886fac7f4399 97 if(agz.nowMode == MANUAL_MODE){
s1200058 2:886fac7f4399 98 agz.changeSpeed(packetdata);
s1200058 2:886fac7f4399 99 }
s1200058 2:886fac7f4399 100
s1200058 2:886fac7f4399 101 }
kityann 0:daab5accfd83 102
kityann 0:daab5accfd83 103 /////////////////////////////////////////
kityann 0:daab5accfd83 104 //
s1200058 2:886fac7f4399 105 //Send Status
kityann 0:daab5accfd83 106 //
kityann 0:daab5accfd83 107 //リクエストがきたとき、自分の位置情報などを返信する
kityann 0:daab5accfd83 108 /////////////////////////////////////////
kityann 0:daab5accfd83 109 void Send_Status(char SenderIDc){
kityann 0:daab5accfd83 110 XBeeAddress64 send_Address;
kityann 0:daab5accfd83 111 if(SenderIDc == '0'){
kityann 0:daab5accfd83 112 send_Address = manager_Address;
kityann 0:daab5accfd83 113 }
kityann 0:daab5accfd83 114 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
kityann 0:daab5accfd83 115 send_Address = robot_Address[SenderIDc - 'A'];
kityann 0:daab5accfd83 116 }
kityann 0:daab5accfd83 117 if(SenderIDc >= 'a' && SenderIDc <= 'z'){
kityann 0:daab5accfd83 118 send_Address = base_Address[SenderIDc - 'a'];
kityann 0:daab5accfd83 119 }
kityann 0:daab5accfd83 120 //send normal data
kityann 0:daab5accfd83 121 //Create GPS Infomation Packet
s1200058 18:a817972b89cd 122 agz.createReceiveStatusCommand(MyID,SenderIDc, (int)agz.gpsAuto(),
kityann 0:daab5accfd83 123 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 0:daab5accfd83 124 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 0:daab5accfd83 125 agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 0:daab5accfd83 126
kityann 0:daab5accfd83 127 //debug***************************************************
kityann 0:daab5accfd83 128 printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
kityann 0:daab5accfd83 129 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 0:daab5accfd83 130 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 0:daab5accfd83 131 agz.get_agzCov_lati(),agz.get_agzCov_longi()
kityann 0:daab5accfd83 132 );
kityann 0:daab5accfd83 133 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
kityann 0:daab5accfd83 134 printf("\n");
kityann 0:daab5accfd83 135 //debug end***************************************************
kityann 0:daab5accfd83 136
kityann 0:daab5accfd83 137 //Select Destination
kityann 0:daab5accfd83 138 ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
kityann 0:daab5accfd83 139 //Send -> Base
kityann 0:daab5accfd83 140 xbee.send(tx64request);
s1200058 20:eee8ac8d1788 141
kityann 0:daab5accfd83 142 }
kityann 0:daab5accfd83 143
kityann 0:daab5accfd83 144 /////////////////////////////////////////
kityann 0:daab5accfd83 145 //
kityann 0:daab5accfd83 146 //Get GPS function
kityann 0:daab5accfd83 147 //
kityann 0:daab5accfd83 148 /////////////////////////////////////////
kityann 0:daab5accfd83 149
kityann 0:daab5accfd83 150 void Get_GPS(Adafruit_GPS *myGPS){
kityann 0:daab5accfd83 151 static bool flag = true;
kityann 0:daab5accfd83 152
kityann 0:daab5accfd83 153 if (myGPS->fix) {
kityann 0:daab5accfd83 154 agz.nowStatus = GPS_AVAIL;
kityann 0:daab5accfd83 155
kityann 0:daab5accfd83 156 if(flag){//初期値設定
kityann 0:daab5accfd83 157 if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){
kityann 0:daab5accfd83 158 flag = false;
kityann 0:daab5accfd83 159 x[0][0]=(double)myGPS->latitudeL;
kityann 0:daab5accfd83 160 x[0][1]=(double)myGPS->longitudeL;
kityann 0:daab5accfd83 161 }
kityann 0:daab5accfd83 162 }
kityann 0:daab5accfd83 163
kityann 0:daab5accfd83 164 if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
kityann 0:daab5accfd83 165 return;
kityann 0:daab5accfd83 166 }
kityann 0:daab5accfd83 167 //Kalman Filter
kityann 0:daab5accfd83 168 Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
kityann 0:daab5accfd83 169
kityann 0:daab5accfd83 170 agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
kityann 0:daab5accfd83 171 agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
kityann 0:daab5accfd83 172 }
kityann 0:daab5accfd83 173 else agz.nowStatus = GPS_UNAVAIL;
kityann 0:daab5accfd83 174
kityann 0:daab5accfd83 175 }
s1200058 2:886fac7f4399 176
s1200058 2:886fac7f4399 177 /////////////////////////////////////////
s1200058 2:886fac7f4399 178 //
s1200058 2:886fac7f4399 179 //New Mode
s1200058 2:886fac7f4399 180 //
s1200058 2:886fac7f4399 181 /////////////////////////////////////////
s1200058 2:886fac7f4399 182
s1200058 2:886fac7f4399 183 void New_Mode(uint8_t *packetdata){
s1200058 2:886fac7f4399 184
s1200058 19:a806105ba365 185 //bool result;
s1200058 20:eee8ac8d1788 186 agz.changeMode(packetdata);
s1200058 2:886fac7f4399 187
s1200058 2:886fac7f4399 188 }
kityann 1:b2b950b916ce 189
kityann 1:b2b950b916ce 190 /////////////////////////////////////////
kityann 1:b2b950b916ce 191 //
kityann 1:b2b950b916ce 192 //Get Status
kityann 1:b2b950b916ce 193 //
kityann 1:b2b950b916ce 194 /////////////////////////////////////////
kityann 1:b2b950b916ce 195 void Get_Status(char SenderIDc,uint8_t *packetdata){
kityann 1:b2b950b916ce 196
kityann 1:b2b950b916ce 197 //マネージャからデータが来たとき
kityann 1:b2b950b916ce 198 if(SenderIDc == '0'){
kityann 1:b2b950b916ce 199 printf("get manager Status\n");
kityann 1:b2b950b916ce 200 }
kityann 1:b2b950b916ce 201 //他のロボットからデータが来たとき
kityann 1:b2b950b916ce 202 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
kityann 1:b2b950b916ce 203 printf("get other robots Status\n");
kityann 1:b2b950b916ce 204 }
kityann 1:b2b950b916ce 205 //基地局からデータが来たとき
kityann 1:b2b950b916ce 206 if(SenderIDc >= 'a' && SenderIDc <= 'z'){
kityann 1:b2b950b916ce 207 printf("Get Base data\n");
kityann 1:b2b950b916ce 208 int id = SenderIDc - 'a';
kityann 1:b2b950b916ce 209 agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
kityann 1:b2b950b916ce 210 agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
kityann 1:b2b950b916ce 211
kityann 1:b2b950b916ce 212 //debug
kityann 1:b2b950b916ce 213 for(int i = 0;i < 4;i++){
kityann 1:b2b950b916ce 214 printf("BASE:%d\n",i);
kityann 1:b2b950b916ce 215 printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n",
kityann 1:b2b950b916ce 216 agz.get_basePoint_lati(i),agz.get_basePoint_longi(i),
kityann 1:b2b950b916ce 217 agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)
kityann 1:b2b950b916ce 218 );
kityann 1:b2b950b916ce 219 }
kityann 1:b2b950b916ce 220 }
kityann 1:b2b950b916ce 221 }
kityann 1:b2b950b916ce 222
s1200058 2:886fac7f4399 223 void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata){
s1200058 2:886fac7f4399 224
s1200058 2:886fac7f4399 225 //マネージャからデータが来たとき
s1200058 2:886fac7f4399 226 if(SenderIDc == '0'){
s1200058 2:886fac7f4399 227 printf("get manager Status Kalman\n");
s1200058 2:886fac7f4399 228 }
s1200058 2:886fac7f4399 229 //他のロボットからデータが来たとき
s1200058 2:886fac7f4399 230 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
s1200058 2:886fac7f4399 231 printf("get other robots Status Kalman\n");
s1200058 2:886fac7f4399 232 }
s1200058 2:886fac7f4399 233 //基地局からデータが来たとき
s1200058 2:886fac7f4399 234 if(SenderIDc >= 'a' && SenderIDc <= 'z'){
s1200058 2:886fac7f4399 235 printf("Get Base data Kalman\n");
s1200058 2:886fac7f4399 236 int id = SenderIDc - 'a';
s1200058 2:886fac7f4399 237 agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
s1200058 2:886fac7f4399 238
s1200058 2:886fac7f4399 239 //debug
s1200058 2:886fac7f4399 240 for(int i = 0;i < 4;i++){
s1200058 2:886fac7f4399 241 printf("BASE:%d\n",i);
s1200058 2:886fac7f4399 242 printf("latitudeK:%f,longitudeK:%f\n",
s1200058 2:886fac7f4399 243 agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i));
s1200058 2:886fac7f4399 244 }
s1200058 2:886fac7f4399 245 }
s1200058 2:886fac7f4399 246 }
s1200058 2:886fac7f4399 247
kityann 1:b2b950b916ce 248 /////////////////////////////////////////
kityann 1:b2b950b916ce 249 //
kityann 1:b2b950b916ce 250 //Send_Request_to_base
kityann 1:b2b950b916ce 251 //
kityann 1:b2b950b916ce 252 /////////////////////////////////////////
kityann 1:b2b950b916ce 253 void Send_Request_Base(int basenumber){
kityann 1:b2b950b916ce 254 printf("send\n");
kityann 1:b2b950b916ce 255 agz.createRequestCommand(MyID, basenumber);
kityann 1:b2b950b916ce 256 //Select Destination
kityann 1:b2b950b916ce 257 ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength());
kityann 1:b2b950b916ce 258 //Send -> Base
kityann 1:b2b950b916ce 259 xbee.send(tx64request);
kityann 1:b2b950b916ce 260 }
kityann 0:daab5accfd83 261
s1200058 3:1ac506a96fd6 262 /////////////////////////////////////////
s1200058 3:1ac506a96fd6 263 //
s1200058 3:1ac506a96fd6 264 //auto_Move
s1200058 3:1ac506a96fd6 265 //
s1200058 3:1ac506a96fd6 266 //InAreaかOutAreaの判定
s1200058 3:1ac506a96fd6 267 //Kalmanを通した値を出力(Baseと自分)
s1200058 3:1ac506a96fd6 268 /////////////////////////////////////////
s1200058 3:1ac506a96fd6 269
s1200058 3:1ac506a96fd6 270 void auto_Move(){
s1200058 3:1ac506a96fd6 271
s1200058 3:1ac506a96fd6 272 bool result;
s1200058 3:1ac506a96fd6 273 int i;
s1200058 3:1ac506a96fd6 274
s1200058 3:1ac506a96fd6 275 result = agz.gpsAuto();
s1200058 20:eee8ac8d1788 276 //agz.set_agzAutoGPS();
s1200058 20:eee8ac8d1788 277 //agz.set_agzKalmanGPS();
s1200058 3:1ac506a96fd6 278
s1200058 3:1ac506a96fd6 279 if(result == true){
s1200058 3:1ac506a96fd6 280 printf("Out Area\n");
s1200058 3:1ac506a96fd6 281 }
s1200058 3:1ac506a96fd6 282 else if(result == false){
s1200058 3:1ac506a96fd6 283 printf("In Area\n");
s1200058 3:1ac506a96fd6 284 }
s1200058 3:1ac506a96fd6 285 for(i = 0; i < 4; i++){
s1200058 3:1ac506a96fd6 286 printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i));
s1200058 3:1ac506a96fd6 287 }
s1200058 3:1ac506a96fd6 288 printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
s1200058 3:1ac506a96fd6 289
s1200058 3:1ac506a96fd6 290 }
s1200058 3:1ac506a96fd6 291
s1200058 5:522c47c78401 292 void print_gps(int count){
s1200058 5:522c47c78401 293
s1200058 5:522c47c78401 294 printf("%d times:\n", count);
s1200058 5:522c47c78401 295 printf("%f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
s1200058 5:522c47c78401 296
s1200058 5:522c47c78401 297 }
kityann 0:daab5accfd83 298
kityann 0:daab5accfd83 299 /////////////////////////////////////////
kityann 0:daab5accfd83 300 //
kityann 0:daab5accfd83 301 //Kalman Processing
kityann 0:daab5accfd83 302 //
kityann 0:daab5accfd83 303 /////////////////////////////////////////
kityann 0:daab5accfd83 304
kityann 0:daab5accfd83 305 void get_K(){
kityann 0:daab5accfd83 306 double temp[2][2]={
kityann 0:daab5accfd83 307 {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
kityann 0:daab5accfd83 308 {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
kityann 0:daab5accfd83 309 };
kityann 0:daab5accfd83 310 double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
kityann 0:daab5accfd83 311 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
kityann 0:daab5accfd83 312 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
kityann 0:daab5accfd83 313 }
kityann 0:daab5accfd83 314
kityann 0:daab5accfd83 315
kityann 0:daab5accfd83 316 void get_x(){
kityann 0:daab5accfd83 317 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
kityann 0:daab5accfd83 318 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
kityann 0:daab5accfd83 319 }
kityann 0:daab5accfd83 320
kityann 0:daab5accfd83 321
kityann 0:daab5accfd83 322 void get_sigma(){
kityann 0:daab5accfd83 323 double temp[2][2];
kityann 0:daab5accfd83 324 for(int i=0;i<2;i++) {
kityann 0:daab5accfd83 325 for(int j=0;j<2;j++) {
kityann 0:daab5accfd83 326 for(int k=0;k<2;k++) {
kityann 0:daab5accfd83 327 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
kityann 0:daab5accfd83 328 }
kityann 0:daab5accfd83 329 }
kityann 0:daab5accfd83 330 }
kityann 0:daab5accfd83 331 for(int i = 0;i < 2;i++){
kityann 0:daab5accfd83 332 for(int j = 0;j < 2;j++){
kityann 0:daab5accfd83 333 sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
kityann 0:daab5accfd83 334 }
kityann 0:daab5accfd83 335 }
kityann 0:daab5accfd83 336 }
kityann 0:daab5accfd83 337
kityann 0:daab5accfd83 338 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
kityann 0:daab5accfd83 339 y[0] = Latitude;
kityann 0:daab5accfd83 340 y[1] = Longitude;
kityann 0:daab5accfd83 341 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
kityann 0:daab5accfd83 342 get_K();
kityann 0:daab5accfd83 343 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
kityann 0:daab5accfd83 344 get_x();
kityann 0:daab5accfd83 345 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
kityann 0:daab5accfd83 346 get_sigma();
kityann 0:daab5accfd83 347
kityann 0:daab5accfd83 348
kityann 0:daab5accfd83 349 //kousinn
kityann 0:daab5accfd83 350 for(int i = 0;i < 2;i++){
kityann 0:daab5accfd83 351 for(int j = 0;j < 2;j++){
kityann 0:daab5accfd83 352 K[0][i][j]=K[1][i][j];
kityann 0:daab5accfd83 353 x[0][i]=x[1][i];
kityann 0:daab5accfd83 354 sigma[0][i][j]=sigma[1][i][j];
kityann 0:daab5accfd83 355 }
kityann 0:daab5accfd83 356 }
kityann 0:daab5accfd83 357
kityann 4:f36986ceb73d 358 if(sigma[0][0][0] < SIGMA_MIN)sigma[0][0][0]=SIGMA_MIN;
kityann 4:f36986ceb73d 359 if(sigma[0][1][1] < SIGMA_MIN)sigma[0][1][1]=SIGMA_MIN;
kityann 4:f36986ceb73d 360
kityann 0:daab5accfd83 361 myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
kityann 0:daab5accfd83 362 myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
kityann 0:daab5accfd83 363 myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
kityann 0:daab5accfd83 364 myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
kityann 0:daab5accfd83 365
kityann 0:daab5accfd83 366 agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
kityann 0:daab5accfd83 367 }
kityann 0:daab5accfd83 368
kityann 0:daab5accfd83 369
kityann 0:daab5accfd83 370 /////////////////////////////////////////
kityann 0:daab5accfd83 371 //
kityann 0:daab5accfd83 372 //Main Processing
kityann 0:daab5accfd83 373 //
kityann 0:daab5accfd83 374 /////////////////////////////////////////
kityann 0:daab5accfd83 375 int main() {
kityann 0:daab5accfd83 376 //start up time
kityann 0:daab5accfd83 377 wait(3);
kityann 0:daab5accfd83 378 //set pc frequency to 57600bps
kityann 0:daab5accfd83 379 pc.baud(PC_BAUD_RATE);
kityann 0:daab5accfd83 380 //set xbee frequency to 57600bps
kityann 0:daab5accfd83 381 xbee.begin(XBEE_BAUD_RATE);
kityann 0:daab5accfd83 382
kityann 0:daab5accfd83 383
kityann 0:daab5accfd83 384 //GPS setting
kityann 0:daab5accfd83 385 gps_Serial = new Serial(p28,p27);
kityann 0:daab5accfd83 386 Adafruit_GPS myGPS(gps_Serial);
kityann 0:daab5accfd83 387 Timer refresh_Timer;
s1200058 7:a9a1acc7673b 388 const int refresh_Time = 1000; //refresh time in ms
s1200058 3:1ac506a96fd6 389 Timer auto_Timer;
s1200058 3:1ac506a96fd6 390 const int auto_Time = 2000; //refresh time in ms
s1200058 5:522c47c78401 391 int count = 0;
kityann 0:daab5accfd83 392 myGPS.begin(GPS_BAUD_RATE);
kityann 0:daab5accfd83 393
s1200058 14:5deb7a4f1cd4 394 Timer collect_Timer;
s1200058 14:5deb7a4f1cd4 395 const int collect_Time = 2000; //when we collect 4 GPS point, we use
s1200058 14:5deb7a4f1cd4 396 int collect_flag = 0;
s1200058 14:5deb7a4f1cd4 397 char collect_state = 'a';
s1200058 14:5deb7a4f1cd4 398
kityann 0:daab5accfd83 399 char SenderIDc;
kityann 0:daab5accfd83 400 //GPS Send Command
kityann 0:daab5accfd83 401 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:daab5accfd83 402 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:daab5accfd83 403 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:daab5accfd83 404
kityann 0:daab5accfd83 405 wait(2);
kityann 0:daab5accfd83 406
kityann 0:daab5accfd83 407 //interrupt start
kityann 0:daab5accfd83 408 refresh_Timer.start();
s1200058 3:1ac506a96fd6 409 auto_Timer.start();
s1200058 16:a07350b3eb64 410 collect_Timer.start();
kityann 1:b2b950b916ce 411
kityann 0:daab5accfd83 412 printf("start\n");
kityann 0:daab5accfd83 413
kityann 0:daab5accfd83 414 while (true) {
kityann 0:daab5accfd83 415
kityann 0:daab5accfd83 416 //Check Xbee Buffer Available
kityann 0:daab5accfd83 417 xbee.readPacket();
kityann 0:daab5accfd83 418
kityann 0:daab5accfd83 419 if (xbee.getResponse().isAvailable()) {
kityann 0:daab5accfd83 420 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 421 uint8_t *buf = zbRx.getFrameData();
kityann 0:daab5accfd83 422
kityann 0:daab5accfd83 423 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:daab5accfd83 424 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 425 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
kityann 0:daab5accfd83 426 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
kityann 0:daab5accfd83 427 SenderIDc = buf1[5];//送信元のIDを取得する
kityann 0:daab5accfd83 428 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
kityann 0:daab5accfd83 429
kityann 0:daab5accfd83 430 //Check Command Type
kityann 0:daab5accfd83 431 switch(Command_type){
kityann 0:daab5accfd83 432 //Get Request command
s1200058 2:886fac7f4399 433 case MANUAL:{
s1200058 12:48ef69b4f0e4 434 Plus_Speed(buf);
s1200058 2:886fac7f4399 435 break;
s1200058 2:886fac7f4399 436 }
kityann 0:daab5accfd83 437 case STATUS_REQUEST:{
kityann 0:daab5accfd83 438 Send_Status(SenderIDc);
kityann 0:daab5accfd83 439 break;
kityann 0:daab5accfd83 440 }
s1200058 2:886fac7f4399 441 case CHANGE_MODE:{
s1200058 12:48ef69b4f0e4 442 New_Mode(buf);
s1200058 2:886fac7f4399 443 break;
s1200058 2:886fac7f4399 444 }
kityann 1:b2b950b916ce 445 case RECEIVE_STATUS:{
kityann 1:b2b950b916ce 446 Get_Status(SenderIDc,buf1);
kityann 1:b2b950b916ce 447 break;
kityann 1:b2b950b916ce 448 }
s1200058 2:886fac7f4399 449 case RECEIVE_KALMAN:{
s1200058 2:886fac7f4399 450 Get_Status_Kalman(SenderIDc, buf1);
s1200058 2:886fac7f4399 451 break;
s1200058 2:886fac7f4399 452 }
kityann 0:daab5accfd83 453 default:{
kityann 0:daab5accfd83 454 break;
kityann 0:daab5accfd83 455 }
kityann 0:daab5accfd83 456 }//endswitch
kityann 0:daab5accfd83 457 }//endifZB_RX_RESPONSE
kityann 0:daab5accfd83 458 }//endifisAvailable
kityann 0:daab5accfd83 459
kityann 0:daab5accfd83 460 myGPS.read();
kityann 0:daab5accfd83 461 //recive gps module
kityann 0:daab5accfd83 462 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:daab5accfd83 463 if ( myGPS.newNMEAreceived() ) {
kityann 0:daab5accfd83 464 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
s1200058 8:994c73f6fad9 465 continue;
s1200058 8:994c73f6fad9 466 }
s1200058 8:994c73f6fad9 467 else{
s1200058 5:522c47c78401 468 count++;
s1200058 8:994c73f6fad9 469 }
kityann 0:daab5accfd83 470 }
kityann 1:b2b950b916ce 471 //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
kityann 0:daab5accfd83 472 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:daab5accfd83 473 refresh_Timer.reset();
s1200058 12:48ef69b4f0e4 474 //print_gps(count);
kityann 0:daab5accfd83 475 Get_GPS(&myGPS);
kityann 0:daab5accfd83 476
kityann 0:daab5accfd83 477 }
s1200058 20:eee8ac8d1788 478
s1200058 20:eee8ac8d1788 479 //get base GPS
s1200058 14:5deb7a4f1cd4 480 if( collect_Timer.read_ms() >= collect_Time){
s1200058 15:f3d01f37f00d 481 collect_Timer.reset();
s1200058 15:f3d01f37f00d 482
s1200058 15:f3d01f37f00d 483 Send_Request_Base(collect_flag);
s1200058 14:5deb7a4f1cd4 484
s1200058 14:5deb7a4f1cd4 485 collect_flag++;
s1200058 14:5deb7a4f1cd4 486 collect_state++;
s1200058 14:5deb7a4f1cd4 487
s1200058 14:5deb7a4f1cd4 488 if(collect_flag == 4){
s1200058 14:5deb7a4f1cd4 489 collect_state = 'a';
s1200058 14:5deb7a4f1cd4 490 collect_flag = 0;
s1200058 14:5deb7a4f1cd4 491 }
s1200058 14:5deb7a4f1cd4 492 }
s1200058 20:eee8ac8d1788 493
s1200058 14:5deb7a4f1cd4 494 if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
s1200058 3:1ac506a96fd6 495 auto_Timer.reset();
s1200058 3:1ac506a96fd6 496 auto_Move();
kityann 1:b2b950b916ce 497 }
kityann 0:daab5accfd83 498 }
kityann 0:daab5accfd83 499 }