CanSat2015aizu / Mbed 2 deprecated Cansat_program

test

Dependencies:   ADXL345 CanSat HMC5883L ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed

main.cpp@0:1fcc61be1dcf, 2015-07-09 (annotated)

Committer:
kityann
Date:
Thu Jul 09 07:46:44 2015 +0000
Revision:
0:1fcc61be1dcf
Child:
1:f1f7413ae6bd
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:1fcc61be1dcf 1 /**********************************************/
kityann 0:1fcc61be1dcf 2 //更新情報
kityann 0:1fcc61be1dcf 3
kityann 0:1fcc61be1dcf 4 /**********************************************/
kityann 0:1fcc61be1dcf 5
kityann 0:1fcc61be1dcf 6 #include "mbed.h"
kityann 0:1fcc61be1dcf 7 #include "XBee.h"
kityann 0:1fcc61be1dcf 8 #include "MBed_Adafruit_GPS.h"
kityann 0:1fcc61be1dcf 9 #include "AigamozuControlPackets.h"
kityann 0:1fcc61be1dcf 10 #include "agzIDLIST.h"
kityann 0:1fcc61be1dcf 11 #include "aigamozuSetting.h"
kityann 0:1fcc61be1dcf 12 #include "math.h"
kityann 0:1fcc61be1dcf 13
kityann 0:1fcc61be1dcf 14 #define SIGMA_MIN 0.0001
kityann 0:1fcc61be1dcf 15
kityann 0:1fcc61be1dcf 16 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 17 //
kityann 0:1fcc61be1dcf 18 //Pin Setting
kityann 0:1fcc61be1dcf 19 //
kityann 0:1fcc61be1dcf 20 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 21 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:1fcc61be1dcf 22
kityann 0:1fcc61be1dcf 23
kityann 0:1fcc61be1dcf 24 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 25 //
kityann 0:1fcc61be1dcf 26 //Connection Setting
kityann 0:1fcc61be1dcf 27 //
kityann 0:1fcc61be1dcf 28 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 29
kityann 0:1fcc61be1dcf 30 //Serial Connect Setting: PC <--> mbed
kityann 0:1fcc61be1dcf 31 Serial pc(USBTX, USBRX);
kityann 0:1fcc61be1dcf 32
kityann 0:1fcc61be1dcf 33 //Serial Connect Setting: GPS <--> mbed
kityann 0:1fcc61be1dcf 34 Serial * gps_Serial;
kityann 0:1fcc61be1dcf 35
kityann 0:1fcc61be1dcf 36 //Serial Connect Setting: XBEE <--> mbed
kityann 0:1fcc61be1dcf 37 XBee xbee(p13,p14);
kityann 0:1fcc61be1dcf 38 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:1fcc61be1dcf 39
kityann 0:1fcc61be1dcf 40 //set up GPS module
kityann 0:1fcc61be1dcf 41
kityann 0:1fcc61be1dcf 42 //set up AigamozuControlPackets library
kityann 0:1fcc61be1dcf 43 AigamozuControlPackets agz(agz_motorShield);
kityann 0:1fcc61be1dcf 44
kityann 0:1fcc61be1dcf 45
kityann 0:1fcc61be1dcf 46 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 47 //
kityann 0:1fcc61be1dcf 48 //For Kalman data
kityann 0:1fcc61be1dcf 49 //
kityann 0:1fcc61be1dcf 50 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 51 #define FIRST_S2_1 1.0e-8
kityann 0:1fcc61be1dcf 52 #define FIRST_S2_2 1.0e-6
kityann 0:1fcc61be1dcf 53 #define COUNTER_MAX 10000
kityann 0:1fcc61be1dcf 54 #define ERROR_RANGE 0.001
kityann 0:1fcc61be1dcf 55
kityann 0:1fcc61be1dcf 56 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
kityann 0:1fcc61be1dcf 57 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:1fcc61be1dcf 58 double s2_R=FIRST_S2_2;//GPSセンサの分散
kityann 0:1fcc61be1dcf 59 double s2_Q=FIRST_S2_2;
kityann 0:1fcc61be1dcf 60 double Kx=0,Ky=0;//カルマンゲイン
kityann 0:1fcc61be1dcf 61 double zx,zy;//観測値
kityann 0:1fcc61be1dcf 62 void Kalman(double Latitude,double Longitude);
kityann 0:1fcc61be1dcf 63 int change = 0;
kityann 0:1fcc61be1dcf 64
kityann 0:1fcc61be1dcf 65
kityann 0:1fcc61be1dcf 66
kityann 0:1fcc61be1dcf 67 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 68 //
kityann 0:1fcc61be1dcf 69 //Kalman Processing
kityann 0:1fcc61be1dcf 70 //
kityann 0:1fcc61be1dcf 71 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 72 void calc_Kalman(){
kityann 0:1fcc61be1dcf 73 //calc Kalman gain
kityann 0:1fcc61be1dcf 74 Kx = (s2x_prev+s2_Q)/(s2x_prev+s2_R+s2_Q);
kityann 0:1fcc61be1dcf 75 Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q);
kityann 0:1fcc61be1dcf 76 //estimate
kityann 0:1fcc61be1dcf 77 x_cur = x_prev + Kx*(zx-x_prev);
kityann 0:1fcc61be1dcf 78 y_cur = y_prev + Ky*(zy-y_prev);
kityann 0:1fcc61be1dcf 79 //calc sigma
kityann 0:1fcc61be1dcf 80 s2x_cur = (1-Kx)*(s2x_prev+s2_Q);
kityann 0:1fcc61be1dcf 81 s2y_cur = (1-Ky)*(s2y_prev+s2_Q);
kityann 0:1fcc61be1dcf 82
kityann 0:1fcc61be1dcf 83 }
kityann 0:1fcc61be1dcf 84
kityann 0:1fcc61be1dcf 85 void Kalman(double Latitude,double Longitude){
kityann 0:1fcc61be1dcf 86
kityann 0:1fcc61be1dcf 87 zx = Latitude;
kityann 0:1fcc61be1dcf 88 zy = Longitude;
kityann 0:1fcc61be1dcf 89
kityann 0:1fcc61be1dcf 90 calc_Kalman();
kityann 0:1fcc61be1dcf 91
kityann 0:1fcc61be1dcf 92 //更新
kityann 0:1fcc61be1dcf 93 x_prev = x_cur;
kityann 0:1fcc61be1dcf 94 y_prev = y_cur;
kityann 0:1fcc61be1dcf 95 s2x_prev = s2x_cur;
kityann 0:1fcc61be1dcf 96 s2y_prev = s2y_cur;
kityann 0:1fcc61be1dcf 97
kityann 0:1fcc61be1dcf 98 //agzPontKalmanとagzCovに格納する
kityann 0:1fcc61be1dcf 99 agz.set_agzPointKalman_lati(x_cur);
kityann 0:1fcc61be1dcf 100 agz.set_agzPointKalman_longi(y_cur);
kityann 0:1fcc61be1dcf 101 agz.set_agzCov(s2x_cur,s2y_cur);
kityann 0:1fcc61be1dcf 102
kityann 0:1fcc61be1dcf 103 }
kityann 0:1fcc61be1dcf 104
kityann 0:1fcc61be1dcf 105 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 106 //
kityann 0:1fcc61be1dcf 107 //Get GPS function
kityann 0:1fcc61be1dcf 108 //
kityann 0:1fcc61be1dcf 109 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 110
kityann 0:1fcc61be1dcf 111 void Get_GPS(Adafruit_GPS *myGPS){
kityann 0:1fcc61be1dcf 112 static int flag = 0;
kityann 0:1fcc61be1dcf 113
kityann 0:1fcc61be1dcf 114 if (myGPS->fix) {
kityann 0:1fcc61be1dcf 115
kityann 0:1fcc61be1dcf 116 agz.nowStatus = GPS_AVAIL;
kityann 0:1fcc61be1dcf 117 agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
kityann 0:1fcc61be1dcf 118
kityann 0:1fcc61be1dcf 119 if(flag < COUNTER_MAX){
kityann 0:1fcc61be1dcf 120 flag++;
kityann 0:1fcc61be1dcf 121 }
kityann 0:1fcc61be1dcf 122 if(flag == 5){
kityann 0:1fcc61be1dcf 123 x_prev = agz.get_agzPoint_lati();
kityann 0:1fcc61be1dcf 124 y_prev = agz.get_agzPoint_longi();
kityann 0:1fcc61be1dcf 125 }
kityann 0:1fcc61be1dcf 126
kityann 0:1fcc61be1dcf 127 if(flag >= 6){
kityann 0:1fcc61be1dcf 128 if(abs(x_prev - agz.get_agzPoint_lati()) < ERROR_RANGE && abs(y_prev - agz.get_agzPoint_longi()) < ERROR_RANGE){
kityann 0:1fcc61be1dcf 129 Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
kityann 0:1fcc61be1dcf 130 change = 1;
kityann 0:1fcc61be1dcf 131 }
kityann 0:1fcc61be1dcf 132 else{
kityann 0:1fcc61be1dcf 133 change = 0;
kityann 0:1fcc61be1dcf 134 }
kityann 0:1fcc61be1dcf 135 printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
kityann 0:1fcc61be1dcf 136 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 0:1fcc61be1dcf 137 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 0:1fcc61be1dcf 138 agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 0:1fcc61be1dcf 139 }
kityann 0:1fcc61be1dcf 140 else agz.nowStatus = GPS_UNAVAIL;
kityann 0:1fcc61be1dcf 141
kityann 0:1fcc61be1dcf 142 }
kityann 0:1fcc61be1dcf 143
kityann 0:1fcc61be1dcf 144 /******************************
kityann 0:1fcc61be1dcf 145 スタンバイモード
kityann 0:1fcc61be1dcf 146 ******************************/
kityann 0:1fcc61be1dcf 147 void standby(){
kityann 0:1fcc61be1dcf 148 ;
kityann 0:1fcc61be1dcf 149 }
kityann 0:1fcc61be1dcf 150
kityann 0:1fcc61be1dcf 151 /******************************
kityann 0:1fcc61be1dcf 152 落下モード
kityann 0:1fcc61be1dcf 153 ******************************/
kityann 0:1fcc61be1dcf 154 void falling(){
kityann 0:1fcc61be1dcf 155
kityann 0:1fcc61be1dcf 156 }
kityann 0:1fcc61be1dcf 157
kityann 0:1fcc61be1dcf 158 /******************************
kityann 0:1fcc61be1dcf 159 走行モード
kityann 0:1fcc61be1dcf 160 ******************************/
kityann 0:1fcc61be1dcf 161 void running(){
kityann 0:1fcc61be1dcf 162
kityann 0:1fcc61be1dcf 163 }
kityann 0:1fcc61be1dcf 164
kityann 0:1fcc61be1dcf 165 /******************************
kityann 0:1fcc61be1dcf 166 停止モード
kityann 0:1fcc61be1dcf 167 ******************************/
kityann 0:1fcc61be1dcf 168 void stopping(){
kityann 0:1fcc61be1dcf 169
kityann 0:1fcc61be1dcf 170
kityann 0:1fcc61be1dcf 171 }
kityann 0:1fcc61be1dcf 172
kityann 0:1fcc61be1dcf 173 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 174 //
kityann 0:1fcc61be1dcf 175 //Main Processing
kityann 0:1fcc61be1dcf 176 //
kityann 0:1fcc61be1dcf 177 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 178 int main() {
kityann 0:1fcc61be1dcf 179 //start up time
kityann 0:1fcc61be1dcf 180 wait(3);
kityann 0:1fcc61be1dcf 181 //set pc frequency to 57600bps
kityann 0:1fcc61be1dcf 182 pc.baud(PC_BAUD_RATE);
kityann 0:1fcc61be1dcf 183 //set xbee frequency to 57600bps
kityann 0:1fcc61be1dcf 184 xbee.begin(XBEE_BAUD_RATE);
kityann 0:1fcc61be1dcf 185
kityann 0:1fcc61be1dcf 186
kityann 0:1fcc61be1dcf 187 //GPS setting
kityann 0:1fcc61be1dcf 188 gps_Serial = new Serial(p28,p27);
kityann 0:1fcc61be1dcf 189 Adafruit_GPS myGPS(gps_Serial);
kityann 0:1fcc61be1dcf 190 Timer refresh_Timer;
kityann 0:1fcc61be1dcf 191 const int refresh_Time = 1000; //refresh time in ms
kityann 0:1fcc61be1dcf 192 Timer auto_Timer;
kityann 0:1fcc61be1dcf 193 const int auto_Time = 100; //refresh time in ms
kityann 0:1fcc61be1dcf 194 int count = 0;
kityann 0:1fcc61be1dcf 195
kityann 0:1fcc61be1dcf 196 myGPS.begin(GPS_BAUD_RATE);
kityann 0:1fcc61be1dcf 197
kityann 0:1fcc61be1dcf 198 //GPS Send Command
kityann 0:1fcc61be1dcf 199 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:1fcc61be1dcf 200 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:1fcc61be1dcf 201 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:1fcc61be1dcf 202
kityann 0:1fcc61be1dcf 203 wait_ms(2000);
kityann 0:1fcc61be1dcf 204
kityann 0:1fcc61be1dcf 205 //interrupt start
kityann 0:1fcc61be1dcf 206 refresh_Timer.start();
kityann 0:1fcc61be1dcf 207
kityann 0:1fcc61be1dcf 208 printf("start\n");
kityann 0:1fcc61be1dcf 209
kityann 0:1fcc61be1dcf 210 int mode = -1;
kityann 0:1fcc61be1dcf 211
kityann 0:1fcc61be1dcf 212 while (true) {
kityann 0:1fcc61be1dcf 213
kityann 0:1fcc61be1dcf 214 switch(mode){
kityann 0:1fcc61be1dcf 215 //スタートモード：パラシュートが開くまではこのモードを実行
kityann 0:1fcc61be1dcf 216 case -1:
kityann 0:1fcc61be1dcf 217 standby();
kityann 0:1fcc61be1dcf 218 break;
kityann 0:1fcc61be1dcf 219 //落下モード：落下時はこのモード。気圧計または、時間でロボットとパラシュートを分離する
kityann 0:1fcc61be1dcf 220 case 1:
kityann 0:1fcc61be1dcf 221 falling();
kityann 0:1fcc61be1dcf 222 break;
kityann 0:1fcc61be1dcf 223 //走行モード：ターゲットにむかって走行を行う
kityann 0:1fcc61be1dcf 224 case 2:
kityann 0:1fcc61be1dcf 225 running();
kityann 0:1fcc61be1dcf 226 break;
kityann 0:1fcc61be1dcf 227 //停止モード：ターゲット
kityann 0:1fcc61be1dcf 228 case 100:
kityann 0:1fcc61be1dcf 229 stopping();
kityann 0:1fcc61be1dcf 230 break;
kityann 0:1fcc61be1dcf 231 }
kityann 0:1fcc61be1dcf 232
kityann 0:1fcc61be1dcf 233 myGPS.read();
kityann 0:1fcc61be1dcf 234 //recive gps module
kityann 0:1fcc61be1dcf 235 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:1fcc61be1dcf 236 if ( myGPS.newNMEAreceived() ) {
kityann 0:1fcc61be1dcf 237 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:1fcc61be1dcf 238 continue;
kityann 0:1fcc61be1dcf 239 }
kityann 0:1fcc61be1dcf 240 else{
kityann 0:1fcc61be1dcf 241 count++;
kityann 0:1fcc61be1dcf 242 }
kityann 0:1fcc61be1dcf 243 }
kityann 0:1fcc61be1dcf 244 //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
kityann 0:1fcc61be1dcf 245 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:1fcc61be1dcf 246 refresh_Timer.reset();
kityann 0:1fcc61be1dcf 247 //print_gps(count);
kityann 0:1fcc61be1dcf 248 Get_GPS(&myGPS);
kityann 0:1fcc61be1dcf 249
kityann 0:1fcc61be1dcf 250 }
kityann 0:1fcc61be1dcf 251 }
kityann 0:1fcc61be1dcf 252
kityann 0:1fcc61be1dcf 253 }