2015.07.23
Dependencies: ADXL345 CanSat_aoki HMC5883L ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed
Fork of Cansat_program by
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?
User | Revision | Line number | New 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 | } |