aigamozu / Mbed 2 deprecated Aigamozu_Robot_ver4_5

2015/06/09

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

Fork of Aigamozu_Robot_ver4_4 by aigamozu

main.cpp@39:f6c7b746f0c4, 2015-06-08 (annotated)

Committer:
kityann
Date:
Mon Jun 08 15:48:20 2015 +0000
Revision:
39:f6c7b746f0c4
Parent:
38:01505448b500 
2015/06/09

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