aigamozu / Mbed 2 deprecated Aigamozu_Robot_ver4_4

2015/06/05

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

Fork of Aigamozu_Robot_ver4_3 by aigamozu

main.cpp@29:524684a1198f, 2015-05-19 (annotated)

Committer:
s1200058
Date:
Tue May 19 10:27:43 2015 +0000
Revision:
29:524684a1198f
Parent:
28:4ccd7cfc6b1b
Child:
30:7f6ebe2121d9
change for test

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