aigamozu / Mbed 2 deprecated Aigamozu_Base_ver3_2

2015/06/09

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

Fork of Aigamozu_Base_ver3_1 by aigamozu

main.cpp@16:ad381954fc13, 2015-06-08 (annotated)

Committer:
kityann
Date:
Mon Jun 08 15:47:05 2015 +0000
Revision:
16:ad381954fc13
Parent:
15:35e3917fcbf5 
2015/06/09

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:eee5e3d906ce 1 /**********************************************/
kityann 0:eee5e3d906ce 2 //
kityann 0:eee5e3d906ce 3 //
kityann 0:eee5e3d906ce 4 //
kityann 0:eee5e3d906ce 5 // Program name: Aigamozu BASE
kityann 0:eee5e3d906ce 6 // Author: Mineta Kizuku
kityann 0:eee5e3d906ce 7 //
kityann 0:eee5e3d906ce 8 //
kityann 0:eee5e3d906ce 9 /**********************************************/
kityann 0:eee5e3d906ce 10
kityann 0:eee5e3d906ce 11 /**********************************************/
kityann 0:eee5e3d906ce 12 //更新情報
kityann 0:eee5e3d906ce 13 //2015/05/11
kityann 0:eee5e3d906ce 14 //ベースプログラムの作成
kityann 0:eee5e3d906ce 15 //
kityann 2:d97700414618 16 //2015/05/13
kityann 2:d97700414618 17 //カルマンフィルタの共分散の値を0.0001以下にならないようにした
kityann 2:d97700414618 18 //共分散の値を10進数に変換するようにした
s1200058 12:83be8a3c212d 19 //
s1200058 12:83be8a3c212d 20 //2015/05/17
s1200058 12:83be8a3c212d 21 //Get_GPS()の中身longitudeの範囲138〜140に変更
s1200058 12:83be8a3c212d 22 //
s1200058 13:82f4f00f11f6 23 //2015/05/24
s1200058 13:82f4f00f11f6 24 //Kalmanフィルターを十進数で計算するようにした。
s1200058 13:82f4f00f11f6 25 //Kalmanフィルターの計算式を変更した。
s1200058 13:82f4f00f11f6 26 //set_kalmanを追加した。
kityann 16:ad381954fc13 27
kityann 16:ad381954fc13 28 //2015/06/09
kityann 16:ad381954fc13 29 //カルマンフィルタをロボットと同じ移動のブレを考えたものに変更した
kityann 0:eee5e3d906ce 30 /**********************************************/
kityann 0:eee5e3d906ce 31
kityann 0:eee5e3d906ce 32 #include "mbed.h"
kityann 0:eee5e3d906ce 33 #include "XBee.h"
kityann 0:eee5e3d906ce 34 #include "MBed_Adafruit_GPS.h"
kityann 0:eee5e3d906ce 35 #include "AigamozuControlPackets.h"
kityann 0:eee5e3d906ce 36 #include "agzIDLIST.h"
kityann 0:eee5e3d906ce 37 #include "aigamozuSetting.h"
kityann 0:eee5e3d906ce 38 #include "agz_common.h"
kityann 0:eee5e3d906ce 39 #include "Kalman.h"
kityann 0:eee5e3d906ce 40
kityann 2:d97700414618 41
kityann 2:d97700414618 42 #define SIGMA_MIN 0.0001
kityann 2:d97700414618 43
kityann 0:eee5e3d906ce 44 //************ID Number*****************
s1200058 14:3aa6d735a2fa 45 const char MyID = 'd';
kityann 0:eee5e3d906ce 46 //************ID Number*****************
kityann 0:eee5e3d906ce 47
kityann 0:eee5e3d906ce 48 /////////////////////////////////////////
kityann 0:eee5e3d906ce 49 //
kityann 0:eee5e3d906ce 50 //Pin Setting
kityann 0:eee5e3d906ce 51 //
kityann 0:eee5e3d906ce 52 /////////////////////////////////////////
kityann 0:eee5e3d906ce 53 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:eee5e3d906ce 54
kityann 0:eee5e3d906ce 55
kityann 0:eee5e3d906ce 56 /////////////////////////////////////////
kityann 0:eee5e3d906ce 57 //
kityann 0:eee5e3d906ce 58 //Connection Setting
kityann 0:eee5e3d906ce 59 //
kityann 0:eee5e3d906ce 60 /////////////////////////////////////////
kityann 0:eee5e3d906ce 61
kityann 0:eee5e3d906ce 62 //Serial Connect Setting: PC <--> mbed
kityann 0:eee5e3d906ce 63 Serial pc(USBTX, USBRX);
kityann 0:eee5e3d906ce 64
kityann 0:eee5e3d906ce 65 //Serial Connect Setting: GPS <--> mbed
kityann 0:eee5e3d906ce 66 Serial * gps_Serial;
kityann 0:eee5e3d906ce 67
kityann 0:eee5e3d906ce 68 //Serial Connect Setting: XBEE <--> mbed
kityann 0:eee5e3d906ce 69 XBee xbee(p13,p14);
kityann 0:eee5e3d906ce 70 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:eee5e3d906ce 71
kityann 0:eee5e3d906ce 72 //set up GPS module
kityann 0:eee5e3d906ce 73
kityann 0:eee5e3d906ce 74 //set up AigamozuControlPackets library
kityann 0:eee5e3d906ce 75 AigamozuControlPackets agz(agz_motorShield);
kityann 0:eee5e3d906ce 76
kityann 0:eee5e3d906ce 77
kityann 0:eee5e3d906ce 78 /////////////////////////////////////////
kityann 0:eee5e3d906ce 79 //
kityann 0:eee5e3d906ce 80 //For Kalman data
kityann 0:eee5e3d906ce 81 //
kityann 0:eee5e3d906ce 82 /////////////////////////////////////////
kityann 16:ad381954fc13 83 #define FIRST_S2_1 1.0e-8
kityann 16:ad381954fc13 84 #define FIRST_S2_2 1.0e-6
s1200058 13:82f4f00f11f6 85 #define COUNTER_MAX 10000
s1200058 15:35e3917fcbf5 86 #define ERROR_RANGE 0.001
s1200058 13:82f4f00f11f6 87 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
kityann 16:ad381954fc13 88 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 16:ad381954fc13 89 double s2_R=FIRST_S2_2;//GPSセンサの分散
kityann 16:ad381954fc13 90 double s2_Q=FIRST_S2_2;
s1200058 13:82f4f00f11f6 91 double Kx=0,Ky=0;//カルマンゲイン
s1200058 13:82f4f00f11f6 92 double zx,zy;//観測値
s1200058 13:82f4f00f11f6 93 void Kalman(double Latitude,double Longitude);
s1200058 14:3aa6d735a2fa 94 int change = 0;
kityann 0:eee5e3d906ce 95
s1200058 14:3aa6d735a2fa 96 /*
s1200058 14:3aa6d735a2fa 97 LocalFileSystem local("local"); // マウントポイントを定義（ディレクトリパスになる）
s1200058 14:3aa6d735a2fa 98 FILE *fp;
s1200058 14:3aa6d735a2fa 99 char filename[16] = "/local/out0.txt";
s1200058 14:3aa6d735a2fa 100 */
kityann 0:eee5e3d906ce 101 /////////////////////////////////////////
kityann 0:eee5e3d906ce 102 //
kityann 0:eee5e3d906ce 103 //Send_Status
kityann 0:eee5e3d906ce 104 //
kityann 0:eee5e3d906ce 105 //リクエストがきたとき、自分の位置情報などを返信する
kityann 0:eee5e3d906ce 106 /////////////////////////////////////////
kityann 0:eee5e3d906ce 107 void Send_Status(char SenderIDc){
kityann 0:eee5e3d906ce 108 XBeeAddress64 send_Address;
kityann 0:eee5e3d906ce 109 if(SenderIDc == '0'){
kityann 0:eee5e3d906ce 110 send_Address = manager_Address;
kityann 0:eee5e3d906ce 111 }
kityann 0:eee5e3d906ce 112 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
kityann 0:eee5e3d906ce 113 send_Address = robot_Address[SenderIDc - 'A'];
kityann 0:eee5e3d906ce 114 }
kityann 0:eee5e3d906ce 115 if(SenderIDc >= 'a' && SenderIDc <= 'z'){
kityann 0:eee5e3d906ce 116 send_Address = base_Address[SenderIDc - 'a'];
kityann 0:eee5e3d906ce 117 }
kityann 0:eee5e3d906ce 118 //send normal data
kityann 0:eee5e3d906ce 119 //Create GPS Infomation Packet
kityann 1:ee2713435312 120 agz.createReceiveStatusCommand(MyID,SenderIDc,
kityann 1:ee2713435312 121 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 0:eee5e3d906ce 122 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 0:eee5e3d906ce 123 agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 1:ee2713435312 124
s1200058 13:82f4f00f11f6 125 /* //debug***************************************************
kityann 1:ee2713435312 126 printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
kityann 1:ee2713435312 127 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 1:ee2713435312 128 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 1:ee2713435312 129 agz.get_agzCov_lati(),agz.get_agzCov_longi()
kityann 1:ee2713435312 130 );
kityann 1:ee2713435312 131 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
kityann 1:ee2713435312 132 printf("\n");
kityann 1:ee2713435312 133 //debug end***************************************************
s1200058 13:82f4f00f11f6 134 */
kityann 0:eee5e3d906ce 135 //Select Destination
kityann 0:eee5e3d906ce 136 ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
kityann 0:eee5e3d906ce 137 //Send -> Base
kityann 0:eee5e3d906ce 138 xbee.send(tx64request);
kityann 0:eee5e3d906ce 139 }
kityann 0:eee5e3d906ce 140
kityann 0:eee5e3d906ce 141 /////////////////////////////////////////
kityann 0:eee5e3d906ce 142 //
kityann 0:eee5e3d906ce 143 //Get GPS function
kityann 0:eee5e3d906ce 144 //
kityann 0:eee5e3d906ce 145 /////////////////////////////////////////
kityann 0:eee5e3d906ce 146
kityann 0:eee5e3d906ce 147 void Get_GPS(Adafruit_GPS *myGPS){
s1200058 14:3aa6d735a2fa 148 static int flag = 0;
s1200058 14:3aa6d735a2fa 149 // static int save_counter = 0;
kityann 0:eee5e3d906ce 150
kityann 0:eee5e3d906ce 151 if (myGPS->fix) {
kityann 0:eee5e3d906ce 152 agz.nowStatus = GPS_AVAIL;
s1200058 13:82f4f00f11f6 153 agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
kityann 0:eee5e3d906ce 154
s1200058 13:82f4f00f11f6 155 if(flag < COUNTER_MAX){
s1200058 13:82f4f00f11f6 156 flag++;
s1200058 13:82f4f00f11f6 157 }
s1200058 13:82f4f00f11f6 158 if(flag == 15){
s1200058 13:82f4f00f11f6 159 x_prev = agz.get_agzPoint_lati();
s1200058 13:82f4f00f11f6 160 y_prev = agz.get_agzPoint_longi();
kityann 0:eee5e3d906ce 161 }
s1200058 13:82f4f00f11f6 162
s1200058 13:82f4f00f11f6 163 if(flag >= 16){
s1200058 15:35e3917fcbf5 164 if(abs(x_prev - agz.get_agzPoint_lati()) < ERROR_RANGE && abs(y_prev - agz.get_agzPoint_longi()) < ERROR_RANGE){
s1200058 14:3aa6d735a2fa 165 Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
s1200058 15:35e3917fcbf5 166 change = 1;
s1200058 14:3aa6d735a2fa 167 }
s1200058 15:35e3917fcbf5 168 /* else{
s1200058 14:3aa6d735a2fa 169 change = 0;
s1200058 14:3aa6d735a2fa 170 }
s1200058 14:3aa6d735a2fa 171
s1200058 14:3aa6d735a2fa 172 if(save_counter < 10){
s1200058 14:3aa6d735a2fa 173 fp = fopen(filename, "a");
s1200058 14:3aa6d735a2fa 174 fprintf(fp, "%d %.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
s1200058 14:3aa6d735a2fa 175 change, agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1200058 14:3aa6d735a2fa 176 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1200058 14:3aa6d735a2fa 177 agz.get_agzCov_lati(),agz.get_agzCov_longi());
s1200058 14:3aa6d735a2fa 178 fclose(fp);
s1200058 14:3aa6d735a2fa 179
s1200058 14:3aa6d735a2fa 180 if((flag - 16) % 500 == 0){
s1200058 14:3aa6d735a2fa 181 filename[10]++;
s1200058 14:3aa6d735a2fa 182 save_counter++;
s1200058 14:3aa6d735a2fa 183 }
s1200058 14:3aa6d735a2fa 184 }
s1200058 14:3aa6d735a2fa 185 */
s1200058 13:82f4f00f11f6 186 }
s1200058 13:82f4f00f11f6 187
s1200058 13:82f4f00f11f6 188 printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
s1200058 13:82f4f00f11f6 189 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1200058 13:82f4f00f11f6 190 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1200058 13:82f4f00f11f6 191 agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 0:eee5e3d906ce 192 }
kityann 0:eee5e3d906ce 193 else agz.nowStatus = GPS_UNAVAIL;
kityann 0:eee5e3d906ce 194
kityann 0:eee5e3d906ce 195 }
kityann 0:eee5e3d906ce 196
kityann 0:eee5e3d906ce 197
kityann 0:eee5e3d906ce 198 /////////////////////////////////////////
kityann 0:eee5e3d906ce 199 //
kityann 0:eee5e3d906ce 200 //Kalman Processing
kityann 0:eee5e3d906ce 201 //
kityann 0:eee5e3d906ce 202 /////////////////////////////////////////
s1200058 13:82f4f00f11f6 203 void calc_Kalman(){
s1200058 13:82f4f00f11f6 204 //calc Kalman gain
kityann 16:ad381954fc13 205 Kx = (s2x_prev+s2_Q)/(s2x_prev+s2_R+s2_Q);
kityann 16:ad381954fc13 206 Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q);
s1200058 13:82f4f00f11f6 207 //estimate
s1200058 13:82f4f00f11f6 208 x_cur = x_prev + Kx*(zx-x_prev);
s1200058 13:82f4f00f11f6 209 y_cur = y_prev + Ky*(zy-y_prev);
s1200058 13:82f4f00f11f6 210 //calc sigma
kityann 16:ad381954fc13 211 s2x_cur = (1-Kx)*(s2x_prev+s2_Q);
kityann 16:ad381954fc13 212 s2y_cur = (1-Ky)*(s2y_prev+s2_Q);
kityann 0:eee5e3d906ce 213
kityann 0:eee5e3d906ce 214 }
kityann 0:eee5e3d906ce 215
kityann 16:ad381954fc13 216
s1200058 13:82f4f00f11f6 217 void Kalman(double Latitude,double Longitude){
kityann 0:eee5e3d906ce 218
s1200058 13:82f4f00f11f6 219 zx = Latitude;
s1200058 13:82f4f00f11f6 220 zy = Longitude;
kityann 0:eee5e3d906ce 221
s1200058 13:82f4f00f11f6 222 calc_Kalman();
kityann 0:eee5e3d906ce 223
s1200058 13:82f4f00f11f6 224 //更新
s1200058 13:82f4f00f11f6 225 x_prev = x_cur;
s1200058 13:82f4f00f11f6 226 y_prev = y_cur;
s1200058 13:82f4f00f11f6 227 s2x_prev = s2x_cur;
s1200058 13:82f4f00f11f6 228 s2y_prev = s2y_cur;
kityann 2:d97700414618 229
s1200058 13:82f4f00f11f6 230 //agzPontKalmanとagzCovに格納する
s1200058 13:82f4f00f11f6 231 agz.set_agzPointKalman_lati(x_cur);
s1200058 13:82f4f00f11f6 232 agz.set_agzPointKalman_longi(y_cur);
s1200058 13:82f4f00f11f6 233 agz.set_agzCov(s2x_cur,s2y_cur);
kityann 0:eee5e3d906ce 234
kityann 0:eee5e3d906ce 235 }
kityann 0:eee5e3d906ce 236
kityann 0:eee5e3d906ce 237
kityann 0:eee5e3d906ce 238 /////////////////////////////////////////
kityann 0:eee5e3d906ce 239 //
kityann 0:eee5e3d906ce 240 //Main Processing
kityann 0:eee5e3d906ce 241 //
kityann 0:eee5e3d906ce 242 /////////////////////////////////////////
kityann 0:eee5e3d906ce 243 int main() {
kityann 0:eee5e3d906ce 244 //start up time
kityann 0:eee5e3d906ce 245 wait(3);
kityann 0:eee5e3d906ce 246 //set pc frequency to 57600bps
kityann 0:eee5e3d906ce 247 pc.baud(PC_BAUD_RATE);
kityann 0:eee5e3d906ce 248 //set xbee frequency to 57600bps
kityann 0:eee5e3d906ce 249 xbee.begin(XBEE_BAUD_RATE);
kityann 0:eee5e3d906ce 250
kityann 0:eee5e3d906ce 251
kityann 0:eee5e3d906ce 252 //GPS setting
kityann 0:eee5e3d906ce 253 gps_Serial = new Serial(p28,p27);
kityann 0:eee5e3d906ce 254 Adafruit_GPS myGPS(gps_Serial);
kityann 0:eee5e3d906ce 255 Timer refresh_Timer;
s1200058 3:60bad3679b4b 256 const int refresh_Time = 1000; //refresh time in ms
kityann 0:eee5e3d906ce 257 myGPS.begin(GPS_BAUD_RATE);
kityann 0:eee5e3d906ce 258
kityann 0:eee5e3d906ce 259 char SenderIDc;
kityann 0:eee5e3d906ce 260 //GPS Send Command
kityann 0:eee5e3d906ce 261 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:eee5e3d906ce 262 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:eee5e3d906ce 263 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:eee5e3d906ce 264
kityann 0:eee5e3d906ce 265 wait(2);
kityann 0:eee5e3d906ce 266
kityann 0:eee5e3d906ce 267 //interrupt start
kityann 0:eee5e3d906ce 268 refresh_Timer.start();
kityann 0:eee5e3d906ce 269
kityann 0:eee5e3d906ce 270 printf("start\n");
kityann 0:eee5e3d906ce 271
kityann 0:eee5e3d906ce 272 while (true) {
kityann 0:eee5e3d906ce 273
kityann 0:eee5e3d906ce 274 //Check Xbee Buffer Available
kityann 0:eee5e3d906ce 275 xbee.readPacket();
kityann 0:eee5e3d906ce 276
kityann 0:eee5e3d906ce 277 if (xbee.getResponse().isAvailable()) {
kityann 0:eee5e3d906ce 278 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:eee5e3d906ce 279 uint8_t *buf = zbRx.getFrameData();
kityann 0:eee5e3d906ce 280
kityann 0:eee5e3d906ce 281 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:eee5e3d906ce 282 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:eee5e3d906ce 283 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
kityann 0:eee5e3d906ce 284 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
kityann 0:eee5e3d906ce 285 SenderIDc = buf1[5];//送信元のIDを取得する
kityann 0:eee5e3d906ce 286 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
kityann 0:eee5e3d906ce 287
kityann 0:eee5e3d906ce 288 //Check Command Type
kityann 0:eee5e3d906ce 289 switch(Command_type){
kityann 0:eee5e3d906ce 290 //Get Request command
kityann 0:eee5e3d906ce 291 case STATUS_REQUEST:{
kityann 1:ee2713435312 292 Send_Status(SenderIDc);
kityann 0:eee5e3d906ce 293 break;
kityann 0:eee5e3d906ce 294 }
kityann 0:eee5e3d906ce 295 default:{
kityann 0:eee5e3d906ce 296 break;
kityann 0:eee5e3d906ce 297 }
kityann 0:eee5e3d906ce 298 }//endswitch
kityann 0:eee5e3d906ce 299 }//endifZB_RX_RESPONSE
kityann 0:eee5e3d906ce 300 }//endifisAvailable
kityann 0:eee5e3d906ce 301
kityann 0:eee5e3d906ce 302 myGPS.read();
kityann 0:eee5e3d906ce 303 //recive gps module
kityann 0:eee5e3d906ce 304 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:eee5e3d906ce 305 if ( myGPS.newNMEAreceived() ) {
kityann 0:eee5e3d906ce 306 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:eee5e3d906ce 307 continue;
kityann 0:eee5e3d906ce 308 }
kityann 0:eee5e3d906ce 309 }
kityann 0:eee5e3d906ce 310
kityann 0:eee5e3d906ce 311
kityann 0:eee5e3d906ce 312 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:eee5e3d906ce 313 refresh_Timer.reset();
kityann 0:eee5e3d906ce 314 Get_GPS(&myGPS);
kityann 1:ee2713435312 315
kityann 0:eee5e3d906ce 316 }
kityann 0:eee5e3d906ce 317 }
kityann 0:eee5e3d906ce 318 }