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@6:23bb3b018f44, 2015-08-07 (annotated)

Committer:
s1200058
Date:
Fri Aug 07 14:35:26 2015 +0000
Revision:
6:23bb3b018f44
Parent:
5:ba883a4bddc3
Child:
7:db6b436c0baa
add programs. for about control_motor

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 6:23bb3b018f44 253 cansat.control_Motor(1, cansat.get_speed());
s1200058 5:ba883a4bddc3 254 if(short_in == 0){
s1200058 5:ba883a4bddc3 255 mode = 1;
s1200058 5:ba883a4bddc3 256 }
s1200058 5:ba883a4bddc3 257
kityann 0:1fcc61be1dcf 258 }
kityann 0:1fcc61be1dcf 259
kityann 0:1fcc61be1dcf 260 /******************************
kityann 0:1fcc61be1dcf 261 落下モード
kityann 0:1fcc61be1dcf 262 ******************************/
kityann 0:1fcc61be1dcf 263 void falling(){
s1200058 5:ba883a4bddc3 264 if(cansat.get_pressure() >= goal_Pressure){
s1200058 5:ba883a4bddc3 265 nic = 1;
s1200058 6:23bb3b018f44 266 wait_ms(10000);
s1200058 5:ba883a4bddc3 267 mode = 2;
s1200058 5:ba883a4bddc3 268 }
kityann 0:1fcc61be1dcf 269 }
kityann 0:1fcc61be1dcf 270
kityann 0:1fcc61be1dcf 271 /******************************
kityann 0:1fcc61be1dcf 272 走行モード
kityann 0:1fcc61be1dcf 273 ******************************/
kityann 0:1fcc61be1dcf 274 void running(){
aoki0731 4:0fc7221e2f79 275 double r = 6378.137;
s1200058 5:ba883a4bddc3 276 double y1 = cansat.get_target_y();
s1200058 5:ba883a4bddc3 277 double y2 = cansat.get_robot_y();
s1200058 5:ba883a4bddc3 278 double x1 = cansat.get_target_x();
s1200058 5:ba883a4bddc3 279 double x2 = cansat.get_robot_x();
s1200058 5:ba883a4bddc3 280 double dx = x2 - x1;
s1200058 5:ba883a4bddc3 281
s1200058 5:ba883a4bddc3 282 cansat.set_target_distance(r*acos(sin(y1)*sin(y2)+cos(y1)*cos(y2)*cos(dx)));
s1200058 5:ba883a4bddc3 283
s1200058 5:ba883a4bddc3 284 if(cansat.get_target_distance() < 10) cansat.set_speed(32);
s1200058 5:ba883a4bddc3 285 else if(cansat.get_target_distance() < 20 && cansat.get_target_distance() > 10) cansat.set_speed(64);
s1200058 5:ba883a4bddc3 286 else cansat.set_speed(128);
s1200058 5:ba883a4bddc3 287
s1200058 5:ba883a4bddc3 288 if(cansat.get_compass_z() < 0) {
aoki0731 4:0fc7221e2f79 289 //ひっくり返っている
s1200058 5:ba883a4bddc3 290 cansat.control_Motor(0, cansat.get_speed());
s1200058 5:ba883a4bddc3 291 } else {
s1200058 5:ba883a4bddc3 292 switch(robot_Action(cansat.get_robot_angle(), cansat.get_target_angle())) {
s1200058 5:ba883a4bddc3 293 case 'f': //前進
s1200058 5:ba883a4bddc3 294 cansat.control_Motor(0, cansat.get_speed());
s1200058 5:ba883a4bddc3 295 break;
s1200058 5:ba883a4bddc3 296 case 'l':
s1200058 5:ba883a4bddc3 297 cansat.control_Motor(2, cansat.get_speed());
s1200058 5:ba883a4bddc3 298 break;
s1200058 5:ba883a4bddc3 299 case 'r':
s1200058 5:ba883a4bddc3 300 cansat.control_Motor(3, cansat.get_speed());
s1200058 5:ba883a4bddc3 301 break;
s1200058 5:ba883a4bddc3 302 }
s1200058 5:ba883a4bddc3 303 }
s1200058 5:ba883a4bddc3 304
s1200058 6:23bb3b018f44 305 if(cansat.get_robot_x() - cansat.get_target_x() <= 0 && cansat.get_robot_y() - cansat.get_target_y() <= 0){
s1200058 6:23bb3b018f44 306 mode = 100;
s1200058 6:23bb3b018f44 307 }
kityann 0:1fcc61be1dcf 308 }
kityann 0:1fcc61be1dcf 309
kityann 0:1fcc61be1dcf 310 /******************************
kityann 0:1fcc61be1dcf 311 停止モード
kityann 0:1fcc61be1dcf 312 ******************************/
kityann 0:1fcc61be1dcf 313 void stopping(){
kityann 0:1fcc61be1dcf 314
s1200058 6:23bb3b018f44 315 cansat.control_Motor(0, cansat.get_speed());
kityann 0:1fcc61be1dcf 316
kityann 0:1fcc61be1dcf 317 }
kityann 0:1fcc61be1dcf 318
kityann 0:1fcc61be1dcf 319 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 320 //
kityann 0:1fcc61be1dcf 321 //Main Processing
kityann 0:1fcc61be1dcf 322 //
kityann 0:1fcc61be1dcf 323 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 324 int main() {
kityann 0:1fcc61be1dcf 325 //start up time
kityann 0:1fcc61be1dcf 326 wait(3);
kityann 0:1fcc61be1dcf 327 //set pc frequency to 57600bps
kityann 0:1fcc61be1dcf 328 pc.baud(PC_BAUD_RATE);
kityann 0:1fcc61be1dcf 329 //set xbee frequency to 57600bps
kityann 0:1fcc61be1dcf 330 xbee.begin(XBEE_BAUD_RATE);
kityann 0:1fcc61be1dcf 331
kityann 0:1fcc61be1dcf 332
kityann 0:1fcc61be1dcf 333 //GPS setting
kityann 0:1fcc61be1dcf 334 gps_Serial = new Serial(p28,p27);
kityann 0:1fcc61be1dcf 335 Adafruit_GPS myGPS(gps_Serial);
kityann 0:1fcc61be1dcf 336 Timer refresh_Timer;
kityann 0:1fcc61be1dcf 337 const int refresh_Time = 1000; //refresh time in ms
kityann 0:1fcc61be1dcf 338 int count = 0;
kityann 0:1fcc61be1dcf 339
kityann 0:1fcc61be1dcf 340 myGPS.begin(GPS_BAUD_RATE);
kityann 0:1fcc61be1dcf 341
kityann 0:1fcc61be1dcf 342 //GPS Send Command
kityann 0:1fcc61be1dcf 343 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:1fcc61be1dcf 344 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:1fcc61be1dcf 345 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:1fcc61be1dcf 346
kityann 0:1fcc61be1dcf 347 wait_ms(2000);
kityann 0:1fcc61be1dcf 348
kityann 0:1fcc61be1dcf 349 //interrupt start
kityann 0:1fcc61be1dcf 350 refresh_Timer.start();
kityann 0:1fcc61be1dcf 351
kityann 0:1fcc61be1dcf 352 printf("start\n");
kityann 0:1fcc61be1dcf 353
s1200058 5:ba883a4bddc3 354 // int mode = -1;
s1200058 5:ba883a4bddc3 355 short_out = 1; //ショートピンの出力:high
s1200058 5:ba883a4bddc3 356 nic.output();
s1200058 5:ba883a4bddc3 357 nic = 0;
kityann 0:1fcc61be1dcf 358
kityann 0:1fcc61be1dcf 359 while (true) {
kityann 0:1fcc61be1dcf 360
kityann 0:1fcc61be1dcf 361 switch(mode){
kityann 0:1fcc61be1dcf 362 //スタートモード：パラシュートが開くまではこのモードを実行
kityann 0:1fcc61be1dcf 363 case -1:
kityann 0:1fcc61be1dcf 364 standby();
kityann 0:1fcc61be1dcf 365 break;
kityann 0:1fcc61be1dcf 366 //落下モード：落下時はこのモード。気圧計または、時間でロボットとパラシュートを分離する
kityann 0:1fcc61be1dcf 367 case 1:
kityann 0:1fcc61be1dcf 368 falling();
kityann 0:1fcc61be1dcf 369 break;
kityann 0:1fcc61be1dcf 370 //走行モード：ターゲットにむかって走行を行う
kityann 0:1fcc61be1dcf 371 case 2:
kityann 0:1fcc61be1dcf 372 running();
kityann 0:1fcc61be1dcf 373 break;
kityann 0:1fcc61be1dcf 374 //停止モード：ターゲット
kityann 0:1fcc61be1dcf 375 case 100:
kityann 0:1fcc61be1dcf 376 stopping();
kityann 0:1fcc61be1dcf 377 break;
kityann 0:1fcc61be1dcf 378 }
kityann 0:1fcc61be1dcf 379
kityann 0:1fcc61be1dcf 380 myGPS.read();
kityann 0:1fcc61be1dcf 381 //recive gps module
kityann 0:1fcc61be1dcf 382 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:1fcc61be1dcf 383 if ( myGPS.newNMEAreceived() ) {
kityann 0:1fcc61be1dcf 384 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:1fcc61be1dcf 385 continue;
kityann 0:1fcc61be1dcf 386 }
kityann 0:1fcc61be1dcf 387 else{
kityann 0:1fcc61be1dcf 388 count++;
kityann 0:1fcc61be1dcf 389 }
kityann 0:1fcc61be1dcf 390 }
kityann 0:1fcc61be1dcf 391 //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
kityann 0:1fcc61be1dcf 392 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:1fcc61be1dcf 393 refresh_Timer.reset();
kityann 0:1fcc61be1dcf 394 //print_gps(count);
kityann 0:1fcc61be1dcf 395 Get_GPS(&myGPS);
s1200058 5:ba883a4bddc3 396 pc.printf("%2.2f degC, %04.2f hPa, %2.2f %%\n", sensor.getTemperature(), sensor.getPressure(), sensor.getHumidity());
kityann 0:1fcc61be1dcf 397 }
kityann 0:1fcc61be1dcf 398 }
kityann 0:1fcc61be1dcf 399
kityann 0:1fcc61be1dcf 400 }