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@37:19a9a37a5658, 2015-10-29 (annotated)

Committer:
s1200058
Date:
Thu Oct 29 07:52:14 2015 +0000
Revision:
37:19a9a37a5658
Parent:
36:f2013fc33b28
Child:
38:5cd6d4964f65
????

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