aigamozu / Mbed 2 deprecated Aigamozu_Base_ver3_1

change kalman

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

Fork of Aigamozu_Base_ver3 by aigamozu

main.cpp@12:83be8a3c212d, 2015-05-18 (annotated)

Committer:
s1200058
Date:
Mon May 18 02:38:35 2015 +0000
Revision:
12:83be8a3c212d
Parent:
11:617453352e91
Child:
13:82f4f00f11f6
//2015/05/17; //Get_GPS()???longitude???138?140???

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 //
kityann 0:eee5e3d906ce 23 /**********************************************/
kityann 0:eee5e3d906ce 24
kityann 0:eee5e3d906ce 25 #include "mbed.h"
kityann 0:eee5e3d906ce 26 #include "XBee.h"
kityann 0:eee5e3d906ce 27 #include "MBed_Adafruit_GPS.h"
kityann 0:eee5e3d906ce 28 #include "AigamozuControlPackets.h"
kityann 0:eee5e3d906ce 29 #include "agzIDLIST.h"
kityann 0:eee5e3d906ce 30 #include "aigamozuSetting.h"
kityann 0:eee5e3d906ce 31 #include "agz_common.h"
kityann 0:eee5e3d906ce 32 #include "Kalman.h"
kityann 0:eee5e3d906ce 33
kityann 2:d97700414618 34
kityann 2:d97700414618 35 #define SIGMA_MIN 0.0001
kityann 2:d97700414618 36
kityann 0:eee5e3d906ce 37 //************ID Number*****************
s1200058 11:617453352e91 38 const char MyID = 'a';
kityann 0:eee5e3d906ce 39 //************ID Number*****************
kityann 0:eee5e3d906ce 40
kityann 0:eee5e3d906ce 41 /////////////////////////////////////////
kityann 0:eee5e3d906ce 42 //
kityann 0:eee5e3d906ce 43 //Pin Setting
kityann 0:eee5e3d906ce 44 //
kityann 0:eee5e3d906ce 45 /////////////////////////////////////////
kityann 0:eee5e3d906ce 46 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:eee5e3d906ce 47
kityann 0:eee5e3d906ce 48
kityann 0:eee5e3d906ce 49 /////////////////////////////////////////
kityann 0:eee5e3d906ce 50 //
kityann 0:eee5e3d906ce 51 //Connection Setting
kityann 0:eee5e3d906ce 52 //
kityann 0:eee5e3d906ce 53 /////////////////////////////////////////
kityann 0:eee5e3d906ce 54
kityann 0:eee5e3d906ce 55 //Serial Connect Setting: PC <--> mbed
kityann 0:eee5e3d906ce 56 Serial pc(USBTX, USBRX);
kityann 0:eee5e3d906ce 57
kityann 0:eee5e3d906ce 58 //Serial Connect Setting: GPS <--> mbed
kityann 0:eee5e3d906ce 59 Serial * gps_Serial;
kityann 0:eee5e3d906ce 60
kityann 0:eee5e3d906ce 61 //Serial Connect Setting: XBEE <--> mbed
kityann 0:eee5e3d906ce 62 XBee xbee(p13,p14);
kityann 0:eee5e3d906ce 63 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:eee5e3d906ce 64
kityann 0:eee5e3d906ce 65 //set up GPS module
kityann 0:eee5e3d906ce 66
kityann 0:eee5e3d906ce 67 //set up AigamozuControlPackets library
kityann 0:eee5e3d906ce 68 AigamozuControlPackets agz(agz_motorShield);
kityann 0:eee5e3d906ce 69
kityann 0:eee5e3d906ce 70
kityann 0:eee5e3d906ce 71 /////////////////////////////////////////
kityann 0:eee5e3d906ce 72 //
kityann 0:eee5e3d906ce 73 //For Kalman data
kityann 0:eee5e3d906ce 74 //
kityann 0:eee5e3d906ce 75 /////////////////////////////////////////
kityann 0:eee5e3d906ce 76 double sigmaGPS[2][2] = {{250,0},{0,250}};
kityann 0:eee5e3d906ce 77 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
kityann 0:eee5e3d906ce 78 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
kityann 0:eee5e3d906ce 79 double y[2],x[2][2]={0};
kityann 0:eee5e3d906ce 80 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS);
kityann 0:eee5e3d906ce 81
kityann 0:eee5e3d906ce 82
kityann 0:eee5e3d906ce 83 /////////////////////////////////////////
kityann 0:eee5e3d906ce 84 //
kityann 0:eee5e3d906ce 85 //Send_Status
kityann 0:eee5e3d906ce 86 //
kityann 0:eee5e3d906ce 87 //リクエストがきたとき、自分の位置情報などを返信する
kityann 0:eee5e3d906ce 88 /////////////////////////////////////////
kityann 0:eee5e3d906ce 89 void Send_Status(char SenderIDc){
kityann 0:eee5e3d906ce 90 XBeeAddress64 send_Address;
kityann 0:eee5e3d906ce 91 if(SenderIDc == '0'){
kityann 0:eee5e3d906ce 92 send_Address = manager_Address;
kityann 0:eee5e3d906ce 93 }
kityann 0:eee5e3d906ce 94 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
kityann 0:eee5e3d906ce 95 send_Address = robot_Address[SenderIDc - 'A'];
kityann 0:eee5e3d906ce 96 }
kityann 0:eee5e3d906ce 97 if(SenderIDc >= 'a' && SenderIDc <= 'z'){
kityann 0:eee5e3d906ce 98 send_Address = base_Address[SenderIDc - 'a'];
kityann 0:eee5e3d906ce 99 }
kityann 0:eee5e3d906ce 100 //send normal data
kityann 0:eee5e3d906ce 101 //Create GPS Infomation Packet
kityann 1:ee2713435312 102 agz.createReceiveStatusCommand(MyID,SenderIDc,
kityann 1:ee2713435312 103 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 0:eee5e3d906ce 104 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 0:eee5e3d906ce 105 agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 1:ee2713435312 106
kityann 1:ee2713435312 107 //debug***************************************************
kityann 1:ee2713435312 108 printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
kityann 1:ee2713435312 109 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 1:ee2713435312 110 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 1:ee2713435312 111 agz.get_agzCov_lati(),agz.get_agzCov_longi()
kityann 1:ee2713435312 112 );
kityann 1:ee2713435312 113 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
kityann 1:ee2713435312 114 printf("\n");
kityann 1:ee2713435312 115 //debug end***************************************************
kityann 1:ee2713435312 116
kityann 0:eee5e3d906ce 117 //Select Destination
kityann 0:eee5e3d906ce 118 ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
kityann 0:eee5e3d906ce 119 //Send -> Base
kityann 0:eee5e3d906ce 120 xbee.send(tx64request);
kityann 0:eee5e3d906ce 121 }
kityann 0:eee5e3d906ce 122
kityann 0:eee5e3d906ce 123 /////////////////////////////////////////
kityann 0:eee5e3d906ce 124 //
kityann 0:eee5e3d906ce 125 //Get GPS function
kityann 0:eee5e3d906ce 126 //
kityann 0:eee5e3d906ce 127 /////////////////////////////////////////
kityann 0:eee5e3d906ce 128
kityann 0:eee5e3d906ce 129 void Get_GPS(Adafruit_GPS *myGPS){
kityann 0:eee5e3d906ce 130 static bool flag = true;
kityann 0:eee5e3d906ce 131
kityann 0:eee5e3d906ce 132 if (myGPS->fix) {
kityann 0:eee5e3d906ce 133 agz.nowStatus = GPS_AVAIL;
kityann 0:eee5e3d906ce 134
kityann 0:eee5e3d906ce 135 if(flag){//初期値設定
s1200058 11:617453352e91 136 if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=140){
kityann 0:eee5e3d906ce 137 flag = false;
kityann 0:eee5e3d906ce 138 x[0][0]=(double)myGPS->latitudeL;
kityann 0:eee5e3d906ce 139 x[0][1]=(double)myGPS->longitudeL;
kityann 0:eee5e3d906ce 140 }
kityann 0:eee5e3d906ce 141 }
kityann 0:eee5e3d906ce 142
s1200058 11:617453352e91 143 if(myGPS->longitudeH<138 || myGPS->longitudeH>140 || myGPS->latitudeH<37 || myGPS->latitudeH>38){
kityann 0:eee5e3d906ce 144 return;
kityann 0:eee5e3d906ce 145 }
kityann 0:eee5e3d906ce 146 //Kalman Filter
kityann 0:eee5e3d906ce 147 Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
kityann 0:eee5e3d906ce 148
kityann 1:ee2713435312 149 agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
kityann 1:ee2713435312 150 agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
kityann 0:eee5e3d906ce 151 }
kityann 0:eee5e3d906ce 152 else agz.nowStatus = GPS_UNAVAIL;
kityann 0:eee5e3d906ce 153
kityann 0:eee5e3d906ce 154 }
kityann 0:eee5e3d906ce 155
kityann 0:eee5e3d906ce 156
kityann 0:eee5e3d906ce 157 /////////////////////////////////////////
kityann 0:eee5e3d906ce 158 //
kityann 0:eee5e3d906ce 159 //Kalman Processing
kityann 0:eee5e3d906ce 160 //
kityann 0:eee5e3d906ce 161 /////////////////////////////////////////
kityann 0:eee5e3d906ce 162
kityann 0:eee5e3d906ce 163 void get_K(){
kityann 0:eee5e3d906ce 164 double temp[2][2]={
kityann 0:eee5e3d906ce 165 {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
kityann 0:eee5e3d906ce 166 {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
kityann 0:eee5e3d906ce 167 };
kityann 0:eee5e3d906ce 168 double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
kityann 0:eee5e3d906ce 169 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
kityann 0:eee5e3d906ce 170 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
kityann 0:eee5e3d906ce 171 }
kityann 0:eee5e3d906ce 172
kityann 0:eee5e3d906ce 173
kityann 0:eee5e3d906ce 174 void get_x(){
kityann 0:eee5e3d906ce 175 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
kityann 0:eee5e3d906ce 176 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
kityann 0:eee5e3d906ce 177 }
kityann 0:eee5e3d906ce 178
kityann 0:eee5e3d906ce 179
kityann 0:eee5e3d906ce 180 void get_sigma(){
s1200058 10:ea620ba4117c 181 double temp[2][2] = {0};
kityann 0:eee5e3d906ce 182 for(int i=0;i<2;i++) {
kityann 0:eee5e3d906ce 183 for(int j=0;j<2;j++) {
kityann 0:eee5e3d906ce 184 for(int k=0;k<2;k++) {
kityann 0:eee5e3d906ce 185 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
kityann 0:eee5e3d906ce 186 }
kityann 0:eee5e3d906ce 187 }
kityann 0:eee5e3d906ce 188 }
kityann 0:eee5e3d906ce 189 for(int i = 0;i < 2;i++){
kityann 0:eee5e3d906ce 190 for(int j = 0;j < 2;j++){
kityann 0:eee5e3d906ce 191 sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
kityann 0:eee5e3d906ce 192 }
kityann 0:eee5e3d906ce 193 }
kityann 0:eee5e3d906ce 194 }
kityann 0:eee5e3d906ce 195
kityann 0:eee5e3d906ce 196 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
kityann 0:eee5e3d906ce 197 y[0] = Latitude;
kityann 0:eee5e3d906ce 198 y[1] = Longitude;
kityann 0:eee5e3d906ce 199 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
kityann 0:eee5e3d906ce 200 get_K();
kityann 0:eee5e3d906ce 201 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
kityann 0:eee5e3d906ce 202 get_x();
kityann 0:eee5e3d906ce 203 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
kityann 0:eee5e3d906ce 204 get_sigma();
kityann 0:eee5e3d906ce 205
kityann 0:eee5e3d906ce 206
kityann 0:eee5e3d906ce 207 //kousinn
kityann 0:eee5e3d906ce 208 for(int i = 0;i < 2;i++){
kityann 0:eee5e3d906ce 209 for(int j = 0;j < 2;j++){
kityann 0:eee5e3d906ce 210 K[0][i][j]=K[1][i][j];
kityann 0:eee5e3d906ce 211 x[0][i]=x[1][i];
kityann 0:eee5e3d906ce 212 sigma[0][i][j]=sigma[1][i][j];
kityann 0:eee5e3d906ce 213 }
kityann 0:eee5e3d906ce 214 }
kityann 2:d97700414618 215
kityann 2:d97700414618 216 if(sigma[0][0][0] < SIGMA_MIN)sigma[0][0][0]=SIGMA_MIN;
kityann 2:d97700414618 217 if(sigma[0][1][1] < SIGMA_MIN)sigma[0][1][1]=SIGMA_MIN;
kityann 0:eee5e3d906ce 218
kityann 0:eee5e3d906ce 219 myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
kityann 0:eee5e3d906ce 220 myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
kityann 0:eee5e3d906ce 221 myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
kityann 0:eee5e3d906ce 222 myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
kityann 0:eee5e3d906ce 223
kityann 0:eee5e3d906ce 224 agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
kityann 0:eee5e3d906ce 225 }
kityann 0:eee5e3d906ce 226
kityann 0:eee5e3d906ce 227
kityann 0:eee5e3d906ce 228 /////////////////////////////////////////
kityann 0:eee5e3d906ce 229 //
kityann 0:eee5e3d906ce 230 //Main Processing
kityann 0:eee5e3d906ce 231 //
kityann 0:eee5e3d906ce 232 /////////////////////////////////////////
kityann 0:eee5e3d906ce 233 int main() {
kityann 0:eee5e3d906ce 234 //start up time
kityann 0:eee5e3d906ce 235 wait(3);
kityann 0:eee5e3d906ce 236 //set pc frequency to 57600bps
kityann 0:eee5e3d906ce 237 pc.baud(PC_BAUD_RATE);
kityann 0:eee5e3d906ce 238 //set xbee frequency to 57600bps
kityann 0:eee5e3d906ce 239 xbee.begin(XBEE_BAUD_RATE);
kityann 0:eee5e3d906ce 240
kityann 0:eee5e3d906ce 241
kityann 0:eee5e3d906ce 242 //GPS setting
kityann 0:eee5e3d906ce 243 gps_Serial = new Serial(p28,p27);
kityann 0:eee5e3d906ce 244 Adafruit_GPS myGPS(gps_Serial);
kityann 0:eee5e3d906ce 245 Timer refresh_Timer;
s1200058 3:60bad3679b4b 246 const int refresh_Time = 1000; //refresh time in ms
kityann 0:eee5e3d906ce 247 myGPS.begin(GPS_BAUD_RATE);
kityann 0:eee5e3d906ce 248
kityann 0:eee5e3d906ce 249 char SenderIDc;
kityann 0:eee5e3d906ce 250 //GPS Send Command
kityann 0:eee5e3d906ce 251 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:eee5e3d906ce 252 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:eee5e3d906ce 253 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:eee5e3d906ce 254
kityann 0:eee5e3d906ce 255 wait(2);
kityann 0:eee5e3d906ce 256
kityann 0:eee5e3d906ce 257 //interrupt start
kityann 0:eee5e3d906ce 258 refresh_Timer.start();
kityann 0:eee5e3d906ce 259
kityann 0:eee5e3d906ce 260 printf("start\n");
kityann 0:eee5e3d906ce 261
kityann 0:eee5e3d906ce 262 while (true) {
kityann 0:eee5e3d906ce 263
kityann 0:eee5e3d906ce 264 //Check Xbee Buffer Available
kityann 0:eee5e3d906ce 265 xbee.readPacket();
kityann 0:eee5e3d906ce 266
kityann 0:eee5e3d906ce 267 if (xbee.getResponse().isAvailable()) {
kityann 0:eee5e3d906ce 268 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:eee5e3d906ce 269 uint8_t *buf = zbRx.getFrameData();
kityann 0:eee5e3d906ce 270
kityann 0:eee5e3d906ce 271 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:eee5e3d906ce 272 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:eee5e3d906ce 273 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
kityann 0:eee5e3d906ce 274 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
kityann 0:eee5e3d906ce 275 SenderIDc = buf1[5];//送信元のIDを取得する
kityann 0:eee5e3d906ce 276 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
kityann 0:eee5e3d906ce 277
kityann 0:eee5e3d906ce 278 //Check Command Type
kityann 0:eee5e3d906ce 279 switch(Command_type){
kityann 0:eee5e3d906ce 280 //Get Request command
kityann 0:eee5e3d906ce 281 case STATUS_REQUEST:{
kityann 1:ee2713435312 282 Send_Status(SenderIDc);
kityann 0:eee5e3d906ce 283 break;
kityann 0:eee5e3d906ce 284 }
kityann 0:eee5e3d906ce 285 default:{
kityann 0:eee5e3d906ce 286 break;
kityann 0:eee5e3d906ce 287 }
kityann 0:eee5e3d906ce 288 }//endswitch
kityann 0:eee5e3d906ce 289 }//endifZB_RX_RESPONSE
kityann 0:eee5e3d906ce 290 }//endifisAvailable
kityann 0:eee5e3d906ce 291
kityann 0:eee5e3d906ce 292 myGPS.read();
kityann 0:eee5e3d906ce 293 //recive gps module
kityann 0:eee5e3d906ce 294 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:eee5e3d906ce 295 if ( myGPS.newNMEAreceived() ) {
kityann 0:eee5e3d906ce 296 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:eee5e3d906ce 297 continue;
kityann 0:eee5e3d906ce 298 }
kityann 0:eee5e3d906ce 299 }
kityann 0:eee5e3d906ce 300
kityann 0:eee5e3d906ce 301
kityann 0:eee5e3d906ce 302 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:eee5e3d906ce 303 refresh_Timer.reset();
kityann 0:eee5e3d906ce 304 Get_GPS(&myGPS);
kityann 1:ee2713435312 305
kityann 0:eee5e3d906ce 306 }
kityann 0:eee5e3d906ce 307 }
kityann 0:eee5e3d906ce 308 }