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@8:ca21b4e8a350, 2015-08-08 (annotated)

Committer:
kityann
Date:
Sat Aug 08 07:44:32 2015 +0000
Revision:
8:ca21b4e8a350
Parent:
5:ba883a4bddc3 
2015/08/08

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 "MBed_Adafruit_GPS.h"
kityann 0:1fcc61be1dcf 8 #include "aigamozuSetting.h"
kityann 1:f1f7413ae6bd 9 #include "HMC5883L.h"
kityann 1:f1f7413ae6bd 10 #include "VNH5019.h"
kityann 3:0bd9ad37f319 11 #include "cansat.h"
kityann 0:1fcc61be1dcf 12 #include "math.h"
s1200058 5:ba883a4bddc3 13 #include "BME280.h"
kityann 0:1fcc61be1dcf 14
kityann 0:1fcc61be1dcf 15 #define SIGMA_MIN 0.0001
kityann 0:1fcc61be1dcf 16
kityann 0:1fcc61be1dcf 17 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 18 //
kityann 0:1fcc61be1dcf 19 //Pin Setting
kityann 0:1fcc61be1dcf 20 //
kityann 0:1fcc61be1dcf 21 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 22 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:1fcc61be1dcf 23
kityann 0:1fcc61be1dcf 24 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 25 //
kityann 0:1fcc61be1dcf 26 //Connection Setting
kityann 0:1fcc61be1dcf 27 //
kityann 0:1fcc61be1dcf 28 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 29
kityann 0:1fcc61be1dcf 30 //Serial Connect Setting: PC <--> mbed
kityann 0:1fcc61be1dcf 31 Serial pc(USBTX, USBRX);
kityann 0:1fcc61be1dcf 32
kityann 0:1fcc61be1dcf 33 //Serial Connect Setting: GPS <--> mbed
kityann 0:1fcc61be1dcf 34 Serial * gps_Serial;
kityann 0:1fcc61be1dcf 35
kityann 0:1fcc61be1dcf 36 //Serial Connect Setting: XBEE <--> mbed
kityann 8:ca21b4e8a350 37 Serial xbee(p13,p14);
kityann 0:1fcc61be1dcf 38
kityann 0:1fcc61be1dcf 39 //set up GPS module
kityann 0:1fcc61be1dcf 40
kityann 0:1fcc61be1dcf 41 //set up AigamozuControlPackets library
kityann 1:f1f7413ae6bd 42 //AigamozuControlPackets agz(agz_motorShield);
kityann 3:0bd9ad37f319 43 CanSat cansat(agz_motorShield);
kityann 0:1fcc61be1dcf 44
s1200058 5:ba883a4bddc3 45 //set up for tempratures...
s1200058 5:ba883a4bddc3 46 #if defined(TARGET_LPC1768)
s1200058 5:ba883a4bddc3 47 BME280 sensor(p9, p10);
s1200058 5:ba883a4bddc3 48 #else
s1200058 5:ba883a4bddc3 49 BME280 sensor(I2C_SDA, I2C_SCL);
s1200058 5:ba883a4bddc3 50 #endif
s1200058 5:ba883a4bddc3 51
s1200058 5:ba883a4bddc3 52 DigitalIn short_in(p29);
s1200058 5:ba883a4bddc3 53 DigitalOut short_out(p30);
s1200058 5:ba883a4bddc3 54 DigitalInOut nic(p5);
kityann 0:1fcc61be1dcf 55
kityann 8:ca21b4e8a350 56
kityann 8:ca21b4e8a350 57
kityann 8:ca21b4e8a350 58 Timer sep_Timer;
kityann 8:ca21b4e8a350 59 const int sep_Time = 1000; //seperate time in ms
kityann 8:ca21b4e8a350 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
kityann 8:ca21b4e8a350 247 void calc_target_angle(double angle){
kityann 8:ca21b4e8a350 248 if(0 <= angle && angle < 22.5){
kityann 8:ca21b4e8a350 249 cansat.set_target_angle(0);
kityann 8:ca21b4e8a350 250 }
kityann 8:ca21b4e8a350 251 if(22.5 <= angle && angle < 67.5){
kityann 8:ca21b4e8a350 252 cansat.set_target_angle(1);
kityann 8:ca21b4e8a350 253 }
kityann 8:ca21b4e8a350 254 if(67.5 <= angle && angle < 112.5){
kityann 8:ca21b4e8a350 255 cansat.set_target_angle(2);
kityann 8:ca21b4e8a350 256 }
kityann 8:ca21b4e8a350 257 if(112.5 <= angle && angle < 157.5){
kityann 8:ca21b4e8a350 258 cansat.set_target_angle(3);
kityann 8:ca21b4e8a350 259 }
kityann 8:ca21b4e8a350 260 if(157.5 <= angle && angle < 202.5){
kityann 8:ca21b4e8a350 261 cansat.set_target_angle(4);
kityann 8:ca21b4e8a350 262 }
kityann 8:ca21b4e8a350 263 if(202.5 <= angle && angle < 247.5){
kityann 8:ca21b4e8a350 264 cansat.set_target_angle(5);
kityann 8:ca21b4e8a350 265 }
kityann 8:ca21b4e8a350 266 if(247.5 <= angle && angle < 292.5){
kityann 8:ca21b4e8a350 267 cansat.set_target_angle(6);
kityann 8:ca21b4e8a350 268 }
kityann 8:ca21b4e8a350 269 if(292.5 <= angle && angle < 337.5){
kityann 8:ca21b4e8a350 270 cansat.set_target_angle(7);
kityann 8:ca21b4e8a350 271 }
kityann 8:ca21b4e8a350 272 if(337.5 <= angle && angle < 360){
kityann 8:ca21b4e8a350 273 cansat.set_target_angle(7);
kityann 8:ca21b4e8a350 274 }
kityann 8:ca21b4e8a350 275 }
kityann 0:1fcc61be1dcf 276 /******************************
kityann 0:1fcc61be1dcf 277 スタンバイモード
kityann 0:1fcc61be1dcf 278 ******************************/
kityann 0:1fcc61be1dcf 279 void standby(){
s1200058 5:ba883a4bddc3 280
s1200058 5:ba883a4bddc3 281 if(short_in == 0){
s1200058 5:ba883a4bddc3 282 mode = 1;
s1200058 5:ba883a4bddc3 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 falling(){
s1200058 5:ba883a4bddc3 291 if(cansat.get_pressure() >= goal_Pressure){
s1200058 5:ba883a4bddc3 292 nic = 1;
s1200058 5:ba883a4bddc3 293 mode = 2;
kityann 8:ca21b4e8a350 294 sep_Timer.reset();
s1200058 5:ba883a4bddc3 295 }
kityann 0:1fcc61be1dcf 296 }
kityann 0:1fcc61be1dcf 297
kityann 8:ca21b4e8a350 298
kityann 0:1fcc61be1dcf 299 /******************************
kityann 0:1fcc61be1dcf 300 走行モード
kityann 0:1fcc61be1dcf 301 ******************************/
kityann 0:1fcc61be1dcf 302 void running(){
kityann 8:ca21b4e8a350 303 if(sep_Timer.read_ms() >= sep_Time){
kityann 8:ca21b4e8a350 304 nic=0;
kityann 8:ca21b4e8a350 305 }
kityann 8:ca21b4e8a350 306
aoki0731 4:0fc7221e2f79 307 double r = 6378.137;
s1200058 5:ba883a4bddc3 308 double y1 = cansat.get_target_y();
s1200058 5:ba883a4bddc3 309 double y2 = cansat.get_robot_y();
s1200058 5:ba883a4bddc3 310 double x1 = cansat.get_target_x();
s1200058 5:ba883a4bddc3 311 double x2 = cansat.get_robot_x();
s1200058 5:ba883a4bddc3 312 double dx = x2 - x1;
s1200058 5:ba883a4bddc3 313
kityann 8:ca21b4e8a350 314 //kyorinoset
kityann 8:ca21b4e8a350 315 cansat.set_target_distance(r*acos(sin(y1)*sin(y2)+cos(y1)*cos(y2)*cos(dx)));
kityann 8:ca21b4e8a350 316 double angle = robot_compass(x2-x1,y2-y1);
kityann 8:ca21b4e8a350 317
kityann 8:ca21b4e8a350 318
kityann 8:ca21b4e8a350 319
s1200058 5:ba883a4bddc3 320
s1200058 5:ba883a4bddc3 321 if(cansat.get_target_distance() < 10) cansat.set_speed(32);
s1200058 5:ba883a4bddc3 322 else if(cansat.get_target_distance() < 20 && cansat.get_target_distance() > 10) cansat.set_speed(64);
s1200058 5:ba883a4bddc3 323 else cansat.set_speed(128);
s1200058 5:ba883a4bddc3 324
kityann 8:ca21b4e8a350 325
s1200058 5:ba883a4bddc3 326 if(cansat.get_compass_z() < 0) {
aoki0731 4:0fc7221e2f79 327 //ひっくり返っている
s1200058 5:ba883a4bddc3 328 cansat.control_Motor(0, cansat.get_speed());
s1200058 5:ba883a4bddc3 329 } else {
s1200058 5:ba883a4bddc3 330 switch(robot_Action(cansat.get_robot_angle(), cansat.get_target_angle())) {
s1200058 5:ba883a4bddc3 331 case 'f': //前進
s1200058 5:ba883a4bddc3 332 cansat.control_Motor(0, cansat.get_speed());
s1200058 5:ba883a4bddc3 333 break;
s1200058 5:ba883a4bddc3 334 case 'l':
s1200058 5:ba883a4bddc3 335 cansat.control_Motor(2, cansat.get_speed());
s1200058 5:ba883a4bddc3 336 break;
s1200058 5:ba883a4bddc3 337 case 'r':
s1200058 5:ba883a4bddc3 338 cansat.control_Motor(3, cansat.get_speed());
s1200058 5:ba883a4bddc3 339 break;
s1200058 5:ba883a4bddc3 340 }
s1200058 5:ba883a4bddc3 341 }
s1200058 5:ba883a4bddc3 342
kityann 0:1fcc61be1dcf 343 }
kityann 0:1fcc61be1dcf 344
kityann 0:1fcc61be1dcf 345 /******************************
kityann 0:1fcc61be1dcf 346 停止モード
kityann 0:1fcc61be1dcf 347 ******************************/
kityann 0:1fcc61be1dcf 348 void stopping(){
kityann 8:ca21b4e8a350 349 cansat.control_Motor(1,cansat.get_speed());
kityann 8:ca21b4e8a350 350 xbee.printf("stop\n");
kityann 0:1fcc61be1dcf 351
kityann 0:1fcc61be1dcf 352 }
kityann 0:1fcc61be1dcf 353
kityann 0:1fcc61be1dcf 354 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 355 //
kityann 0:1fcc61be1dcf 356 //Main Processing
kityann 0:1fcc61be1dcf 357 //
kityann 0:1fcc61be1dcf 358 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 359 int main() {
kityann 0:1fcc61be1dcf 360 //start up time
kityann 0:1fcc61be1dcf 361 wait(3);
kityann 0:1fcc61be1dcf 362 //set pc frequency to 57600bps
kityann 0:1fcc61be1dcf 363 pc.baud(PC_BAUD_RATE);
kityann 8:ca21b4e8a350 364 //set xbee frequency to 9600bps
kityann 8:ca21b4e8a350 365 xbee.baud(XBEE_BAUD_RATE);
kityann 0:1fcc61be1dcf 366
kityann 0:1fcc61be1dcf 367
kityann 0:1fcc61be1dcf 368 //GPS setting
kityann 0:1fcc61be1dcf 369 gps_Serial = new Serial(p28,p27);
kityann 0:1fcc61be1dcf 370 Adafruit_GPS myGPS(gps_Serial);
kityann 0:1fcc61be1dcf 371 Timer refresh_Timer;
kityann 0:1fcc61be1dcf 372 const int refresh_Time = 1000; //refresh time in ms
kityann 0:1fcc61be1dcf 373 int count = 0;
kityann 0:1fcc61be1dcf 374
kityann 0:1fcc61be1dcf 375 myGPS.begin(GPS_BAUD_RATE);
kityann 0:1fcc61be1dcf 376
kityann 0:1fcc61be1dcf 377 //GPS Send Command
kityann 0:1fcc61be1dcf 378 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:1fcc61be1dcf 379 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:1fcc61be1dcf 380 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:1fcc61be1dcf 381
kityann 0:1fcc61be1dcf 382 wait_ms(2000);
kityann 0:1fcc61be1dcf 383
kityann 0:1fcc61be1dcf 384 //interrupt start
kityann 0:1fcc61be1dcf 385 refresh_Timer.start();
kityann 0:1fcc61be1dcf 386
kityann 0:1fcc61be1dcf 387 printf("start\n");
kityann 0:1fcc61be1dcf 388
kityann 8:ca21b4e8a350 389 //xbee.printf("start\n");
kityann 8:ca21b4e8a350 390
s1200058 5:ba883a4bddc3 391 // int mode = -1;
s1200058 5:ba883a4bddc3 392 short_out = 1; //ショートピンの出力:high
s1200058 5:ba883a4bddc3 393 nic.output();
s1200058 5:ba883a4bddc3 394 nic = 0;
kityann 0:1fcc61be1dcf 395
kityann 0:1fcc61be1dcf 396 while (true) {
kityann 8:ca21b4e8a350 397 xbee.printf("test\n");
kityann 8:ca21b4e8a350 398 xbee.printf("mode = %d\n",mode);
kityann 0:1fcc61be1dcf 399
kityann 0:1fcc61be1dcf 400 switch(mode){
kityann 0:1fcc61be1dcf 401 //スタートモード：パラシュートが開くまではこのモードを実行
kityann 0:1fcc61be1dcf 402 case -1:
kityann 0:1fcc61be1dcf 403 standby();
kityann 0:1fcc61be1dcf 404 break;
kityann 0:1fcc61be1dcf 405 //落下モード：落下時はこのモード。気圧計または、時間でロボットとパラシュートを分離する
kityann 0:1fcc61be1dcf 406 case 1:
kityann 0:1fcc61be1dcf 407 falling();
kityann 0:1fcc61be1dcf 408 break;
kityann 0:1fcc61be1dcf 409 //走行モード：ターゲットにむかって走行を行う
kityann 0:1fcc61be1dcf 410 case 2:
kityann 0:1fcc61be1dcf 411 running();
kityann 0:1fcc61be1dcf 412 break;
kityann 0:1fcc61be1dcf 413 //停止モード：ターゲット
kityann 0:1fcc61be1dcf 414 case 100:
kityann 0:1fcc61be1dcf 415 stopping();
kityann 0:1fcc61be1dcf 416 break;
kityann 0:1fcc61be1dcf 417 }
kityann 0:1fcc61be1dcf 418
kityann 8:ca21b4e8a350 419 //落下時のみ気圧センサを読む
kityann 8:ca21b4e8a350 420 if(mode == 1){
kityann 8:ca21b4e8a350 421 cansat.set_temperature(sensor.getTemperature());
kityann 8:ca21b4e8a350 422 cansat.set_pressure(sensor.getPressure());
kityann 8:ca21b4e8a350 423 cansat.set_humidity(sensor.getHumidity());
kityann 8:ca21b4e8a350 424 }
kityann 8:ca21b4e8a350 425
kityann 0:1fcc61be1dcf 426 myGPS.read();
kityann 0:1fcc61be1dcf 427 //recive gps module
kityann 0:1fcc61be1dcf 428 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:1fcc61be1dcf 429 if ( myGPS.newNMEAreceived() ) {
kityann 0:1fcc61be1dcf 430 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:1fcc61be1dcf 431 continue;
kityann 0:1fcc61be1dcf 432 }
kityann 0:1fcc61be1dcf 433 else{
kityann 0:1fcc61be1dcf 434 count++;
kityann 0:1fcc61be1dcf 435 }
kityann 0:1fcc61be1dcf 436 }
kityann 0:1fcc61be1dcf 437 //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
kityann 0:1fcc61be1dcf 438 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:1fcc61be1dcf 439 refresh_Timer.reset();
kityann 0:1fcc61be1dcf 440 //print_gps(count);
kityann 0:1fcc61be1dcf 441 Get_GPS(&myGPS);
kityann 8:ca21b4e8a350 442
kityann 8:ca21b4e8a350 443 cansat.set_temperature(sensor.getTemperature());
kityann 8:ca21b4e8a350 444 cansat.set_pressure(sensor.getPressure());
kityann 8:ca21b4e8a350 445 cansat.set_humidity(sensor.getHumidity());
kityann 8:ca21b4e8a350 446 //pc.printf("%2.2f degC, %04.2f hPa, %2.2f %%\n", sensor.getTemperature(), sensor.getPressure(), sensor.getHumidity());
kityann 0:1fcc61be1dcf 447 }
kityann 0:1fcc61be1dcf 448 }
kityann 0:1fcc61be1dcf 449
kityann 0:1fcc61be1dcf 450 }