CanSat2016aizu / Mbed 2 deprecated Cansat_program1

cansat program1

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

Fork of Cansat_program4_1 by CanSat2015aizu

main.cpp@24:e88f40aedb9c, 2016-05-07 (annotated)

Committer:
s1210160
Date:
Sat May 07 06:41:52 2016 +0000
Revision:
24:e88f40aedb9c
Parent:
23:8581b740beef 
cansat;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1210160 23:8581b740beef 1
s1210160 23:8581b740beef 2
kityann 0:1fcc61be1dcf 3 /**********************************************/
kityann 0:1fcc61be1dcf 4 //更新情報
kityann 0:1fcc61be1dcf 5
kityann 0:1fcc61be1dcf 6 /**********************************************/
kityann 0:1fcc61be1dcf 7
kityann 0:1fcc61be1dcf 8 #include "mbed.h"
kityann 0:1fcc61be1dcf 9 #include "XBee.h"
kityann 0:1fcc61be1dcf 10 #include "MBed_Adafruit_GPS.h"
kityann 1:f1f7413ae6bd 11 //#include "AigamozuControlPackets.h"
kityann 0:1fcc61be1dcf 12 #include "agzIDLIST.h"
kityann 0:1fcc61be1dcf 13 #include "aigamozuSetting.h"
kityann 1:f1f7413ae6bd 14 #include "HMC5883L.h"
kityann 1:f1f7413ae6bd 15 #include "VNH5019.h"
kityann 3:0bd9ad37f319 16 #include "cansat.h"
kityann 0:1fcc61be1dcf 17 #include "math.h"
s1200058 5:ba883a4bddc3 18 #include "BME280.h"
kityann 0:1fcc61be1dcf 19
kityann 0:1fcc61be1dcf 20 #define SIGMA_MIN 0.0001
kityann 0:1fcc61be1dcf 21
s1210160 10:ce253d8a5f2c 22 #define STOP 0 //compass initial
s1210160 10:ce253d8a5f2c 23 #define CAL 1 //compass calibration
s1210160 10:ce253d8a5f2c 24 #define RUN 2 //compass run
s1210160 10:ce253d8a5f2c 25
kityann 0:1fcc61be1dcf 26 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 27 //
kityann 0:1fcc61be1dcf 28 //Pin Setting
kityann 0:1fcc61be1dcf 29 //
kityann 0:1fcc61be1dcf 30 /////////////////////////////////////////
s1200058 12:5724d4a57a4c 31 VNH5019 agz_motorShield(p23,p22,p25,p21,p24,p26);
s1200058 12:5724d4a57a4c 32 //VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:1fcc61be1dcf 33
kityann 0:1fcc61be1dcf 34 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 35 //
kityann 0:1fcc61be1dcf 36 //Connection Setting
kityann 0:1fcc61be1dcf 37 //
kityann 0:1fcc61be1dcf 38 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 39
kityann 0:1fcc61be1dcf 40 //Serial Connect Setting: PC <--> mbed
kityann 0:1fcc61be1dcf 41 Serial pc(USBTX, USBRX);
kityann 0:1fcc61be1dcf 42
kityann 0:1fcc61be1dcf 43 //Serial Connect Setting: GPS <--> mbed
kityann 0:1fcc61be1dcf 44 Serial * gps_Serial;
kityann 0:1fcc61be1dcf 45
kityann 0:1fcc61be1dcf 46 //Serial Connect Setting: XBEE <--> mbed
s1200058 12:5724d4a57a4c 47 Serial xbee(p13,p14);
kityann 0:1fcc61be1dcf 48 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:1fcc61be1dcf 49
s1210160 10:ce253d8a5f2c 50 // compass
s1210160 10:ce253d8a5f2c 51 HMC5883L compass(p9, p10);
s1210160 10:ce253d8a5f2c 52
kityann 0:1fcc61be1dcf 53 //set up GPS module
kityann 0:1fcc61be1dcf 54
kityann 0:1fcc61be1dcf 55 //set up AigamozuControlPackets library
kityann 1:f1f7413ae6bd 56 //AigamozuControlPackets agz(agz_motorShield);
kityann 3:0bd9ad37f319 57 CanSat cansat(agz_motorShield);
kityann 0:1fcc61be1dcf 58
s1200058 5:ba883a4bddc3 59 //set up for tempratures...
s1200058 5:ba883a4bddc3 60 #if defined(TARGET_LPC1768)
s1200058 5:ba883a4bddc3 61 BME280 sensor(p9, p10);
s1200058 5:ba883a4bddc3 62 #else
s1200058 5:ba883a4bddc3 63 BME280 sensor(I2C_SDA, I2C_SCL);
s1200058 5:ba883a4bddc3 64 #endif
s1200058 5:ba883a4bddc3 65
s1200058 5:ba883a4bddc3 66 DigitalIn short_in(p29);
s1200058 5:ba883a4bddc3 67 DigitalOut short_out(p30);
s1200058 5:ba883a4bddc3 68 DigitalInOut nic(p5);
kityann 0:1fcc61be1dcf 69
s1200058 18:c86872baed44 70 int short_flag = 0;
s1200058 19:cb3a4b4c3526 71 int running_flag = 0;
s1200058 18:c86872baed44 72
s1210160 21:18a196d3021c 73 Timer pressure_Timer;
s1210160 21:18a196d3021c 74 const int pressure_Time = 10000;
s1200058 12:5724d4a57a4c 75 Timer compass_Timer;
s1200058 20:f92bdcda5a60 76 const int compass_Time = 50;
s1200058 18:c86872baed44 77 Timer running_Timer;
s1200058 20:f92bdcda5a60 78 const int running_Time = 50;
s1200058 18:c86872baed44 79 Timer parachute_Timer;
s1200058 18:c86872baed44 80 const int parachute_Time = 30000;
kityann 0:1fcc61be1dcf 81 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 82 //
s1210160 10:ce253d8a5f2c 83 //For Compass data
s1210160 10:ce253d8a5f2c 84 //
s1210160 10:ce253d8a5f2c 85 /////////////////////////////////////////
s1210160 10:ce253d8a5f2c 86 Ticker compass_interrupt;
s1210160 10:ce253d8a5f2c 87 double heading0 = 0.0;
s1210160 10:ce253d8a5f2c 88 double heading1 = 0.0;
s1210160 10:ce253d8a5f2c 89 double heading2 = 0.0;
s1210160 10:ce253d8a5f2c 90 double heading3 = 0.0;
s1210160 10:ce253d8a5f2c 91 double headingLPF = 0.0;
s1210160 10:ce253d8a5f2c 92 double initHeading;
s1210160 10:ce253d8a5f2c 93 double tgtHeading;
s1210160 10:ce253d8a5f2c 94 double preHeading = 0.0;
s1210160 10:ce253d8a5f2c 95
s1210160 10:ce253d8a5f2c 96 int maxX, minX, maxY, minY;
s1200058 20:f92bdcda5a60 97 const int ofsX = -102; //calibration x
s1200058 20:f92bdcda5a60 98 const int ofsY = -381; //calibration y
s1210160 10:ce253d8a5f2c 99
s1210160 21:18a196d3021c 100 int16_t raw[3];
s1210160 10:ce253d8a5f2c 101
s1210160 10:ce253d8a5f2c 102 /////////////////////////////////////////
s1210160 10:ce253d8a5f2c 103 //
kityann 0:1fcc61be1dcf 104 //For Kalman data
kityann 0:1fcc61be1dcf 105 //
kityann 0:1fcc61be1dcf 106 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 107 #define FIRST_S2_1 1.0e-8
kityann 0:1fcc61be1dcf 108 #define FIRST_S2_2 1.0e-6
kityann 0:1fcc61be1dcf 109 #define COUNTER_MAX 10000
kityann 0:1fcc61be1dcf 110 #define ERROR_RANGE 0.001
kityann 0:1fcc61be1dcf 111
kityann 0:1fcc61be1dcf 112 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
kityann 0:1fcc61be1dcf 113 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 114 double s2_R=FIRST_S2_2;//GPSセンサの分散
kityann 0:1fcc61be1dcf 115 double s2_Q=FIRST_S2_2;
kityann 0:1fcc61be1dcf 116 double Kx=0,Ky=0;//カルマンゲイン
kityann 0:1fcc61be1dcf 117 double zx,zy;//観測値
kityann 0:1fcc61be1dcf 118 void Kalman(double Latitude,double Longitude);
kityann 0:1fcc61be1dcf 119 int change = 0;
kityann 0:1fcc61be1dcf 120
s1200058 18:c86872baed44 121 int mode = -1; //ロボットのモード
s1200058 19:cb3a4b4c3526 122 double target_x = 139.987305,target_y = 40.142655;
s1210160 21:18a196d3021c 123 double goal_Pressure = 0.0, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度
kityann 0:1fcc61be1dcf 124
s1200058 7:db6b436c0baa 125 Timer sep_Timer;
s1200058 18:c86872baed44 126 const int sep_Time = 3000; //seperate time in ms
s1200058 7:db6b436c0baa 127 int fall_flag = 0;
s1200058 7:db6b436c0baa 128
s1200058 14:4dfeeca65308 129 int log_number = 0;
s1200058 14:4dfeeca65308 130
kityann 0:1fcc61be1dcf 131
kityann 0:1fcc61be1dcf 132 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 133 //
kityann 0:1fcc61be1dcf 134 //Kalman Processing
kityann 0:1fcc61be1dcf 135 //
kityann 0:1fcc61be1dcf 136 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 137 void calc_Kalman(){
kityann 0:1fcc61be1dcf 138 //calc Kalman gain
kityann 0:1fcc61be1dcf 139 Kx = (s2x_prev+s2_Q)/(s2x_prev+s2_R+s2_Q);
kityann 0:1fcc61be1dcf 140 Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q);
kityann 0:1fcc61be1dcf 141 //estimate
kityann 0:1fcc61be1dcf 142 x_cur = x_prev + Kx*(zx-x_prev);
kityann 0:1fcc61be1dcf 143 y_cur = y_prev + Ky*(zy-y_prev);
kityann 0:1fcc61be1dcf 144 //calc sigma
kityann 0:1fcc61be1dcf 145 s2x_cur = (1-Kx)*(s2x_prev+s2_Q);
kityann 0:1fcc61be1dcf 146 s2y_cur = (1-Ky)*(s2y_prev+s2_Q);
kityann 0:1fcc61be1dcf 147
kityann 0:1fcc61be1dcf 148 }
kityann 0:1fcc61be1dcf 149
kityann 0:1fcc61be1dcf 150 void Kalman(double Latitude,double Longitude){
kityann 0:1fcc61be1dcf 151
kityann 0:1fcc61be1dcf 152 zx = Latitude;
kityann 0:1fcc61be1dcf 153 zy = Longitude;
kityann 0:1fcc61be1dcf 154
kityann 0:1fcc61be1dcf 155 calc_Kalman();
kityann 0:1fcc61be1dcf 156
kityann 0:1fcc61be1dcf 157 //更新
kityann 0:1fcc61be1dcf 158 x_prev = x_cur;
kityann 0:1fcc61be1dcf 159 y_prev = y_cur;
kityann 0:1fcc61be1dcf 160 s2x_prev = s2x_cur;
kityann 0:1fcc61be1dcf 161 s2y_prev = s2y_cur;
kityann 0:1fcc61be1dcf 162
kityann 3:0bd9ad37f319 163 //robotK\x,robotK_yに格納する
kityann 3:0bd9ad37f319 164 cansat.set_robotKalman_x(x_cur);
kityann 3:0bd9ad37f319 165 cansat.set_robotKalman_y(y_cur);
kityann 0:1fcc61be1dcf 166 }
kityann 0:1fcc61be1dcf 167
kityann 0:1fcc61be1dcf 168 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 169 //
kityann 0:1fcc61be1dcf 170 //Get GPS function
kityann 0:1fcc61be1dcf 171 //
kityann 0:1fcc61be1dcf 172 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 173
kityann 0:1fcc61be1dcf 174 void Get_GPS(Adafruit_GPS *myGPS){
kityann 0:1fcc61be1dcf 175 static int flag = 0;
kityann 0:1fcc61be1dcf 176
kityann 0:1fcc61be1dcf 177 if (myGPS->fix) {
kityann 0:1fcc61be1dcf 178
kityann 3:0bd9ad37f319 179 cansat.nowStatus = GPS_AVAIL;
s1200058 19:cb3a4b4c3526 180 cansat.set_robot_y((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 10000.0 / 60.0));
s1200058 19:cb3a4b4c3526 181 cansat.set_robot_x((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 10000.0 / 60.0));
kityann 3:0bd9ad37f319 182
kityann 0:1fcc61be1dcf 183
kityann 0:1fcc61be1dcf 184 if(flag < COUNTER_MAX){
kityann 0:1fcc61be1dcf 185 flag++;
kityann 0:1fcc61be1dcf 186 }
kityann 0:1fcc61be1dcf 187 if(flag == 5){
kityann 3:0bd9ad37f319 188 x_prev = cansat.get_robot_x();
kityann 3:0bd9ad37f319 189 y_prev = cansat.get_robot_y();
kityann 0:1fcc61be1dcf 190 }
kityann 0:1fcc61be1dcf 191
kityann 0:1fcc61be1dcf 192 if(flag >= 6){
kityann 3:0bd9ad37f319 193 if(abs(x_prev - cansat.get_robot_x()) < ERROR_RANGE && abs(y_prev - cansat.get_robot_y()) < ERROR_RANGE){
kityann 3:0bd9ad37f319 194 Kalman(cansat.get_robot_x(), cansat.get_robot_y());
kityann 0:1fcc61be1dcf 195 change = 1;
kityann 0:1fcc61be1dcf 196 }
kityann 0:1fcc61be1dcf 197 else{
kityann 0:1fcc61be1dcf 198 change = 0;
kityann 0:1fcc61be1dcf 199 }
kityann 1:f1f7413ae6bd 200 //printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
kityann 1:f1f7413ae6bd 201 //agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 1:f1f7413ae6bd 202 //agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 1:f1f7413ae6bd 203 //agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 1:f1f7413ae6bd 204 }
kityann 0:1fcc61be1dcf 205 }
kityann 3:0bd9ad37f319 206 else cansat.nowStatus = GPS_UNAVAIL;
kityann 0:1fcc61be1dcf 207
kityann 0:1fcc61be1dcf 208 }
kityann 0:1fcc61be1dcf 209
aoki0731 4:0fc7221e2f79 210 //ロボットの動き方
s1200058 11:19091694455e 211 char robot_Action(int robot_angle, int target_angle) {
aoki0731 4:0fc7221e2f79 212
s1200058 11:19091694455e 213 int n, t, r;
aoki0731 4:0fc7221e2f79 214 t = target_angle;
aoki0731 4:0fc7221e2f79 215 r = robot_angle;
aoki0731 4:0fc7221e2f79 216 n = r - t;
aoki0731 4:0fc7221e2f79 217 if(n<0) n *= -1;
aoki0731 4:0fc7221e2f79 218
aoki0731 4:0fc7221e2f79 219
aoki0731 4:0fc7221e2f79 220 if(t==r) {
s1210160 23:8581b740beef 221 return 'f'; // target_angleとrobot_angleが等しいときは前進
s1200058 20:f92bdcda5a60 222 } else if(n == 4) {
s1210160 23:8581b740beef 223 return 'r'; // target_angleとrobot_angleが正反対にあるときは右回転
s1200058 20:f92bdcda5a60 224 } else if(n > 0 && n < 4) {
aoki0731 4:0fc7221e2f79 225 if(t > r)
s1210160 23:8581b740beef 226 return 'r'; // robot_angleから見て右方向にtarget_angleがあるときに右回転
aoki0731 4:0fc7221e2f79 227 else
s1210160 23:8581b740beef 228 return 'l'; // robot_angleから見て左方向にtarget_angleがあるときに左回転
s1200058 20:f92bdcda5a60 229 } else if(n > 4 && n < 8){
s1200058 20:f92bdcda5a60 230 if(t > r)
s1210160 23:8581b740beef 231 return 'l'; // robot_angleから見て左方向にtarget_angleがあるときに左回
aoki0731 4:0fc7221e2f79 232 else
s1210160 23:8581b740beef 233 return 'r'; // robot_angleから見て右方向にtarget_angleがあるときに右回転
s1200058 20:f92bdcda5a60 234 } else
s1200058 20:f92bdcda5a60 235 return 'b';
aoki0731 4:0fc7221e2f79 236
aoki0731 4:0fc7221e2f79 237 }
aoki0731 4:0fc7221e2f79 238
s1200058 5:ba883a4bddc3 239 //対象とロボットの角度
s1200058 5:ba883a4bddc3 240 double robot_compass(double robot_x, double robot_y) {
s1200058 5:ba883a4bddc3 241 double angle = 0;
s1200058 5:ba883a4bddc3 242
s1200058 5:ba883a4bddc3 243 if(robot_x==0&&robot_y>0)
s1200058 5:ba883a4bddc3 244 return 0;
s1200058 5:ba883a4bddc3 245 else if(robot_x>0&&robot_y==0) //東
s1200058 5:ba883a4bddc3 246 return 90;
s1200058 5:ba883a4bddc3 247 else if(robot_x==0&&robot_y<0) //南
s1200058 5:ba883a4bddc3 248 return 180;
s1200058 5:ba883a4bddc3 249 else if(robot_x<0&&robot_y==0) //西
s1200058 5:ba883a4bddc3 250 return 270;
s1200058 5:ba883a4bddc3 251 else if(robot_x>=0&&robot_y>=0) { //北東
s1200058 5:ba883a4bddc3 252 if(robot_x<=robot_y)
s1200058 5:ba883a4bddc3 253 angle = atan2(robot_x, robot_y);
s1200058 5:ba883a4bddc3 254 else
s1200058 5:ba883a4bddc3 255 angle = (M_PI/2) - atan2(robot_y, robot_x);
s1200058 5:ba883a4bddc3 256 return angle * 180.0 / M_PI;
s1200058 5:ba883a4bddc3 257 } else if(robot_x>=0&&robot_y<0) { //南東
s1200058 5:ba883a4bddc3 258 if(robot_x>abs(robot_y)){
s1200058 5:ba883a4bddc3 259 angle = (M_PI/2) - atan2(abs(robot_y), robot_x);
s1200058 5:ba883a4bddc3 260 }
s1200058 5:ba883a4bddc3 261 else{
s1200058 5:ba883a4bddc3 262 angle = atan2(abs(robot_y), robot_x);
s1200058 5:ba883a4bddc3 263 }
s1200058 5:ba883a4bddc3 264 return angle * 180.0 / M_PI + 90;
s1200058 5:ba883a4bddc3 265 } else if(robot_x<0&&robot_y<0) { //南西
s1200058 5:ba883a4bddc3 266 if(abs(robot_x)<abs(robot_y)){
s1200058 5:ba883a4bddc3 267 angle = atan2(abs(robot_x), abs(robot_y));
s1200058 5:ba883a4bddc3 268 }
s1200058 5:ba883a4bddc3 269 else{
s1200058 5:ba883a4bddc3 270 angle = (M_PI/2) - atan2(abs(robot_y), abs(robot_x));
s1200058 5:ba883a4bddc3 271 }
s1200058 5:ba883a4bddc3 272 return angle * 180.0 / M_PI + 180;
s1200058 5:ba883a4bddc3 273 } else if(robot_x<0&&robot_y>=0) { //北西
s1200058 5:ba883a4bddc3 274 if(abs(robot_x)>robot_y){
s1200058 5:ba883a4bddc3 275 angle = (M_PI/2) - atan2(robot_y, abs(robot_x));
s1200058 5:ba883a4bddc3 276 }
s1200058 5:ba883a4bddc3 277 else{
s1200058 5:ba883a4bddc3 278 angle = atan2(abs(robot_x), robot_y);
s1200058 5:ba883a4bddc3 279 }
kityann 9:2741e17438d6 280 return 360 - angle * 180.0 / M_PI;
s1200058 5:ba883a4bddc3 281 }
s1200058 5:ba883a4bddc3 282
s1200058 5:ba883a4bddc3 283 return -1;
s1200058 5:ba883a4bddc3 284 }
s1200058 5:ba883a4bddc3 285
s1210160 10:ce253d8a5f2c 286 int calc_angle(double c){
s1210160 23:8581b740beef 287 if(c > 337.5 ||(c >=0 && c < 22.5)) return 0; // 北
s1210160 23:8581b740beef 288 else if(c >= 22.5 && c < 67.5) return 1; // 北東
s1210160 23:8581b740beef 289 else if(c >= 67.5 && c < 112.5) return 2; // 東
s1210160 23:8581b740beef 290 else if(c >= 112.5 && c < 157.5) return 3; // 南東
s1210160 23:8581b740beef 291 else if(c >= 157.5 && c < 202.5) return 4; // 南
s1210160 23:8581b740beef 292 else if(c >= 202.5 && c < 247.5) return 5; // 南西
s1210160 23:8581b740beef 293 else if(c >= 247.5 && c < 292.5) return 6; // 西
s1210160 23:8581b740beef 294 else if(c >= 292.5 && c < 337.5) return 7; // 北西
s1210160 10:ce253d8a5f2c 295 else return 8;
s1210160 10:ce253d8a5f2c 296 }
s1200058 5:ba883a4bddc3 297
s1210160 10:ce253d8a5f2c 298 void Compass_intrpt(){
s1210160 10:ce253d8a5f2c 299
s1210160 10:ce253d8a5f2c 300 compass.getXYZ(raw);
s1210160 10:ce253d8a5f2c 301 double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2]
s1210160 10:ce253d8a5f2c 302 if(heading < 0)heading += 2*M_PI;
s1210160 10:ce253d8a5f2c 303 if(heading > 2*M_PI)heading -= 2*M_PI;
s1210160 10:ce253d8a5f2c 304 heading3 = heading2;
s1210160 10:ce253d8a5f2c 305 heading2 = heading1;
s1210160 10:ce253d8a5f2c 306 heading1 = heading0;
s1210160 10:ce253d8a5f2c 307 heading0 = heading;
s1200058 19:cb3a4b4c3526 308 headingLPF = (heading0 + heading1 + heading2 + heading3)/4.0; //low pass filter
s1210160 10:ce253d8a5f2c 309
s1210160 10:ce253d8a5f2c 310 headingLPF = headingLPF * 180.0 / M_PI;
s1210160 10:ce253d8a5f2c 311 // pc.printf("heading=%f\r\n",headingLPF);
s1210160 10:ce253d8a5f2c 312
s1210160 10:ce253d8a5f2c 313 cansat.set_compass(raw[0], raw[2], raw[1], headingLPF);
s1200058 18:c86872baed44 314 cansat.set_robot_angle(calc_angle(cansat.get_compass_angle()));
s1210160 10:ce253d8a5f2c 315 }
s1210160 10:ce253d8a5f2c 316
s1210160 10:ce253d8a5f2c 317
kityann 0:1fcc61be1dcf 318 /******************************
kityann 0:1fcc61be1dcf 319 スタンバイモード
kityann 0:1fcc61be1dcf 320 ******************************/
kityann 0:1fcc61be1dcf 321 void standby(){
s1200058 5:ba883a4bddc3 322
s1200058 6:23bb3b018f44 323 cansat.control_Motor(1, cansat.get_speed());
s1210160 21:18a196d3021c 324 cansat.set_temperature(sensor.getTemperature());
s1210160 21:18a196d3021c 325 cansat.set_pressure(sensor.getPressure());
s1210160 21:18a196d3021c 326 cansat.set_humidity(sensor.getHumidity());
s1210160 21:18a196d3021c 327
s1210160 21:18a196d3021c 328 if(cansat.get_pressure() > goal_Pressure){
s1210160 21:18a196d3021c 329 goal_Pressure = cansat.get_pressure();
s1210160 21:18a196d3021c 330 }
s1210160 21:18a196d3021c 331
s1200058 18:c86872baed44 332 if(!short_in){
s1200058 18:c86872baed44 333 xbee.printf("change mode: falling\n");
s1200058 18:c86872baed44 334 short_flag++;
s1200058 18:c86872baed44 335 }
s1200058 18:c86872baed44 336 else{
s1200058 18:c86872baed44 337 if(running_Timer.read_ms() >= running_Time){
s1200058 18:c86872baed44 338 running_Timer.reset();
s1210160 21:18a196d3021c 339 xbee.printf("stand by, %f, %f\n", goal_Pressure, cansat.get_pressure());
s1200058 18:c86872baed44 340 }
s1200058 18:c86872baed44 341 }
s1200058 18:c86872baed44 342 if(short_flag >= 5){
s1200058 5:ba883a4bddc3 343 mode = 1;
s1210160 22:92408c551605 344 goal_Pressure = goal_Pressure - 0.5;
s1210160 21:18a196d3021c 345 pressure_Timer.start();
s1200058 19:cb3a4b4c3526 346 parachute_Timer.start();
s1200058 5:ba883a4bddc3 347 }
s1200058 5:ba883a4bddc3 348
kityann 0:1fcc61be1dcf 349 }
kityann 0:1fcc61be1dcf 350
kityann 0:1fcc61be1dcf 351 /******************************
kityann 0:1fcc61be1dcf 352 落下モード
kityann 0:1fcc61be1dcf 353 ******************************/
s1200058 15:dd7c2ab65a09 354 void falling_print(){
s1200058 15:dd7c2ab65a09 355
s1200058 19:cb3a4b4c3526 356 xbee.printf("%d: %04.2f hPa\n", log_number, cansat.get_pressure());
s1200058 15:dd7c2ab65a09 357
s1200058 15:dd7c2ab65a09 358 log_number++;
s1200058 15:dd7c2ab65a09 359
s1200058 15:dd7c2ab65a09 360 }
s1200058 15:dd7c2ab65a09 361
kityann 0:1fcc61be1dcf 362 void falling(){
s1200058 7:db6b436c0baa 363
s1210160 23:8581b740beef 364 cansat.set_temperature(sensor.getTemperature()); // 取得した温度を変数に格納
s1210160 23:8581b740beef 365 cansat.set_pressure(sensor.getPressure()); // 取得した気圧を変数に格納
s1210160 23:8581b740beef 366 cansat.set_humidity(sensor.getHumidity()); // 取得した湿度を変数に格納
s1200058 7:db6b436c0baa 367
s1200058 18:c86872baed44 368 falling_print();
s1200058 18:c86872baed44 369
s1210160 23:8581b740beef 370 if(cansat.get_pressure() >= goal_Pressure){ // 取得した気圧と閾値を比較
s1200058 7:db6b436c0baa 371 if(fall_flag == 0){
s1200058 7:db6b436c0baa 372 fall_flag = 1;
s1200058 7:db6b436c0baa 373 sep_Timer.reset();
s1200058 7:db6b436c0baa 374 }
s1200058 18:c86872baed44 375 }
s1210160 21:18a196d3021c 376
s1210160 23:8581b740beef 377 if(fall_flag == 1 && pressure_Timer.read_ms() >= pressure_Time){ // 取得した気圧が閾値より大きく、落下してから10秒以上経過しているとき
s1210160 23:8581b740beef 378 mode = 2; // 走行モードに移る
s1210160 24:e88f40aedb9c 379 nic = 1; // ニクロム線を切る
s1210160 21:18a196d3021c 380 xbee.printf("my pressure is high!\n");
s1210160 21:18a196d3021c 381 xbee.printf("remove the parachute\n");
s1210160 21:18a196d3021c 382 xbee.printf("change mode: running\n");
s1210160 21:18a196d3021c 383 }
s1210160 21:18a196d3021c 384
s1210160 23:8581b740beef 385 if(fall_flag == 0 && parachute_Timer.read_ms() >= parachute_Time){ // 取得した気圧が閾値より小さく、落下してから30秒以上経過しているとき
s1210160 23:8581b740beef 386 mode = 2; // 走行モードに移る
s1210160 23:8581b740beef 387 nic = 1; // ニクロム線を切る
s1200058 18:c86872baed44 388 xbee.printf("Time out!\n");
s1200058 18:c86872baed44 389 xbee.printf("remove the parachute\n");
s1200058 18:c86872baed44 390 wait_ms(3000);
s1200058 18:c86872baed44 391 nic = 0;
s1200058 18:c86872baed44 392 xbee.printf("change mode: running\n");
s1200058 7:db6b436c0baa 393
s1200058 5:ba883a4bddc3 394 }
s1200058 15:dd7c2ab65a09 395
s1200058 15:dd7c2ab65a09 396
kityann 0:1fcc61be1dcf 397 }
kityann 0:1fcc61be1dcf 398
kityann 0:1fcc61be1dcf 399 /******************************
kityann 0:1fcc61be1dcf 400 走行モード
kityann 0:1fcc61be1dcf 401 ******************************/
s1200058 14:4dfeeca65308 402
s1200058 14:4dfeeca65308 403 void running_print(){
s1200058 14:4dfeeca65308 404
s1200058 19:cb3a4b4c3526 405 if(running_flag <= 20){
s1200058 19:cb3a4b4c3526 406 xbee.printf("%d: %f, %f, %d\n", log_number, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
s1200058 19:cb3a4b4c3526 407 }
s1200058 19:cb3a4b4c3526 408 else{
s1200058 19:cb3a4b4c3526 409 xbee.printf("%d: %f, %f, %d\n", log_number, cansat.get_robotKalman_x(), cansat.get_robotKalman_y(), cansat.get_speed());
s1200058 19:cb3a4b4c3526 410 }
s1200058 19:cb3a4b4c3526 411 xbee.printf("%c, %f\n", cansat.motor_command, cansat.get_compass_angle());
s1200058 19:cb3a4b4c3526 412 xbee.printf("%d, %d\n",cansat.get_robot_angle(), cansat.get_target_angle());
s1200058 19:cb3a4b4c3526 413 // double y1 = cansat.get_target_y();
s1200058 19:cb3a4b4c3526 414 // double y2 = cansat.get_robot_y();
s1200058 19:cb3a4b4c3526 415 // double x1 = cansat.get_target_x();
s1200058 19:cb3a4b4c3526 416 // double x2 = cansat.get_robot_x();
s1200058 19:cb3a4b4c3526 417 // xbee.printf("taret_angle:%f",robot_compass(x1-x2, y1-y2));
s1200058 14:4dfeeca65308 418
s1200058 14:4dfeeca65308 419 log_number++;
s1200058 14:4dfeeca65308 420
s1200058 14:4dfeeca65308 421 }
s1200058 14:4dfeeca65308 422
kityann 0:1fcc61be1dcf 423 void running(){
s1200058 14:4dfeeca65308 424
s1200058 18:c86872baed44 425 // double r = 6378.137;
s1200058 5:ba883a4bddc3 426 double y1 = cansat.get_target_y();
s1200058 5:ba883a4bddc3 427 double y2 = cansat.get_robot_y();
s1200058 5:ba883a4bddc3 428 double x1 = cansat.get_target_x();
s1200058 5:ba883a4bddc3 429 double x2 = cansat.get_robot_x();
s1200058 19:cb3a4b4c3526 430 double x_k2 = cansat.get_robotKalman_x();
s1200058 19:cb3a4b4c3526 431 double y_k2 = cansat.get_robotKalman_y();
s1200058 5:ba883a4bddc3 432 double dx = x2 - x1;
s1200058 18:c86872baed44 433 double dy = y2 - y1;
s1200058 19:cb3a4b4c3526 434 double dx_k = x_k2 - x1;
s1200058 19:cb3a4b4c3526 435 double dy_k = y_k2 - y1;
s1200058 19:cb3a4b4c3526 436 double y_sub;
s1200058 19:cb3a4b4c3526 437 double x_sub;
s1200058 19:cb3a4b4c3526 438
s1200058 19:cb3a4b4c3526 439 if(running_flag < 20){
s1200058 19:cb3a4b4c3526 440 y_sub = dy*111135.0;
s1200058 19:cb3a4b4c3526 441 x_sub = dx*91191.0;
s1200058 19:cb3a4b4c3526 442 cansat.set_target_distance(sqrt(y_sub*y_sub + x_sub*x_sub));
s1200058 19:cb3a4b4c3526 443 cansat.set_target_angle(calc_angle(robot_compass(x1-x2, y1-y2)));
s1200058 19:cb3a4b4c3526 444 running_flag++;
s1200058 19:cb3a4b4c3526 445 }
s1200058 19:cb3a4b4c3526 446 else{
s1200058 19:cb3a4b4c3526 447 y_sub = dy_k*111135.0;
s1200058 19:cb3a4b4c3526 448 x_sub = dx_k*91191.0;
s1200058 19:cb3a4b4c3526 449 cansat.set_target_distance(sqrt(y_sub*y_sub + x_sub*x_sub));
s1200058 19:cb3a4b4c3526 450 cansat.set_target_angle(calc_angle(robot_compass(x1-x_k2, y1-y_k2)));
s1200058 19:cb3a4b4c3526 451 }
s1200058 14:4dfeeca65308 452
s1200058 19:cb3a4b4c3526 453 if(cansat.get_target_distance() < 10) cansat.set_speed(64);
s1200058 19:cb3a4b4c3526 454 else if(cansat.get_target_distance() < 20 && cansat.get_target_distance() > 10) cansat.set_speed(128);
s1200058 19:cb3a4b4c3526 455 else cansat.set_speed(255);
s1200058 5:ba883a4bddc3 456
s1200058 18:c86872baed44 457 if(fall_flag == 1 && sep_Timer.read_ms() >= sep_Time){
s1200058 18:c86872baed44 458 nic = 0;
s1200058 18:c86872baed44 459 }
s1200058 12:5724d4a57a4c 460 if(compass_Timer.read_ms() >= compass_Time){
s1200058 12:5724d4a57a4c 461 compass_Timer.reset();
s1200058 12:5724d4a57a4c 462 if(cansat.get_compass_z() < 0) {
s1200058 12:5724d4a57a4c 463 //ひっくり返っている
s1200058 12:5724d4a57a4c 464 cansat.control_Motor(0, cansat.get_speed());
s1200058 12:5724d4a57a4c 465 } else {
s1200058 14:4dfeeca65308 466 cansat.motor_command = robot_Action(cansat.get_robot_angle(), cansat.get_target_angle());
s1200058 14:4dfeeca65308 467 switch(cansat.motor_command) {
s1200058 12:5724d4a57a4c 468 case 'f': //前進
s1200058 12:5724d4a57a4c 469 cansat.control_Motor(0, cansat.get_speed());
s1200058 12:5724d4a57a4c 470 break;
s1200058 12:5724d4a57a4c 471 case 'l':
s1200058 12:5724d4a57a4c 472 cansat.control_Motor(2, cansat.get_speed());
s1200058 12:5724d4a57a4c 473 break;
s1200058 12:5724d4a57a4c 474 case 'r':
s1200058 12:5724d4a57a4c 475 cansat.control_Motor(3, cansat.get_speed());
s1200058 12:5724d4a57a4c 476 break;
s1200058 13:4f3fd6c4ddc2 477 case 'b':
s1200058 13:4f3fd6c4ddc2 478 cansat.control_Motor(4, cansat.get_speed());
s1200058 13:4f3fd6c4ddc2 479 break;
s1200058 12:5724d4a57a4c 480 }
s1200058 5:ba883a4bddc3 481 }
s1200058 5:ba883a4bddc3 482 }
s1200058 18:c86872baed44 483
s1200058 18:c86872baed44 484 running_print();
s1200058 18:c86872baed44 485
s1200058 7:db6b436c0baa 486 if(cansat.get_target_distance() <= 1){
s1200058 6:23bb3b018f44 487 mode = 100;
s1200058 14:4dfeeca65308 488 xbee.printf("change mode: stopping\n");
s1200058 14:4dfeeca65308 489 xbee.printf("it is goal point.\n");
s1200058 6:23bb3b018f44 490 }
kityann 0:1fcc61be1dcf 491 }
kityann 0:1fcc61be1dcf 492
kityann 0:1fcc61be1dcf 493 /******************************
kityann 0:1fcc61be1dcf 494 停止モード
kityann 0:1fcc61be1dcf 495 ******************************/
kityann 0:1fcc61be1dcf 496 void stopping(){
kityann 0:1fcc61be1dcf 497
s1200058 13:4f3fd6c4ddc2 498 cansat.control_Motor(1, cansat.get_speed());
kityann 0:1fcc61be1dcf 499
kityann 0:1fcc61be1dcf 500 }
kityann 0:1fcc61be1dcf 501
kityann 0:1fcc61be1dcf 502 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 503 //
kityann 0:1fcc61be1dcf 504 //Main Processing
kityann 0:1fcc61be1dcf 505 //
kityann 0:1fcc61be1dcf 506 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 507 int main() {
kityann 0:1fcc61be1dcf 508 //start up time
kityann 0:1fcc61be1dcf 509 wait(3);
kityann 0:1fcc61be1dcf 510 //set pc frequency to 57600bps
kityann 0:1fcc61be1dcf 511 pc.baud(PC_BAUD_RATE);
kityann 0:1fcc61be1dcf 512 //set xbee frequency to 57600bps
s1200058 12:5724d4a57a4c 513 //xbee.begin(XBEE_BAUD_RATE);
s1200058 19:cb3a4b4c3526 514 xbee.baud(57600);
s1210160 10:ce253d8a5f2c 515 //Compass setting
s1210160 10:ce253d8a5f2c 516 compass.init();
kityann 0:1fcc61be1dcf 517
kityann 0:1fcc61be1dcf 518 //GPS setting
kityann 0:1fcc61be1dcf 519 gps_Serial = new Serial(p28,p27);
kityann 0:1fcc61be1dcf 520 Adafruit_GPS myGPS(gps_Serial);
kityann 0:1fcc61be1dcf 521 Timer refresh_Timer;
kityann 0:1fcc61be1dcf 522 const int refresh_Time = 1000; //refresh time in ms
kityann 0:1fcc61be1dcf 523 int count = 0;
s1200058 19:cb3a4b4c3526 524 Timer compass_refresh_Timer;
s1200058 19:cb3a4b4c3526 525 const int compass_refresh_Time = 500;
kityann 0:1fcc61be1dcf 526
kityann 0:1fcc61be1dcf 527 myGPS.begin(GPS_BAUD_RATE);
kityann 0:1fcc61be1dcf 528
kityann 0:1fcc61be1dcf 529 //GPS Send Command
kityann 0:1fcc61be1dcf 530 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:1fcc61be1dcf 531 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:1fcc61be1dcf 532 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:1fcc61be1dcf 533
kityann 0:1fcc61be1dcf 534 wait_ms(2000);
kityann 0:1fcc61be1dcf 535
kityann 0:1fcc61be1dcf 536 //interrupt start
kityann 0:1fcc61be1dcf 537 refresh_Timer.start();
s1200058 13:4f3fd6c4ddc2 538 compass_Timer.start();
s1200058 14:4dfeeca65308 539 running_Timer.start();
s1200058 18:c86872baed44 540 sep_Timer.start();
s1200058 19:cb3a4b4c3526 541 compass_refresh_Timer.start();
s1200058 11:19091694455e 542 cansat.set_target(target_x, target_y);
s1200058 12:5724d4a57a4c 543 // wait_ms(10000);
s1200058 11:19091694455e 544
s1200058 19:cb3a4b4c3526 545 //compass_interrupt.attach(&Compass_intrpt, 0.5);
s1200058 19:cb3a4b4c3526 546
kityann 0:1fcc61be1dcf 547
kityann 0:1fcc61be1dcf 548 printf("start\n");
s1200058 19:cb3a4b4c3526 549 xbee.printf("target: %f, %f\n",cansat.get_target_x(), cansat.get_target_y());
s1200058 12:5724d4a57a4c 550 //int mode = -1;
s1200058 5:ba883a4bddc3 551 nic.output();
s1200058 5:ba883a4bddc3 552 nic = 0;
kityann 0:1fcc61be1dcf 553
s1200058 18:c86872baed44 554 short_out = 1;
s1200058 18:c86872baed44 555 wait_ms(1000);
s1200058 19:cb3a4b4c3526 556 // if(short_out){
s1200058 19:cb3a4b4c3526 557 // xbee.printf("HIGH\n");
s1200058 19:cb3a4b4c3526 558 // }
s1200058 18:c86872baed44 559
kityann 0:1fcc61be1dcf 560 while (true) {
s1200058 19:cb3a4b4c3526 561
s1200058 19:cb3a4b4c3526 562 myGPS.read();
s1200058 19:cb3a4b4c3526 563 //recive gps module
s1200058 19:cb3a4b4c3526 564 //check if we recieved a new message from GPS, if so, attempt to parse it,
s1200058 19:cb3a4b4c3526 565 if ( myGPS.newNMEAreceived() ) {
s1200058 19:cb3a4b4c3526 566 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
s1200058 19:cb3a4b4c3526 567 continue;
s1200058 19:cb3a4b4c3526 568 }
s1200058 19:cb3a4b4c3526 569 else{
s1200058 19:cb3a4b4c3526 570 count++;
s1200058 19:cb3a4b4c3526 571 }
s1200058 19:cb3a4b4c3526 572 }
s1200058 19:cb3a4b4c3526 573 //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
s1200058 19:cb3a4b4c3526 574 if (refresh_Timer.read_ms() >= refresh_Time) {
s1200058 19:cb3a4b4c3526 575 refresh_Timer.reset();
s1200058 19:cb3a4b4c3526 576 //print_gps(count);
s1200058 19:cb3a4b4c3526 577 Get_GPS(&myGPS);
s1200058 19:cb3a4b4c3526 578 //log++;
s1200058 19:cb3a4b4c3526 579 /* xbee.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
s1200058 19:cb3a4b4c3526 580 xbee.printf("moter command: %c\n", cansat.motor_command);
s1200058 19:cb3a4b4c3526 581 xbee.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_angle(), cansat.get_pressure());
s1200058 19:cb3a4b4c3526 582 */
s1200058 19:cb3a4b4c3526 583 }
s1200058 19:cb3a4b4c3526 584 if(compass_refresh_Timer.read_ms() >= compass_refresh_Time){
s1200058 19:cb3a4b4c3526 585 Compass_intrpt();
s1200058 19:cb3a4b4c3526 586 compass_refresh_Timer.reset();
s1200058 19:cb3a4b4c3526 587 }
s1200058 14:4dfeeca65308 588
kityann 0:1fcc61be1dcf 589 switch(mode){
kityann 0:1fcc61be1dcf 590 //スタートモード：パラシュートが開くまではこのモードを実行
kityann 0:1fcc61be1dcf 591 case -1:
kityann 0:1fcc61be1dcf 592 standby();
kityann 0:1fcc61be1dcf 593 break;
kityann 0:1fcc61be1dcf 594 //落下モード：落下時はこのモード。気圧計または、時間でロボットとパラシュートを分離する
kityann 0:1fcc61be1dcf 595 case 1:
kityann 0:1fcc61be1dcf 596 falling();
kityann 0:1fcc61be1dcf 597 break;
kityann 0:1fcc61be1dcf 598 //走行モード：ターゲットにむかって走行を行う
kityann 0:1fcc61be1dcf 599 case 2:
s1200058 14:4dfeeca65308 600 if (running_Timer.read_ms() >= running_Time) {
s1200058 14:4dfeeca65308 601 running_Timer.reset();
s1200058 14:4dfeeca65308 602 running();
s1200058 14:4dfeeca65308 603 }
kityann 0:1fcc61be1dcf 604 break;
kityann 0:1fcc61be1dcf 605 //停止モード：ターゲット
kityann 0:1fcc61be1dcf 606 case 100:
kityann 0:1fcc61be1dcf 607 stopping();
kityann 0:1fcc61be1dcf 608 break;
kityann 0:1fcc61be1dcf 609 }
s1210160 10:ce253d8a5f2c 610
s1200058 11:19091694455e 611 /* while(1){
s1210160 10:ce253d8a5f2c 612
s1210160 10:ce253d8a5f2c 613 printf("compass x : %i, compass y : %i, compass z : %i\n", raw[0], raw[1], raw[2]);
s1210160 10:ce253d8a5f2c 614 printf("set compass x : %i, set compass y : %i, set compass z : %i\n", cansat.get_compass_x(), cansat.get_compass_y(), cansat.get_compass_z());
s1210160 10:ce253d8a5f2c 615 printf("compass angle : %f\n", headingLPF);
s1210160 10:ce253d8a5f2c 616 printf("set compass angle : %f\n", cansat.get_compass_angle());
s1210160 10:ce253d8a5f2c 617 printf("robot angle : %d\n", calc_angle(headingLPF));
s1210160 10:ce253d8a5f2c 618 printf("set robot angle : %d\n", cansat.get_robot_angle());
s1210160 10:ce253d8a5f2c 619 }
s1200058 11:19091694455e 620 */
s1200058 19:cb3a4b4c3526 621
kityann 0:1fcc61be1dcf 622 }
kityann 0:1fcc61be1dcf 623
kityann 0:1fcc61be1dcf 624 }