aigamozu / Mbed 2 deprecated mbed_control

0106

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

main.cpp@0:75c267089975, 2016-01-06 (annotated)

Committer:
kityann
Date:
Wed Jan 06 10:49:22 2016 +0000
Revision:
0:75c267089975
0106

Who changed what in which revision?

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