CanSat2015aizu / Mbed 2 deprecated Cansat_program4

change "cansat.cpp"

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

Fork of Cansat_program3 by CanSat2015aizu

main.cpp@5:ba883a4bddc3, 2015-08-06 (annotated)

Committer:
s1200058
Date:
Thu Aug 06 17:10:30 2015 +0000
Revision:
5:ba883a4bddc3
Parent:
4:0fc7221e2f79
Child:
6:23bb3b018f44
Child:
8:ca21b4e8a350
debug this program. "mode" became global argument. we can change "mode" when pin's output change, pressure change, and so on.

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"
s1200058 5:ba883a4bddc3 16 #include "BME280.h"
kityann 0:1fcc61be1dcf 17
kityann 0:1fcc61be1dcf 18 #define SIGMA_MIN 0.0001
kityann 0:1fcc61be1dcf 19
kityann 0:1fcc61be1dcf 20 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 21 //
kityann 0:1fcc61be1dcf 22 //Pin Setting
kityann 0:1fcc61be1dcf 23 //
kityann 0:1fcc61be1dcf 24 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 25 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:1fcc61be1dcf 26
kityann 0:1fcc61be1dcf 27
kityann 0:1fcc61be1dcf 28 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 29 //
kityann 0:1fcc61be1dcf 30 //Connection Setting
kityann 0:1fcc61be1dcf 31 //
kityann 0:1fcc61be1dcf 32 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 33
kityann 0:1fcc61be1dcf 34 //Serial Connect Setting: PC <--> mbed
kityann 0:1fcc61be1dcf 35 Serial pc(USBTX, USBRX);
kityann 0:1fcc61be1dcf 36
kityann 0:1fcc61be1dcf 37 //Serial Connect Setting: GPS <--> mbed
kityann 0:1fcc61be1dcf 38 Serial * gps_Serial;
kityann 0:1fcc61be1dcf 39
kityann 0:1fcc61be1dcf 40 //Serial Connect Setting: XBEE <--> mbed
kityann 0:1fcc61be1dcf 41 XBee xbee(p13,p14);
kityann 0:1fcc61be1dcf 42 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:1fcc61be1dcf 43
kityann 0:1fcc61be1dcf 44 //set up GPS module
kityann 0:1fcc61be1dcf 45
kityann 0:1fcc61be1dcf 46 //set up AigamozuControlPackets library
kityann 1:f1f7413ae6bd 47 //AigamozuControlPackets agz(agz_motorShield);
kityann 3:0bd9ad37f319 48 CanSat cansat(agz_motorShield);
kityann 0:1fcc61be1dcf 49
s1200058 5:ba883a4bddc3 50 //set up for tempratures...
s1200058 5:ba883a4bddc3 51 #if defined(TARGET_LPC1768)
s1200058 5:ba883a4bddc3 52 BME280 sensor(p9, p10);
s1200058 5:ba883a4bddc3 53 #else
s1200058 5:ba883a4bddc3 54 BME280 sensor(I2C_SDA, I2C_SCL);
s1200058 5:ba883a4bddc3 55 #endif
s1200058 5:ba883a4bddc3 56
s1200058 5:ba883a4bddc3 57 DigitalIn short_in(p29);
s1200058 5:ba883a4bddc3 58 DigitalOut short_out(p30);
s1200058 5:ba883a4bddc3 59 DigitalInOut nic(p5);
kityann 0:1fcc61be1dcf 60
kityann 0:1fcc61be1dcf 61 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 62 //
kityann 0:1fcc61be1dcf 63 //For Kalman data
kityann 0:1fcc61be1dcf 64 //
kityann 0:1fcc61be1dcf 65 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 66 #define FIRST_S2_1 1.0e-8
kityann 0:1fcc61be1dcf 67 #define FIRST_S2_2 1.0e-6
kityann 0:1fcc61be1dcf 68 #define COUNTER_MAX 10000
kityann 0:1fcc61be1dcf 69 #define ERROR_RANGE 0.001
kityann 0:1fcc61be1dcf 70
kityann 0:1fcc61be1dcf 71 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
kityann 0:1fcc61be1dcf 72 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 73 double s2_R=FIRST_S2_2;//GPSセンサの分散
kityann 0:1fcc61be1dcf 74 double s2_Q=FIRST_S2_2;
kityann 0:1fcc61be1dcf 75 double Kx=0,Ky=0;//カルマンゲイン
kityann 0:1fcc61be1dcf 76 double zx,zy;//観測値
kityann 0:1fcc61be1dcf 77 void Kalman(double Latitude,double Longitude);
kityann 0:1fcc61be1dcf 78 int change = 0;
kityann 0:1fcc61be1dcf 79
s1200058 5:ba883a4bddc3 80 int mode = -1; //ロボットのモード
s1200058 5:ba883a4bddc3 81 double goal_Pressure, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度
kityann 0:1fcc61be1dcf 82
kityann 0:1fcc61be1dcf 83
kityann 0:1fcc61be1dcf 84 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 85 //
kityann 0:1fcc61be1dcf 86 //Kalman Processing
kityann 0:1fcc61be1dcf 87 //
kityann 0:1fcc61be1dcf 88 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 89 void calc_Kalman(){
kityann 0:1fcc61be1dcf 90 //calc Kalman gain
kityann 0:1fcc61be1dcf 91 Kx = (s2x_prev+s2_Q)/(s2x_prev+s2_R+s2_Q);
kityann 0:1fcc61be1dcf 92 Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q);
kityann 0:1fcc61be1dcf 93 //estimate
kityann 0:1fcc61be1dcf 94 x_cur = x_prev + Kx*(zx-x_prev);
kityann 0:1fcc61be1dcf 95 y_cur = y_prev + Ky*(zy-y_prev);
kityann 0:1fcc61be1dcf 96 //calc sigma
kityann 0:1fcc61be1dcf 97 s2x_cur = (1-Kx)*(s2x_prev+s2_Q);
kityann 0:1fcc61be1dcf 98 s2y_cur = (1-Ky)*(s2y_prev+s2_Q);
kityann 0:1fcc61be1dcf 99
kityann 0:1fcc61be1dcf 100 }
kityann 0:1fcc61be1dcf 101
kityann 0:1fcc61be1dcf 102 void Kalman(double Latitude,double Longitude){
kityann 0:1fcc61be1dcf 103
kityann 0:1fcc61be1dcf 104 zx = Latitude;
kityann 0:1fcc61be1dcf 105 zy = Longitude;
kityann 0:1fcc61be1dcf 106
kityann 0:1fcc61be1dcf 107 calc_Kalman();
kityann 0:1fcc61be1dcf 108
kityann 0:1fcc61be1dcf 109 //更新
kityann 0:1fcc61be1dcf 110 x_prev = x_cur;
kityann 0:1fcc61be1dcf 111 y_prev = y_cur;
kityann 0:1fcc61be1dcf 112 s2x_prev = s2x_cur;
kityann 0:1fcc61be1dcf 113 s2y_prev = s2y_cur;
kityann 0:1fcc61be1dcf 114
kityann 3:0bd9ad37f319 115 //robotK\x,robotK_yに格納する
kityann 3:0bd9ad37f319 116 cansat.set_robotKalman_x(x_cur);
kityann 3:0bd9ad37f319 117 cansat.set_robotKalman_y(y_cur);
kityann 0:1fcc61be1dcf 118 }
kityann 0:1fcc61be1dcf 119
kityann 0:1fcc61be1dcf 120 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 121 //
kityann 0:1fcc61be1dcf 122 //Get GPS function
kityann 0:1fcc61be1dcf 123 //
kityann 0:1fcc61be1dcf 124 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 125
kityann 0:1fcc61be1dcf 126 void Get_GPS(Adafruit_GPS *myGPS){
kityann 0:1fcc61be1dcf 127 static int flag = 0;
kityann 0:1fcc61be1dcf 128
kityann 0:1fcc61be1dcf 129 if (myGPS->fix) {
kityann 0:1fcc61be1dcf 130
kityann 3:0bd9ad37f319 131 cansat.nowStatus = GPS_AVAIL;
kityann 3:0bd9ad37f319 132 cansat.set_robot_x((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 10000.0));
kityann 3:0bd9ad37f319 133 cansat.set_robot_y((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 10000.0));
kityann 3:0bd9ad37f319 134
kityann 0:1fcc61be1dcf 135
kityann 0:1fcc61be1dcf 136 if(flag < COUNTER_MAX){
kityann 0:1fcc61be1dcf 137 flag++;
kityann 0:1fcc61be1dcf 138 }
kityann 0:1fcc61be1dcf 139 if(flag == 5){
kityann 3:0bd9ad37f319 140 x_prev = cansat.get_robot_x();
kityann 3:0bd9ad37f319 141 y_prev = cansat.get_robot_y();
kityann 0:1fcc61be1dcf 142 }
kityann 0:1fcc61be1dcf 143
kityann 0:1fcc61be1dcf 144 if(flag >= 6){
kityann 3:0bd9ad37f319 145 if(abs(x_prev - cansat.get_robot_x()) < ERROR_RANGE && abs(y_prev - cansat.get_robot_y()) < ERROR_RANGE){
kityann 3:0bd9ad37f319 146 Kalman(cansat.get_robot_x(), cansat.get_robot_y());
kityann 0:1fcc61be1dcf 147 change = 1;
kityann 0:1fcc61be1dcf 148 }
kityann 0:1fcc61be1dcf 149 else{
kityann 0:1fcc61be1dcf 150 change = 0;
kityann 0:1fcc61be1dcf 151 }
kityann 1:f1f7413ae6bd 152 //printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
kityann 1:f1f7413ae6bd 153 //agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 1:f1f7413ae6bd 154 //agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 1:f1f7413ae6bd 155 //agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 1:f1f7413ae6bd 156 }
kityann 0:1fcc61be1dcf 157 }
kityann 3:0bd9ad37f319 158 else cansat.nowStatus = GPS_UNAVAIL;
kityann 0:1fcc61be1dcf 159
kityann 0:1fcc61be1dcf 160 }
kityann 0:1fcc61be1dcf 161
aoki0731 4:0fc7221e2f79 162 //ロボットの動き方
aoki0731 4:0fc7221e2f79 163 char robot_Action(double robot_angle, double target_angle) {
aoki0731 4:0fc7221e2f79 164
aoki0731 4:0fc7221e2f79 165 double n, t, r;
aoki0731 4:0fc7221e2f79 166 t = target_angle;
aoki0731 4:0fc7221e2f79 167 r = robot_angle;
aoki0731 4:0fc7221e2f79 168 n = r - t;
aoki0731 4:0fc7221e2f79 169 if(n<0) n *= -1;
aoki0731 4:0fc7221e2f79 170
aoki0731 4:0fc7221e2f79 171
aoki0731 4:0fc7221e2f79 172 if(t==r) {
aoki0731 4:0fc7221e2f79 173 //前進
aoki0731 4:0fc7221e2f79 174 return 'f';
aoki0731 4:0fc7221e2f79 175 } else if(n < 4) {
aoki0731 4:0fc7221e2f79 176 if(t > r)
aoki0731 4:0fc7221e2f79 177 t -= 8;
aoki0731 4:0fc7221e2f79 178 else
aoki0731 4:0fc7221e2f79 179 r -= 8;
aoki0731 4:0fc7221e2f79 180
aoki0731 4:0fc7221e2f79 181 if(r-t > 0)
aoki0731 4:0fc7221e2f79 182 return 'l';
aoki0731 4:0fc7221e2f79 183 else
aoki0731 4:0fc7221e2f79 184 return 'r';
aoki0731 4:0fc7221e2f79 185 } else if(n >= 4) {
aoki0731 4:0fc7221e2f79 186 if(t > r)
aoki0731 4:0fc7221e2f79 187 t -= 8;
aoki0731 4:0fc7221e2f79 188 else
aoki0731 4:0fc7221e2f79 189 r -= 8;
aoki0731 4:0fc7221e2f79 190
aoki0731 4:0fc7221e2f79 191 if(r-t > 0)
aoki0731 4:0fc7221e2f79 192 return 'r';
aoki0731 4:0fc7221e2f79 193 else
aoki0731 4:0fc7221e2f79 194 return 'l';
aoki0731 4:0fc7221e2f79 195 }
aoki0731 4:0fc7221e2f79 196
aoki0731 4:0fc7221e2f79 197 return 'b';
aoki0731 4:0fc7221e2f79 198 }
aoki0731 4:0fc7221e2f79 199
s1200058 5:ba883a4bddc3 200 //対象とロボットの角度
s1200058 5:ba883a4bddc3 201 double robot_compass(double robot_x, double robot_y) {
s1200058 5:ba883a4bddc3 202 double angle = 0;
s1200058 5:ba883a4bddc3 203
s1200058 5:ba883a4bddc3 204 if(robot_x==0&&robot_y>0)
s1200058 5:ba883a4bddc3 205 return 0;
s1200058 5:ba883a4bddc3 206 else if(robot_x>0&&robot_y==0) //東
s1200058 5:ba883a4bddc3 207 return 90;
s1200058 5:ba883a4bddc3 208 else if(robot_x==0&&robot_y<0) //南
s1200058 5:ba883a4bddc3 209 return 180;
s1200058 5:ba883a4bddc3 210 else if(robot_x<0&&robot_y==0) //西
s1200058 5:ba883a4bddc3 211 return 270;
s1200058 5:ba883a4bddc3 212 else if(robot_x>=0&&robot_y>=0) { //北東
s1200058 5:ba883a4bddc3 213 if(robot_x<=robot_y)
s1200058 5:ba883a4bddc3 214 angle = atan2(robot_x, robot_y);
s1200058 5:ba883a4bddc3 215 else
s1200058 5:ba883a4bddc3 216 angle = (M_PI/2) - atan2(robot_y, robot_x);
s1200058 5:ba883a4bddc3 217 return angle * 180.0 / M_PI;
s1200058 5:ba883a4bddc3 218 } else if(robot_x>=0&&robot_y<0) { //南東
s1200058 5:ba883a4bddc3 219 if(robot_x>abs(robot_y)){
s1200058 5:ba883a4bddc3 220 angle = (M_PI/2) - atan2(abs(robot_y), robot_x);
s1200058 5:ba883a4bddc3 221 }
s1200058 5:ba883a4bddc3 222 else{
s1200058 5:ba883a4bddc3 223 angle = atan2(abs(robot_y), robot_x);
s1200058 5:ba883a4bddc3 224 }
s1200058 5:ba883a4bddc3 225 return angle * 180.0 / M_PI + 90;
s1200058 5:ba883a4bddc3 226 } else if(robot_x<0&&robot_y<0) { //南西
s1200058 5:ba883a4bddc3 227 if(abs(robot_x)<abs(robot_y)){
s1200058 5:ba883a4bddc3 228 angle = atan2(abs(robot_x), abs(robot_y));
s1200058 5:ba883a4bddc3 229 }
s1200058 5:ba883a4bddc3 230 else{
s1200058 5:ba883a4bddc3 231 angle = (M_PI/2) - atan2(abs(robot_y), abs(robot_x));
s1200058 5:ba883a4bddc3 232 }
s1200058 5:ba883a4bddc3 233 return angle * 180.0 / M_PI + 180;
s1200058 5:ba883a4bddc3 234 } else if(robot_x<0&&robot_y>=0) { //北西
s1200058 5:ba883a4bddc3 235 if(abs(robot_x)>robot_y){
s1200058 5:ba883a4bddc3 236 angle = (M_PI/2) - atan2(robot_y, abs(robot_x));
s1200058 5:ba883a4bddc3 237 }
s1200058 5:ba883a4bddc3 238 else{
s1200058 5:ba883a4bddc3 239 angle = atan2(abs(robot_x), robot_y);
s1200058 5:ba883a4bddc3 240 }
s1200058 5:ba883a4bddc3 241 return angle * 180.0 / M_PI + 270;
s1200058 5:ba883a4bddc3 242 }
s1200058 5:ba883a4bddc3 243
s1200058 5:ba883a4bddc3 244 return -1;
s1200058 5:ba883a4bddc3 245 }
s1200058 5:ba883a4bddc3 246
s1200058 5:ba883a4bddc3 247
kityann 0:1fcc61be1dcf 248 /******************************
kityann 0:1fcc61be1dcf 249 スタンバイモード
kityann 0:1fcc61be1dcf 250 ******************************/
kityann 0:1fcc61be1dcf 251 void standby(){
s1200058 5:ba883a4bddc3 252
s1200058 5:ba883a4bddc3 253 if(short_in == 0){
s1200058 5:ba883a4bddc3 254 mode = 1;
s1200058 5:ba883a4bddc3 255 }
s1200058 5:ba883a4bddc3 256
kityann 0:1fcc61be1dcf 257 }
kityann 0:1fcc61be1dcf 258
kityann 0:1fcc61be1dcf 259 /******************************
kityann 0:1fcc61be1dcf 260 落下モード
kityann 0:1fcc61be1dcf 261 ******************************/
kityann 0:1fcc61be1dcf 262 void falling(){
s1200058 5:ba883a4bddc3 263 if(cansat.get_pressure() >= goal_Pressure){
s1200058 5:ba883a4bddc3 264 nic = 1;
s1200058 5:ba883a4bddc3 265 mode = 2;
s1200058 5:ba883a4bddc3 266 }
kityann 0:1fcc61be1dcf 267 }
kityann 0:1fcc61be1dcf 268
kityann 0:1fcc61be1dcf 269 /******************************
kityann 0:1fcc61be1dcf 270 走行モード
kityann 0:1fcc61be1dcf 271 ******************************/
kityann 0:1fcc61be1dcf 272 void running(){
aoki0731 4:0fc7221e2f79 273 double r = 6378.137;
s1200058 5:ba883a4bddc3 274 double y1 = cansat.get_target_y();
s1200058 5:ba883a4bddc3 275 double y2 = cansat.get_robot_y();
s1200058 5:ba883a4bddc3 276 double x1 = cansat.get_target_x();
s1200058 5:ba883a4bddc3 277 double x2 = cansat.get_robot_x();
s1200058 5:ba883a4bddc3 278 double dx = x2 - x1;
s1200058 5:ba883a4bddc3 279
s1200058 5:ba883a4bddc3 280 cansat.set_target_distance(r*acos(sin(y1)*sin(y2)+cos(y1)*cos(y2)*cos(dx)));
s1200058 5:ba883a4bddc3 281
s1200058 5:ba883a4bddc3 282 if(cansat.get_target_distance() < 10) cansat.set_speed(32);
s1200058 5:ba883a4bddc3 283 else if(cansat.get_target_distance() < 20 && cansat.get_target_distance() > 10) cansat.set_speed(64);
s1200058 5:ba883a4bddc3 284 else cansat.set_speed(128);
s1200058 5:ba883a4bddc3 285
s1200058 5:ba883a4bddc3 286 if(cansat.get_compass_z() < 0) {
aoki0731 4:0fc7221e2f79 287 //ひっくり返っている
s1200058 5:ba883a4bddc3 288 cansat.control_Motor(0, cansat.get_speed());
s1200058 5:ba883a4bddc3 289 } else {
s1200058 5:ba883a4bddc3 290 switch(robot_Action(cansat.get_robot_angle(), cansat.get_target_angle())) {
s1200058 5:ba883a4bddc3 291 case 'f': //前進
s1200058 5:ba883a4bddc3 292 cansat.control_Motor(0, cansat.get_speed());
s1200058 5:ba883a4bddc3 293 break;
s1200058 5:ba883a4bddc3 294 case 'l':
s1200058 5:ba883a4bddc3 295 cansat.control_Motor(2, cansat.get_speed());
s1200058 5:ba883a4bddc3 296 break;
s1200058 5:ba883a4bddc3 297 case 'r':
s1200058 5:ba883a4bddc3 298 cansat.control_Motor(3, cansat.get_speed());
s1200058 5:ba883a4bddc3 299 break;
s1200058 5:ba883a4bddc3 300 }
s1200058 5:ba883a4bddc3 301 }
s1200058 5:ba883a4bddc3 302
kityann 0:1fcc61be1dcf 303 }
kityann 0:1fcc61be1dcf 304
kityann 0:1fcc61be1dcf 305 /******************************
kityann 0:1fcc61be1dcf 306 停止モード
kityann 0:1fcc61be1dcf 307 ******************************/
kityann 0:1fcc61be1dcf 308 void stopping(){
kityann 0:1fcc61be1dcf 309
kityann 0:1fcc61be1dcf 310
kityann 0:1fcc61be1dcf 311 }
kityann 0:1fcc61be1dcf 312
kityann 0:1fcc61be1dcf 313 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 314 //
kityann 0:1fcc61be1dcf 315 //Main Processing
kityann 0:1fcc61be1dcf 316 //
kityann 0:1fcc61be1dcf 317 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 318 int main() {
kityann 0:1fcc61be1dcf 319 //start up time
kityann 0:1fcc61be1dcf 320 wait(3);
kityann 0:1fcc61be1dcf 321 //set pc frequency to 57600bps
kityann 0:1fcc61be1dcf 322 pc.baud(PC_BAUD_RATE);
kityann 0:1fcc61be1dcf 323 //set xbee frequency to 57600bps
kityann 0:1fcc61be1dcf 324 xbee.begin(XBEE_BAUD_RATE);
kityann 0:1fcc61be1dcf 325
kityann 0:1fcc61be1dcf 326
kityann 0:1fcc61be1dcf 327 //GPS setting
kityann 0:1fcc61be1dcf 328 gps_Serial = new Serial(p28,p27);
kityann 0:1fcc61be1dcf 329 Adafruit_GPS myGPS(gps_Serial);
kityann 0:1fcc61be1dcf 330 Timer refresh_Timer;
kityann 0:1fcc61be1dcf 331 const int refresh_Time = 1000; //refresh time in ms
kityann 0:1fcc61be1dcf 332 int count = 0;
kityann 0:1fcc61be1dcf 333
kityann 0:1fcc61be1dcf 334 myGPS.begin(GPS_BAUD_RATE);
kityann 0:1fcc61be1dcf 335
kityann 0:1fcc61be1dcf 336 //GPS Send Command
kityann 0:1fcc61be1dcf 337 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:1fcc61be1dcf 338 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:1fcc61be1dcf 339 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:1fcc61be1dcf 340
kityann 0:1fcc61be1dcf 341 wait_ms(2000);
kityann 0:1fcc61be1dcf 342
kityann 0:1fcc61be1dcf 343 //interrupt start
kityann 0:1fcc61be1dcf 344 refresh_Timer.start();
kityann 0:1fcc61be1dcf 345
kityann 0:1fcc61be1dcf 346 printf("start\n");
kityann 0:1fcc61be1dcf 347
s1200058 5:ba883a4bddc3 348 // int mode = -1;
s1200058 5:ba883a4bddc3 349 short_out = 1; //ショートピンの出力:high
s1200058 5:ba883a4bddc3 350 nic.output();
s1200058 5:ba883a4bddc3 351 nic = 0;
kityann 0:1fcc61be1dcf 352
kityann 0:1fcc61be1dcf 353 while (true) {
kityann 0:1fcc61be1dcf 354
kityann 0:1fcc61be1dcf 355 switch(mode){
kityann 0:1fcc61be1dcf 356 //スタートモード：パラシュートが開くまではこのモードを実行
kityann 0:1fcc61be1dcf 357 case -1:
kityann 0:1fcc61be1dcf 358 standby();
kityann 0:1fcc61be1dcf 359 break;
kityann 0:1fcc61be1dcf 360 //落下モード：落下時はこのモード。気圧計または、時間でロボットとパラシュートを分離する
kityann 0:1fcc61be1dcf 361 case 1:
kityann 0:1fcc61be1dcf 362 falling();
kityann 0:1fcc61be1dcf 363 break;
kityann 0:1fcc61be1dcf 364 //走行モード：ターゲットにむかって走行を行う
kityann 0:1fcc61be1dcf 365 case 2:
kityann 0:1fcc61be1dcf 366 running();
kityann 0:1fcc61be1dcf 367 break;
kityann 0:1fcc61be1dcf 368 //停止モード：ターゲット
kityann 0:1fcc61be1dcf 369 case 100:
kityann 0:1fcc61be1dcf 370 stopping();
kityann 0:1fcc61be1dcf 371 break;
kityann 0:1fcc61be1dcf 372 }
kityann 0:1fcc61be1dcf 373
kityann 0:1fcc61be1dcf 374 myGPS.read();
kityann 0:1fcc61be1dcf 375 //recive gps module
kityann 0:1fcc61be1dcf 376 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:1fcc61be1dcf 377 if ( myGPS.newNMEAreceived() ) {
kityann 0:1fcc61be1dcf 378 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:1fcc61be1dcf 379 continue;
kityann 0:1fcc61be1dcf 380 }
kityann 0:1fcc61be1dcf 381 else{
kityann 0:1fcc61be1dcf 382 count++;
kityann 0:1fcc61be1dcf 383 }
kityann 0:1fcc61be1dcf 384 }
kityann 0:1fcc61be1dcf 385 //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
kityann 0:1fcc61be1dcf 386 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:1fcc61be1dcf 387 refresh_Timer.reset();
kityann 0:1fcc61be1dcf 388 //print_gps(count);
kityann 0:1fcc61be1dcf 389 Get_GPS(&myGPS);
s1200058 5:ba883a4bddc3 390 pc.printf("%2.2f degC, %04.2f hPa, %2.2f %%\n", sensor.getTemperature(), sensor.getPressure(), sensor.getHumidity());
kityann 0:1fcc61be1dcf 391 }
kityann 0:1fcc61be1dcf 392 }
kityann 0:1fcc61be1dcf 393
kityann 0:1fcc61be1dcf 394 }