aigamozu / Mbed 2 deprecated Aigamozu_Robot_templete

展示会用に簡単にしています

Dependencies:   ADXL345 AigamozuControlPackets_展示会 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed

Fork of Aigamozu_Robot_March by Mami Yokokawa

main.cpp@32:6ba2e5402f00, 2015-05-24 (annotated)

Committer:
s1200058
Date:
Sun May 24 06:38:54 2015 +0000
Revision:
32:6ba2e5402f00
Parent:
31:3f91f4bfca8a
Child:
33:3025b16bccd2
change a function Kalman()

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