aigamozu / Mbed 2 deprecated Aigamozu_Base_2016_ver1

2016_05_19 change manager

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

Fork of Aigamozu_Base_ver3_1 by aigamozu

main.cpp@15:35e3917fcbf5, 2015-05-27 (annotated)

Committer:
s1200058
Date:
Wed May 27 12:12:04 2015 +0000
Revision:
15:35e3917fcbf5
Parent:
14:3aa6d735a2fa
Child:
16:a3b96eb5d450
define the range of error

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