CanSat2016aizu / Mbed 2 deprecated Cansat_program1

cansat program1

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

Fork of Cansat_program4_1 by CanSat2015aizu

main.cpp@3:0bd9ad37f319, 2015-07-23 (annotated)

Committer:
kityann
Date:
Thu Jul 23 08:55:47 2015 +0000
Revision:
3:0bd9ad37f319
Parent:
1:f1f7413ae6bd
Child:
4:0fc7221e2f79
2015/07/23

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