CanSat2015aizu / Mbed 2 deprecated Cansat_program3

Add Aoki's program

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

Fork of Cansat_program2 by CanSat2015aizu

main.cpp@7:db6b436c0baa, 2015-08-08 (annotated)

Committer:
s1200058
Date:
Sat Aug 08 16:29:30 2015 +0000
Revision:
7:db6b436c0baa
Parent:
6:23bb3b018f44
Child:
9:2741e17438d6
add programs for serial connection by pc.printf()

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
s1200058 7:db6b436c0baa 83 Timer sep_Timer;
s1200058 7:db6b436c0baa 84 const int sep_Time = 30000; //seperate time in ms
s1200058 7:db6b436c0baa 85 int fall_flag = 0;
s1200058 7:db6b436c0baa 86
kityann 0:1fcc61be1dcf 87
kityann 0:1fcc61be1dcf 88 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 89 //
kityann 0:1fcc61be1dcf 90 //Kalman Processing
kityann 0:1fcc61be1dcf 91 //
kityann 0:1fcc61be1dcf 92 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 93 void calc_Kalman(){
kityann 0:1fcc61be1dcf 94 //calc Kalman gain
kityann 0:1fcc61be1dcf 95 Kx = (s2x_prev+s2_Q)/(s2x_prev+s2_R+s2_Q);
kityann 0:1fcc61be1dcf 96 Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q);
kityann 0:1fcc61be1dcf 97 //estimate
kityann 0:1fcc61be1dcf 98 x_cur = x_prev + Kx*(zx-x_prev);
kityann 0:1fcc61be1dcf 99 y_cur = y_prev + Ky*(zy-y_prev);
kityann 0:1fcc61be1dcf 100 //calc sigma
kityann 0:1fcc61be1dcf 101 s2x_cur = (1-Kx)*(s2x_prev+s2_Q);
kityann 0:1fcc61be1dcf 102 s2y_cur = (1-Ky)*(s2y_prev+s2_Q);
kityann 0:1fcc61be1dcf 103
kityann 0:1fcc61be1dcf 104 }
kityann 0:1fcc61be1dcf 105
kityann 0:1fcc61be1dcf 106 void Kalman(double Latitude,double Longitude){
kityann 0:1fcc61be1dcf 107
kityann 0:1fcc61be1dcf 108 zx = Latitude;
kityann 0:1fcc61be1dcf 109 zy = Longitude;
kityann 0:1fcc61be1dcf 110
kityann 0:1fcc61be1dcf 111 calc_Kalman();
kityann 0:1fcc61be1dcf 112
kityann 0:1fcc61be1dcf 113 //更新
kityann 0:1fcc61be1dcf 114 x_prev = x_cur;
kityann 0:1fcc61be1dcf 115 y_prev = y_cur;
kityann 0:1fcc61be1dcf 116 s2x_prev = s2x_cur;
kityann 0:1fcc61be1dcf 117 s2y_prev = s2y_cur;
kityann 0:1fcc61be1dcf 118
kityann 3:0bd9ad37f319 119 //robotK\x,robotK_yに格納する
kityann 3:0bd9ad37f319 120 cansat.set_robotKalman_x(x_cur);
kityann 3:0bd9ad37f319 121 cansat.set_robotKalman_y(y_cur);
kityann 0:1fcc61be1dcf 122 }
kityann 0:1fcc61be1dcf 123
kityann 0:1fcc61be1dcf 124 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 125 //
kityann 0:1fcc61be1dcf 126 //Get GPS function
kityann 0:1fcc61be1dcf 127 //
kityann 0:1fcc61be1dcf 128 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 129
kityann 0:1fcc61be1dcf 130 void Get_GPS(Adafruit_GPS *myGPS){
kityann 0:1fcc61be1dcf 131 static int flag = 0;
kityann 0:1fcc61be1dcf 132
kityann 0:1fcc61be1dcf 133 if (myGPS->fix) {
kityann 0:1fcc61be1dcf 134
kityann 3:0bd9ad37f319 135 cansat.nowStatus = GPS_AVAIL;
kityann 3:0bd9ad37f319 136 cansat.set_robot_x((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 10000.0));
kityann 3:0bd9ad37f319 137 cansat.set_robot_y((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 10000.0));
kityann 3:0bd9ad37f319 138
kityann 0:1fcc61be1dcf 139
kityann 0:1fcc61be1dcf 140 if(flag < COUNTER_MAX){
kityann 0:1fcc61be1dcf 141 flag++;
kityann 0:1fcc61be1dcf 142 }
kityann 0:1fcc61be1dcf 143 if(flag == 5){
kityann 3:0bd9ad37f319 144 x_prev = cansat.get_robot_x();
kityann 3:0bd9ad37f319 145 y_prev = cansat.get_robot_y();
kityann 0:1fcc61be1dcf 146 }
kityann 0:1fcc61be1dcf 147
kityann 0:1fcc61be1dcf 148 if(flag >= 6){
kityann 3:0bd9ad37f319 149 if(abs(x_prev - cansat.get_robot_x()) < ERROR_RANGE && abs(y_prev - cansat.get_robot_y()) < ERROR_RANGE){
kityann 3:0bd9ad37f319 150 Kalman(cansat.get_robot_x(), cansat.get_robot_y());
kityann 0:1fcc61be1dcf 151 change = 1;
kityann 0:1fcc61be1dcf 152 }
kityann 0:1fcc61be1dcf 153 else{
kityann 0:1fcc61be1dcf 154 change = 0;
kityann 0:1fcc61be1dcf 155 }
kityann 1:f1f7413ae6bd 156 //printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
kityann 1:f1f7413ae6bd 157 //agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 1:f1f7413ae6bd 158 //agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 1:f1f7413ae6bd 159 //agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 1:f1f7413ae6bd 160 }
kityann 0:1fcc61be1dcf 161 }
kityann 3:0bd9ad37f319 162 else cansat.nowStatus = GPS_UNAVAIL;
kityann 0:1fcc61be1dcf 163
kityann 0:1fcc61be1dcf 164 }
kityann 0:1fcc61be1dcf 165
aoki0731 4:0fc7221e2f79 166 //ロボットの動き方
aoki0731 4:0fc7221e2f79 167 char robot_Action(double robot_angle, double target_angle) {
aoki0731 4:0fc7221e2f79 168
aoki0731 4:0fc7221e2f79 169 double n, t, r;
aoki0731 4:0fc7221e2f79 170 t = target_angle;
aoki0731 4:0fc7221e2f79 171 r = robot_angle;
aoki0731 4:0fc7221e2f79 172 n = r - t;
aoki0731 4:0fc7221e2f79 173 if(n<0) n *= -1;
aoki0731 4:0fc7221e2f79 174
aoki0731 4:0fc7221e2f79 175
aoki0731 4:0fc7221e2f79 176 if(t==r) {
aoki0731 4:0fc7221e2f79 177 //前進
aoki0731 4:0fc7221e2f79 178 return 'f';
aoki0731 4:0fc7221e2f79 179 } else if(n < 4) {
aoki0731 4:0fc7221e2f79 180 if(t > r)
aoki0731 4:0fc7221e2f79 181 t -= 8;
aoki0731 4:0fc7221e2f79 182 else
aoki0731 4:0fc7221e2f79 183 r -= 8;
aoki0731 4:0fc7221e2f79 184
aoki0731 4:0fc7221e2f79 185 if(r-t > 0)
aoki0731 4:0fc7221e2f79 186 return 'l';
aoki0731 4:0fc7221e2f79 187 else
aoki0731 4:0fc7221e2f79 188 return 'r';
aoki0731 4:0fc7221e2f79 189 } else if(n >= 4) {
aoki0731 4:0fc7221e2f79 190 if(t > r)
aoki0731 4:0fc7221e2f79 191 t -= 8;
aoki0731 4:0fc7221e2f79 192 else
aoki0731 4:0fc7221e2f79 193 r -= 8;
aoki0731 4:0fc7221e2f79 194
aoki0731 4:0fc7221e2f79 195 if(r-t > 0)
aoki0731 4:0fc7221e2f79 196 return 'r';
aoki0731 4:0fc7221e2f79 197 else
aoki0731 4:0fc7221e2f79 198 return 'l';
aoki0731 4:0fc7221e2f79 199 }
aoki0731 4:0fc7221e2f79 200
aoki0731 4:0fc7221e2f79 201 return 'b';
aoki0731 4:0fc7221e2f79 202 }
aoki0731 4:0fc7221e2f79 203
s1200058 5:ba883a4bddc3 204 //対象とロボットの角度
s1200058 5:ba883a4bddc3 205 double robot_compass(double robot_x, double robot_y) {
s1200058 5:ba883a4bddc3 206 double angle = 0;
s1200058 5:ba883a4bddc3 207
s1200058 5:ba883a4bddc3 208 if(robot_x==0&&robot_y>0)
s1200058 5:ba883a4bddc3 209 return 0;
s1200058 5:ba883a4bddc3 210 else if(robot_x>0&&robot_y==0) //東
s1200058 5:ba883a4bddc3 211 return 90;
s1200058 5:ba883a4bddc3 212 else if(robot_x==0&&robot_y<0) //南
s1200058 5:ba883a4bddc3 213 return 180;
s1200058 5:ba883a4bddc3 214 else if(robot_x<0&&robot_y==0) //西
s1200058 5:ba883a4bddc3 215 return 270;
s1200058 5:ba883a4bddc3 216 else if(robot_x>=0&&robot_y>=0) { //北東
s1200058 5:ba883a4bddc3 217 if(robot_x<=robot_y)
s1200058 5:ba883a4bddc3 218 angle = atan2(robot_x, robot_y);
s1200058 5:ba883a4bddc3 219 else
s1200058 5:ba883a4bddc3 220 angle = (M_PI/2) - atan2(robot_y, robot_x);
s1200058 5:ba883a4bddc3 221 return angle * 180.0 / M_PI;
s1200058 5:ba883a4bddc3 222 } else if(robot_x>=0&&robot_y<0) { //南東
s1200058 5:ba883a4bddc3 223 if(robot_x>abs(robot_y)){
s1200058 5:ba883a4bddc3 224 angle = (M_PI/2) - atan2(abs(robot_y), robot_x);
s1200058 5:ba883a4bddc3 225 }
s1200058 5:ba883a4bddc3 226 else{
s1200058 5:ba883a4bddc3 227 angle = atan2(abs(robot_y), robot_x);
s1200058 5:ba883a4bddc3 228 }
s1200058 5:ba883a4bddc3 229 return angle * 180.0 / M_PI + 90;
s1200058 5:ba883a4bddc3 230 } else if(robot_x<0&&robot_y<0) { //南西
s1200058 5:ba883a4bddc3 231 if(abs(robot_x)<abs(robot_y)){
s1200058 5:ba883a4bddc3 232 angle = atan2(abs(robot_x), abs(robot_y));
s1200058 5:ba883a4bddc3 233 }
s1200058 5:ba883a4bddc3 234 else{
s1200058 5:ba883a4bddc3 235 angle = (M_PI/2) - atan2(abs(robot_y), abs(robot_x));
s1200058 5:ba883a4bddc3 236 }
s1200058 5:ba883a4bddc3 237 return angle * 180.0 / M_PI + 180;
s1200058 5:ba883a4bddc3 238 } else if(robot_x<0&&robot_y>=0) { //北西
s1200058 5:ba883a4bddc3 239 if(abs(robot_x)>robot_y){
s1200058 5:ba883a4bddc3 240 angle = (M_PI/2) - atan2(robot_y, abs(robot_x));
s1200058 5:ba883a4bddc3 241 }
s1200058 5:ba883a4bddc3 242 else{
s1200058 5:ba883a4bddc3 243 angle = atan2(abs(robot_x), robot_y);
s1200058 5:ba883a4bddc3 244 }
s1200058 5:ba883a4bddc3 245 return angle * 180.0 / M_PI + 270;
s1200058 5:ba883a4bddc3 246 }
s1200058 5:ba883a4bddc3 247
s1200058 5:ba883a4bddc3 248 return -1;
s1200058 5:ba883a4bddc3 249 }
s1200058 5:ba883a4bddc3 250
s1200058 5:ba883a4bddc3 251
kityann 0:1fcc61be1dcf 252 /******************************
kityann 0:1fcc61be1dcf 253 スタンバイモード
kityann 0:1fcc61be1dcf 254 ******************************/
kityann 0:1fcc61be1dcf 255 void standby(){
s1200058 5:ba883a4bddc3 256
s1200058 6:23bb3b018f44 257 cansat.control_Motor(1, cansat.get_speed());
s1200058 5:ba883a4bddc3 258 if(short_in == 0){
s1200058 5:ba883a4bddc3 259 mode = 1;
s1200058 5:ba883a4bddc3 260 }
s1200058 5:ba883a4bddc3 261
kityann 0:1fcc61be1dcf 262 }
kityann 0:1fcc61be1dcf 263
kityann 0:1fcc61be1dcf 264 /******************************
kityann 0:1fcc61be1dcf 265 落下モード
kityann 0:1fcc61be1dcf 266 ******************************/
kityann 0:1fcc61be1dcf 267 void falling(){
s1200058 7:db6b436c0baa 268
s1200058 7:db6b436c0baa 269 cansat.set_temperature(sensor.getTemperature());
s1200058 7:db6b436c0baa 270 cansat.set_pressure(sensor.getPressure());
s1200058 7:db6b436c0baa 271 cansat.set_humidity(sensor.getHumidity());
s1200058 7:db6b436c0baa 272
s1200058 5:ba883a4bddc3 273 if(cansat.get_pressure() >= goal_Pressure){
s1200058 7:db6b436c0baa 274 if(fall_flag == 0){
s1200058 7:db6b436c0baa 275 nic = 1;
s1200058 7:db6b436c0baa 276 fall_flag = 1;
s1200058 7:db6b436c0baa 277 sep_Timer.reset();
s1200058 7:db6b436c0baa 278 }
s1200058 7:db6b436c0baa 279 if(sep_Timer.read_ms() >= sep_Time){
s1200058 7:db6b436c0baa 280 mode = 2;
s1200058 7:db6b436c0baa 281 nic = 0;
s1200058 7:db6b436c0baa 282 }
s1200058 7:db6b436c0baa 283
s1200058 5:ba883a4bddc3 284 }
kityann 0:1fcc61be1dcf 285 }
kityann 0:1fcc61be1dcf 286
kityann 0:1fcc61be1dcf 287 /******************************
kityann 0:1fcc61be1dcf 288 走行モード
kityann 0:1fcc61be1dcf 289 ******************************/
kityann 0:1fcc61be1dcf 290 void running(){
aoki0731 4:0fc7221e2f79 291 double r = 6378.137;
s1200058 5:ba883a4bddc3 292 double y1 = cansat.get_target_y();
s1200058 5:ba883a4bddc3 293 double y2 = cansat.get_robot_y();
s1200058 5:ba883a4bddc3 294 double x1 = cansat.get_target_x();
s1200058 5:ba883a4bddc3 295 double x2 = cansat.get_robot_x();
s1200058 5:ba883a4bddc3 296 double dx = x2 - x1;
s1200058 5:ba883a4bddc3 297
s1200058 7:db6b436c0baa 298 cansat.set_target_distance(r*acos(sin(y1)*sin(y2)+cos(y1)*cos(y2)*cos(dx)));
s1200058 5:ba883a4bddc3 299
s1200058 5:ba883a4bddc3 300 if(cansat.get_target_distance() < 10) cansat.set_speed(32);
s1200058 5:ba883a4bddc3 301 else if(cansat.get_target_distance() < 20 && cansat.get_target_distance() > 10) cansat.set_speed(64);
s1200058 5:ba883a4bddc3 302 else cansat.set_speed(128);
s1200058 5:ba883a4bddc3 303
s1200058 5:ba883a4bddc3 304 if(cansat.get_compass_z() < 0) {
aoki0731 4:0fc7221e2f79 305 //ひっくり返っている
s1200058 5:ba883a4bddc3 306 cansat.control_Motor(0, cansat.get_speed());
s1200058 5:ba883a4bddc3 307 } else {
s1200058 5:ba883a4bddc3 308 switch(robot_Action(cansat.get_robot_angle(), cansat.get_target_angle())) {
s1200058 5:ba883a4bddc3 309 case 'f': //前進
s1200058 5:ba883a4bddc3 310 cansat.control_Motor(0, cansat.get_speed());
s1200058 5:ba883a4bddc3 311 break;
s1200058 5:ba883a4bddc3 312 case 'l':
s1200058 5:ba883a4bddc3 313 cansat.control_Motor(2, cansat.get_speed());
s1200058 5:ba883a4bddc3 314 break;
s1200058 5:ba883a4bddc3 315 case 'r':
s1200058 5:ba883a4bddc3 316 cansat.control_Motor(3, cansat.get_speed());
s1200058 5:ba883a4bddc3 317 break;
s1200058 5:ba883a4bddc3 318 }
s1200058 5:ba883a4bddc3 319 }
s1200058 5:ba883a4bddc3 320
s1200058 7:db6b436c0baa 321 if(cansat.get_target_distance() <= 1){
s1200058 6:23bb3b018f44 322 mode = 100;
s1200058 6:23bb3b018f44 323 }
kityann 0:1fcc61be1dcf 324 }
kityann 0:1fcc61be1dcf 325
kityann 0:1fcc61be1dcf 326 /******************************
kityann 0:1fcc61be1dcf 327 停止モード
kityann 0:1fcc61be1dcf 328 ******************************/
kityann 0:1fcc61be1dcf 329 void stopping(){
kityann 0:1fcc61be1dcf 330
s1200058 6:23bb3b018f44 331 cansat.control_Motor(0, cansat.get_speed());
kityann 0:1fcc61be1dcf 332
kityann 0:1fcc61be1dcf 333 }
kityann 0:1fcc61be1dcf 334
kityann 0:1fcc61be1dcf 335 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 336 //
kityann 0:1fcc61be1dcf 337 //Main Processing
kityann 0:1fcc61be1dcf 338 //
kityann 0:1fcc61be1dcf 339 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 340 int main() {
kityann 0:1fcc61be1dcf 341 //start up time
kityann 0:1fcc61be1dcf 342 wait(3);
kityann 0:1fcc61be1dcf 343 //set pc frequency to 57600bps
kityann 0:1fcc61be1dcf 344 pc.baud(PC_BAUD_RATE);
kityann 0:1fcc61be1dcf 345 //set xbee frequency to 57600bps
kityann 0:1fcc61be1dcf 346 xbee.begin(XBEE_BAUD_RATE);
kityann 0:1fcc61be1dcf 347
kityann 0:1fcc61be1dcf 348
kityann 0:1fcc61be1dcf 349 //GPS setting
kityann 0:1fcc61be1dcf 350 gps_Serial = new Serial(p28,p27);
kityann 0:1fcc61be1dcf 351 Adafruit_GPS myGPS(gps_Serial);
kityann 0:1fcc61be1dcf 352 Timer refresh_Timer;
kityann 0:1fcc61be1dcf 353 const int refresh_Time = 1000; //refresh time in ms
kityann 0:1fcc61be1dcf 354 int count = 0;
kityann 0:1fcc61be1dcf 355
kityann 0:1fcc61be1dcf 356 myGPS.begin(GPS_BAUD_RATE);
kityann 0:1fcc61be1dcf 357
kityann 0:1fcc61be1dcf 358 //GPS Send Command
kityann 0:1fcc61be1dcf 359 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:1fcc61be1dcf 360 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:1fcc61be1dcf 361 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:1fcc61be1dcf 362
kityann 0:1fcc61be1dcf 363 wait_ms(2000);
kityann 0:1fcc61be1dcf 364
kityann 0:1fcc61be1dcf 365 //interrupt start
kityann 0:1fcc61be1dcf 366 refresh_Timer.start();
kityann 0:1fcc61be1dcf 367
kityann 0:1fcc61be1dcf 368 printf("start\n");
kityann 0:1fcc61be1dcf 369
s1200058 5:ba883a4bddc3 370 // int mode = -1;
s1200058 5:ba883a4bddc3 371 short_out = 1; //ショートピンの出力:high
s1200058 5:ba883a4bddc3 372 nic.output();
s1200058 5:ba883a4bddc3 373 nic = 0;
s1200058 7:db6b436c0baa 374 int log = 0;
kityann 0:1fcc61be1dcf 375
kityann 0:1fcc61be1dcf 376 while (true) {
kityann 0:1fcc61be1dcf 377
kityann 0:1fcc61be1dcf 378 switch(mode){
kityann 0:1fcc61be1dcf 379 //スタートモード：パラシュートが開くまではこのモードを実行
kityann 0:1fcc61be1dcf 380 case -1:
kityann 0:1fcc61be1dcf 381 standby();
kityann 0:1fcc61be1dcf 382 break;
kityann 0:1fcc61be1dcf 383 //落下モード：落下時はこのモード。気圧計または、時間でロボットとパラシュートを分離する
kityann 0:1fcc61be1dcf 384 case 1:
kityann 0:1fcc61be1dcf 385 falling();
kityann 0:1fcc61be1dcf 386 break;
kityann 0:1fcc61be1dcf 387 //走行モード：ターゲットにむかって走行を行う
kityann 0:1fcc61be1dcf 388 case 2:
kityann 0:1fcc61be1dcf 389 running();
kityann 0:1fcc61be1dcf 390 break;
kityann 0:1fcc61be1dcf 391 //停止モード：ターゲット
kityann 0:1fcc61be1dcf 392 case 100:
kityann 0:1fcc61be1dcf 393 stopping();
kityann 0:1fcc61be1dcf 394 break;
kityann 0:1fcc61be1dcf 395 }
kityann 0:1fcc61be1dcf 396
kityann 0:1fcc61be1dcf 397 myGPS.read();
kityann 0:1fcc61be1dcf 398 //recive gps module
kityann 0:1fcc61be1dcf 399 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:1fcc61be1dcf 400 if ( myGPS.newNMEAreceived() ) {
kityann 0:1fcc61be1dcf 401 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:1fcc61be1dcf 402 continue;
kityann 0:1fcc61be1dcf 403 }
kityann 0:1fcc61be1dcf 404 else{
kityann 0:1fcc61be1dcf 405 count++;
kityann 0:1fcc61be1dcf 406 }
kityann 0:1fcc61be1dcf 407 }
kityann 0:1fcc61be1dcf 408 //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
kityann 0:1fcc61be1dcf 409 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:1fcc61be1dcf 410 refresh_Timer.reset();
kityann 0:1fcc61be1dcf 411 //print_gps(count);
kityann 0:1fcc61be1dcf 412 Get_GPS(&myGPS);
s1200058 7:db6b436c0baa 413 log++;
s1200058 7:db6b436c0baa 414 pc.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
s1200058 7:db6b436c0baa 415 pc.printf("robot_angle:%d, target_angle:%d, robot_compass:%f, %04.2f hPa\n",cansat.get_robot_angle(), cansat.get_target_angle(), cansat.get_compass_z(), cansat.get_pressure());
kityann 0:1fcc61be1dcf 416 }
kityann 0:1fcc61be1dcf 417 }
kityann 0:1fcc61be1dcf 418
kityann 0:1fcc61be1dcf 419 }