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@11:19091694455e, 2015-08-10 (annotated)

Committer:
s1200058
Date:
Mon Aug 10 18:22:40 2015 +0000
Revision:
11:19091694455e
Parent:
10:ce253d8a5f2c
Child:
12:5724d4a57a4c
add set_target();

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
s1210160 10:ce253d8a5f2c 20 #define STOP 0 //compass initial
s1210160 10:ce253d8a5f2c 21 #define CAL 1 //compass calibration
s1210160 10:ce253d8a5f2c 22 #define RUN 2 //compass run
s1210160 10:ce253d8a5f2c 23
kityann 0:1fcc61be1dcf 24 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 25 //
kityann 0:1fcc61be1dcf 26 //Pin Setting
kityann 0:1fcc61be1dcf 27 //
kityann 0:1fcc61be1dcf 28 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 29 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:1fcc61be1dcf 30
kityann 0:1fcc61be1dcf 31
kityann 0:1fcc61be1dcf 32 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 33 //
kityann 0:1fcc61be1dcf 34 //Connection Setting
kityann 0:1fcc61be1dcf 35 //
kityann 0:1fcc61be1dcf 36 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 37
kityann 0:1fcc61be1dcf 38 //Serial Connect Setting: PC <--> mbed
kityann 0:1fcc61be1dcf 39 Serial pc(USBTX, USBRX);
kityann 0:1fcc61be1dcf 40
kityann 0:1fcc61be1dcf 41 //Serial Connect Setting: GPS <--> mbed
kityann 0:1fcc61be1dcf 42 Serial * gps_Serial;
kityann 0:1fcc61be1dcf 43
kityann 0:1fcc61be1dcf 44 //Serial Connect Setting: XBEE <--> mbed
kityann 0:1fcc61be1dcf 45 XBee xbee(p13,p14);
kityann 0:1fcc61be1dcf 46 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:1fcc61be1dcf 47
s1210160 10:ce253d8a5f2c 48 // compass
s1210160 10:ce253d8a5f2c 49 HMC5883L compass(p9, p10);
s1210160 10:ce253d8a5f2c 50
kityann 0:1fcc61be1dcf 51 //set up GPS module
kityann 0:1fcc61be1dcf 52
kityann 0:1fcc61be1dcf 53 //set up AigamozuControlPackets library
kityann 1:f1f7413ae6bd 54 //AigamozuControlPackets agz(agz_motorShield);
kityann 3:0bd9ad37f319 55 CanSat cansat(agz_motorShield);
kityann 0:1fcc61be1dcf 56
s1200058 5:ba883a4bddc3 57 //set up for tempratures...
s1200058 5:ba883a4bddc3 58 #if defined(TARGET_LPC1768)
s1200058 5:ba883a4bddc3 59 BME280 sensor(p9, p10);
s1200058 5:ba883a4bddc3 60 #else
s1200058 5:ba883a4bddc3 61 BME280 sensor(I2C_SDA, I2C_SCL);
s1200058 5:ba883a4bddc3 62 #endif
s1200058 5:ba883a4bddc3 63
s1200058 5:ba883a4bddc3 64 DigitalIn short_in(p29);
s1200058 5:ba883a4bddc3 65 DigitalOut short_out(p30);
s1200058 5:ba883a4bddc3 66 DigitalInOut nic(p5);
kityann 0:1fcc61be1dcf 67
kityann 0:1fcc61be1dcf 68 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 69 //
s1210160 10:ce253d8a5f2c 70 //For Compass data
s1210160 10:ce253d8a5f2c 71 //
s1210160 10:ce253d8a5f2c 72 /////////////////////////////////////////
s1210160 10:ce253d8a5f2c 73 Ticker compass_interrupt;
s1210160 10:ce253d8a5f2c 74 double heading0 = 0.0;
s1210160 10:ce253d8a5f2c 75 double heading1 = 0.0;
s1210160 10:ce253d8a5f2c 76 double heading2 = 0.0;
s1210160 10:ce253d8a5f2c 77 double heading3 = 0.0;
s1210160 10:ce253d8a5f2c 78 double headingLPF = 0.0;
s1210160 10:ce253d8a5f2c 79 double initHeading;
s1210160 10:ce253d8a5f2c 80 double tgtHeading;
s1210160 10:ce253d8a5f2c 81 double preHeading = 0.0;
s1210160 10:ce253d8a5f2c 82
s1210160 10:ce253d8a5f2c 83 int maxX, minX, maxY, minY;
s1210160 10:ce253d8a5f2c 84 const int ofsX = -122; //calibration x
s1210160 10:ce253d8a5f2c 85 const int ofsY = -137; //calibration y
s1210160 10:ce253d8a5f2c 86
s1210160 10:ce253d8a5f2c 87 int16_t raw[3];
s1210160 10:ce253d8a5f2c 88
s1210160 10:ce253d8a5f2c 89 /////////////////////////////////////////
s1210160 10:ce253d8a5f2c 90 //
kityann 0:1fcc61be1dcf 91 //For Kalman data
kityann 0:1fcc61be1dcf 92 //
kityann 0:1fcc61be1dcf 93 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 94 #define FIRST_S2_1 1.0e-8
kityann 0:1fcc61be1dcf 95 #define FIRST_S2_2 1.0e-6
kityann 0:1fcc61be1dcf 96 #define COUNTER_MAX 10000
kityann 0:1fcc61be1dcf 97 #define ERROR_RANGE 0.001
kityann 0:1fcc61be1dcf 98
kityann 0:1fcc61be1dcf 99 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
kityann 0:1fcc61be1dcf 100 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 101 double s2_R=FIRST_S2_2;//GPSセンサの分散
kityann 0:1fcc61be1dcf 102 double s2_Q=FIRST_S2_2;
kityann 0:1fcc61be1dcf 103 double Kx=0,Ky=0;//カルマンゲイン
kityann 0:1fcc61be1dcf 104 double zx,zy;//観測値
kityann 0:1fcc61be1dcf 105 void Kalman(double Latitude,double Longitude);
kityann 0:1fcc61be1dcf 106 int change = 0;
kityann 0:1fcc61be1dcf 107
s1200058 11:19091694455e 108 int mode = 2; //ロボットのモード
s1200058 11:19091694455e 109 double target_x = 37.52260177176629,target_y = 139.93881346945034;
s1200058 5:ba883a4bddc3 110 double goal_Pressure, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度
kityann 0:1fcc61be1dcf 111
s1200058 7:db6b436c0baa 112 Timer sep_Timer;
s1200058 7:db6b436c0baa 113 const int sep_Time = 30000; //seperate time in ms
s1200058 7:db6b436c0baa 114 int fall_flag = 0;
s1200058 7:db6b436c0baa 115
kityann 0:1fcc61be1dcf 116
kityann 0:1fcc61be1dcf 117 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 118 //
kityann 0:1fcc61be1dcf 119 //Kalman Processing
kityann 0:1fcc61be1dcf 120 //
kityann 0:1fcc61be1dcf 121 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 122 void calc_Kalman(){
kityann 0:1fcc61be1dcf 123 //calc Kalman gain
kityann 0:1fcc61be1dcf 124 Kx = (s2x_prev+s2_Q)/(s2x_prev+s2_R+s2_Q);
kityann 0:1fcc61be1dcf 125 Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q);
kityann 0:1fcc61be1dcf 126 //estimate
kityann 0:1fcc61be1dcf 127 x_cur = x_prev + Kx*(zx-x_prev);
kityann 0:1fcc61be1dcf 128 y_cur = y_prev + Ky*(zy-y_prev);
kityann 0:1fcc61be1dcf 129 //calc sigma
kityann 0:1fcc61be1dcf 130 s2x_cur = (1-Kx)*(s2x_prev+s2_Q);
kityann 0:1fcc61be1dcf 131 s2y_cur = (1-Ky)*(s2y_prev+s2_Q);
kityann 0:1fcc61be1dcf 132
kityann 0:1fcc61be1dcf 133 }
kityann 0:1fcc61be1dcf 134
kityann 0:1fcc61be1dcf 135 void Kalman(double Latitude,double Longitude){
kityann 0:1fcc61be1dcf 136
kityann 0:1fcc61be1dcf 137 zx = Latitude;
kityann 0:1fcc61be1dcf 138 zy = Longitude;
kityann 0:1fcc61be1dcf 139
kityann 0:1fcc61be1dcf 140 calc_Kalman();
kityann 0:1fcc61be1dcf 141
kityann 0:1fcc61be1dcf 142 //更新
kityann 0:1fcc61be1dcf 143 x_prev = x_cur;
kityann 0:1fcc61be1dcf 144 y_prev = y_cur;
kityann 0:1fcc61be1dcf 145 s2x_prev = s2x_cur;
kityann 0:1fcc61be1dcf 146 s2y_prev = s2y_cur;
kityann 0:1fcc61be1dcf 147
kityann 3:0bd9ad37f319 148 //robotK\x,robotK_yに格納する
kityann 3:0bd9ad37f319 149 cansat.set_robotKalman_x(x_cur);
kityann 3:0bd9ad37f319 150 cansat.set_robotKalman_y(y_cur);
kityann 0:1fcc61be1dcf 151 }
kityann 0:1fcc61be1dcf 152
kityann 0:1fcc61be1dcf 153 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 154 //
kityann 0:1fcc61be1dcf 155 //Get GPS function
kityann 0:1fcc61be1dcf 156 //
kityann 0:1fcc61be1dcf 157 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 158
kityann 0:1fcc61be1dcf 159 void Get_GPS(Adafruit_GPS *myGPS){
kityann 0:1fcc61be1dcf 160 static int flag = 0;
kityann 0:1fcc61be1dcf 161
kityann 0:1fcc61be1dcf 162 if (myGPS->fix) {
kityann 0:1fcc61be1dcf 163
kityann 3:0bd9ad37f319 164 cansat.nowStatus = GPS_AVAIL;
kityann 3:0bd9ad37f319 165 cansat.set_robot_x((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 10000.0));
kityann 3:0bd9ad37f319 166 cansat.set_robot_y((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 10000.0));
kityann 3:0bd9ad37f319 167
kityann 0:1fcc61be1dcf 168
kityann 0:1fcc61be1dcf 169 if(flag < COUNTER_MAX){
kityann 0:1fcc61be1dcf 170 flag++;
kityann 0:1fcc61be1dcf 171 }
kityann 0:1fcc61be1dcf 172 if(flag == 5){
kityann 3:0bd9ad37f319 173 x_prev = cansat.get_robot_x();
kityann 3:0bd9ad37f319 174 y_prev = cansat.get_robot_y();
kityann 0:1fcc61be1dcf 175 }
kityann 0:1fcc61be1dcf 176
kityann 0:1fcc61be1dcf 177 if(flag >= 6){
kityann 3:0bd9ad37f319 178 if(abs(x_prev - cansat.get_robot_x()) < ERROR_RANGE && abs(y_prev - cansat.get_robot_y()) < ERROR_RANGE){
kityann 3:0bd9ad37f319 179 Kalman(cansat.get_robot_x(), cansat.get_robot_y());
kityann 0:1fcc61be1dcf 180 change = 1;
kityann 0:1fcc61be1dcf 181 }
kityann 0:1fcc61be1dcf 182 else{
kityann 0:1fcc61be1dcf 183 change = 0;
kityann 0:1fcc61be1dcf 184 }
kityann 1:f1f7413ae6bd 185 //printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
kityann 1:f1f7413ae6bd 186 //agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 1:f1f7413ae6bd 187 //agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 1:f1f7413ae6bd 188 //agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 1:f1f7413ae6bd 189 }
kityann 0:1fcc61be1dcf 190 }
kityann 3:0bd9ad37f319 191 else cansat.nowStatus = GPS_UNAVAIL;
kityann 0:1fcc61be1dcf 192
kityann 0:1fcc61be1dcf 193 }
kityann 0:1fcc61be1dcf 194
aoki0731 4:0fc7221e2f79 195 //ロボットの動き方
s1200058 11:19091694455e 196 char robot_Action(int robot_angle, int target_angle) {
aoki0731 4:0fc7221e2f79 197
s1200058 11:19091694455e 198 int n, t, r;
aoki0731 4:0fc7221e2f79 199 t = target_angle;
aoki0731 4:0fc7221e2f79 200 r = robot_angle;
aoki0731 4:0fc7221e2f79 201 n = r - t;
aoki0731 4:0fc7221e2f79 202 if(n<0) n *= -1;
aoki0731 4:0fc7221e2f79 203
aoki0731 4:0fc7221e2f79 204
aoki0731 4:0fc7221e2f79 205 if(t==r) {
aoki0731 4:0fc7221e2f79 206 //前進
aoki0731 4:0fc7221e2f79 207 return 'f';
aoki0731 4:0fc7221e2f79 208 } else if(n < 4) {
aoki0731 4:0fc7221e2f79 209 if(t > r)
aoki0731 4:0fc7221e2f79 210 t -= 8;
aoki0731 4:0fc7221e2f79 211 else
aoki0731 4:0fc7221e2f79 212 r -= 8;
aoki0731 4:0fc7221e2f79 213
aoki0731 4:0fc7221e2f79 214 if(r-t > 0)
aoki0731 4:0fc7221e2f79 215 return 'l';
aoki0731 4:0fc7221e2f79 216 else
aoki0731 4:0fc7221e2f79 217 return 'r';
aoki0731 4:0fc7221e2f79 218 } else if(n >= 4) {
aoki0731 4:0fc7221e2f79 219 if(t > r)
aoki0731 4:0fc7221e2f79 220 t -= 8;
aoki0731 4:0fc7221e2f79 221 else
aoki0731 4:0fc7221e2f79 222 r -= 8;
aoki0731 4:0fc7221e2f79 223
aoki0731 4:0fc7221e2f79 224 if(r-t > 0)
aoki0731 4:0fc7221e2f79 225 return 'r';
aoki0731 4:0fc7221e2f79 226 else
aoki0731 4:0fc7221e2f79 227 return 'l';
aoki0731 4:0fc7221e2f79 228 }
aoki0731 4:0fc7221e2f79 229
aoki0731 4:0fc7221e2f79 230 return 'b';
aoki0731 4:0fc7221e2f79 231 }
aoki0731 4:0fc7221e2f79 232
s1200058 5:ba883a4bddc3 233 //対象とロボットの角度
s1200058 5:ba883a4bddc3 234 double robot_compass(double robot_x, double robot_y) {
s1200058 5:ba883a4bddc3 235 double angle = 0;
s1200058 5:ba883a4bddc3 236
s1200058 5:ba883a4bddc3 237 if(robot_x==0&&robot_y>0)
s1200058 5:ba883a4bddc3 238 return 0;
s1200058 5:ba883a4bddc3 239 else if(robot_x>0&&robot_y==0) //東
s1200058 5:ba883a4bddc3 240 return 90;
s1200058 5:ba883a4bddc3 241 else if(robot_x==0&&robot_y<0) //南
s1200058 5:ba883a4bddc3 242 return 180;
s1200058 5:ba883a4bddc3 243 else if(robot_x<0&&robot_y==0) //西
s1200058 5:ba883a4bddc3 244 return 270;
s1200058 5:ba883a4bddc3 245 else if(robot_x>=0&&robot_y>=0) { //北東
s1200058 5:ba883a4bddc3 246 if(robot_x<=robot_y)
s1200058 5:ba883a4bddc3 247 angle = atan2(robot_x, robot_y);
s1200058 5:ba883a4bddc3 248 else
s1200058 5:ba883a4bddc3 249 angle = (M_PI/2) - atan2(robot_y, robot_x);
s1200058 5:ba883a4bddc3 250 return angle * 180.0 / M_PI;
s1200058 5:ba883a4bddc3 251 } else if(robot_x>=0&&robot_y<0) { //南東
s1200058 5:ba883a4bddc3 252 if(robot_x>abs(robot_y)){
s1200058 5:ba883a4bddc3 253 angle = (M_PI/2) - atan2(abs(robot_y), robot_x);
s1200058 5:ba883a4bddc3 254 }
s1200058 5:ba883a4bddc3 255 else{
s1200058 5:ba883a4bddc3 256 angle = atan2(abs(robot_y), robot_x);
s1200058 5:ba883a4bddc3 257 }
s1200058 5:ba883a4bddc3 258 return angle * 180.0 / M_PI + 90;
s1200058 5:ba883a4bddc3 259 } else if(robot_x<0&&robot_y<0) { //南西
s1200058 5:ba883a4bddc3 260 if(abs(robot_x)<abs(robot_y)){
s1200058 5:ba883a4bddc3 261 angle = atan2(abs(robot_x), abs(robot_y));
s1200058 5:ba883a4bddc3 262 }
s1200058 5:ba883a4bddc3 263 else{
s1200058 5:ba883a4bddc3 264 angle = (M_PI/2) - atan2(abs(robot_y), abs(robot_x));
s1200058 5:ba883a4bddc3 265 }
s1200058 5:ba883a4bddc3 266 return angle * 180.0 / M_PI + 180;
s1200058 5:ba883a4bddc3 267 } else if(robot_x<0&&robot_y>=0) { //北西
s1200058 5:ba883a4bddc3 268 if(abs(robot_x)>robot_y){
s1200058 5:ba883a4bddc3 269 angle = (M_PI/2) - atan2(robot_y, abs(robot_x));
s1200058 5:ba883a4bddc3 270 }
s1200058 5:ba883a4bddc3 271 else{
s1200058 5:ba883a4bddc3 272 angle = atan2(abs(robot_x), robot_y);
s1200058 5:ba883a4bddc3 273 }
kityann 9:2741e17438d6 274 return 360 - angle * 180.0 / M_PI;
s1200058 5:ba883a4bddc3 275 }
s1200058 5:ba883a4bddc3 276
s1200058 5:ba883a4bddc3 277 return -1;
s1200058 5:ba883a4bddc3 278 }
s1200058 5:ba883a4bddc3 279
s1210160 10:ce253d8a5f2c 280 int calc_angle(double c){
s1200058 11:19091694455e 281 if(c > 337.5 ||(c >=0 && c < 22.5)) return 0;
s1210160 10:ce253d8a5f2c 282 else if(c >= 22.5 && c < 67.5) return 1;
s1210160 10:ce253d8a5f2c 283 else if(c >= 67.5 && c < 112.5) return 2;
s1210160 10:ce253d8a5f2c 284 else if(c >= 112.5 && c < 157.5) return 3;
s1210160 10:ce253d8a5f2c 285 else if(c >= 157.5 && c < 202.5) return 4;
s1210160 10:ce253d8a5f2c 286 else if(c >= 202.5 && c < 247.5) return 5;
s1210160 10:ce253d8a5f2c 287 else if(c >= 247.5 && c < 292.5) return 6;
s1210160 10:ce253d8a5f2c 288 else if(c >= 292.5 && c < 337.5) return 7;
s1210160 10:ce253d8a5f2c 289 else return 8;
s1210160 10:ce253d8a5f2c 290 }
s1200058 5:ba883a4bddc3 291
s1210160 10:ce253d8a5f2c 292 void Compass_intrpt(){
s1210160 10:ce253d8a5f2c 293
s1210160 10:ce253d8a5f2c 294 compass.getXYZ(raw);
s1210160 10:ce253d8a5f2c 295 double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2]
s1210160 10:ce253d8a5f2c 296 if(heading < 0)heading += 2*M_PI;
s1210160 10:ce253d8a5f2c 297 if(heading > 2*M_PI)heading -= 2*M_PI;
s1210160 10:ce253d8a5f2c 298 heading3 = heading2;
s1210160 10:ce253d8a5f2c 299 heading2 = heading1;
s1210160 10:ce253d8a5f2c 300 heading1 = heading0;
s1210160 10:ce253d8a5f2c 301 heading0 = heading;
s1210160 10:ce253d8a5f2c 302 headingLPF = (heading0 + heading1 + heading2 + heading3)/4; //low pass filter
s1210160 10:ce253d8a5f2c 303
s1210160 10:ce253d8a5f2c 304 headingLPF = headingLPF * 180.0 / M_PI;
s1210160 10:ce253d8a5f2c 305 // pc.printf("heading=%f\r\n",headingLPF);
s1210160 10:ce253d8a5f2c 306
s1210160 10:ce253d8a5f2c 307 cansat.set_compass(raw[0], raw[2], raw[1], headingLPF);
s1210160 10:ce253d8a5f2c 308 cansat.set_robot_angle(calc_angle(headingLPF));
s1210160 10:ce253d8a5f2c 309 }
s1210160 10:ce253d8a5f2c 310
s1210160 10:ce253d8a5f2c 311
kityann 0:1fcc61be1dcf 312 /******************************
kityann 0:1fcc61be1dcf 313 スタンバイモード
kityann 0:1fcc61be1dcf 314 ******************************/
kityann 0:1fcc61be1dcf 315 void standby(){
s1200058 5:ba883a4bddc3 316
s1200058 6:23bb3b018f44 317 cansat.control_Motor(1, cansat.get_speed());
s1200058 5:ba883a4bddc3 318 if(short_in == 0){
s1200058 5:ba883a4bddc3 319 mode = 1;
s1200058 5:ba883a4bddc3 320 }
s1200058 5:ba883a4bddc3 321
kityann 0:1fcc61be1dcf 322 }
kityann 0:1fcc61be1dcf 323
kityann 0:1fcc61be1dcf 324 /******************************
kityann 0:1fcc61be1dcf 325 落下モード
kityann 0:1fcc61be1dcf 326 ******************************/
kityann 0:1fcc61be1dcf 327 void falling(){
s1200058 7:db6b436c0baa 328
s1200058 7:db6b436c0baa 329 cansat.set_temperature(sensor.getTemperature());
s1200058 7:db6b436c0baa 330 cansat.set_pressure(sensor.getPressure());
s1200058 7:db6b436c0baa 331 cansat.set_humidity(sensor.getHumidity());
s1200058 7:db6b436c0baa 332
s1200058 5:ba883a4bddc3 333 if(cansat.get_pressure() >= goal_Pressure){
s1200058 7:db6b436c0baa 334 if(fall_flag == 0){
s1200058 7:db6b436c0baa 335 nic = 1;
s1200058 7:db6b436c0baa 336 fall_flag = 1;
s1200058 7:db6b436c0baa 337 sep_Timer.reset();
s1200058 7:db6b436c0baa 338 }
s1200058 7:db6b436c0baa 339 if(sep_Timer.read_ms() >= sep_Time){
s1200058 7:db6b436c0baa 340 mode = 2;
s1200058 7:db6b436c0baa 341 nic = 0;
s1200058 7:db6b436c0baa 342 }
s1200058 7:db6b436c0baa 343
s1200058 5:ba883a4bddc3 344 }
kityann 0:1fcc61be1dcf 345 }
kityann 0:1fcc61be1dcf 346
kityann 0:1fcc61be1dcf 347 /******************************
kityann 0:1fcc61be1dcf 348 走行モード
kityann 0:1fcc61be1dcf 349 ******************************/
kityann 0:1fcc61be1dcf 350 void running(){
aoki0731 4:0fc7221e2f79 351 double r = 6378.137;
s1200058 5:ba883a4bddc3 352 double y1 = cansat.get_target_y();
s1200058 5:ba883a4bddc3 353 double y2 = cansat.get_robot_y();
s1200058 5:ba883a4bddc3 354 double x1 = cansat.get_target_x();
s1200058 5:ba883a4bddc3 355 double x2 = cansat.get_robot_x();
s1200058 5:ba883a4bddc3 356 double dx = x2 - x1;
s1200058 5:ba883a4bddc3 357
s1200058 7:db6b436c0baa 358 cansat.set_target_distance(r*acos(sin(y1)*sin(y2)+cos(y1)*cos(y2)*cos(dx)));
s1200058 5:ba883a4bddc3 359
s1200058 5:ba883a4bddc3 360 if(cansat.get_target_distance() < 10) cansat.set_speed(32);
s1200058 5:ba883a4bddc3 361 else if(cansat.get_target_distance() < 20 && cansat.get_target_distance() > 10) cansat.set_speed(64);
s1200058 5:ba883a4bddc3 362 else cansat.set_speed(128);
s1200058 5:ba883a4bddc3 363
s1200058 5:ba883a4bddc3 364 if(cansat.get_compass_z() < 0) {
aoki0731 4:0fc7221e2f79 365 //ひっくり返っている
s1200058 5:ba883a4bddc3 366 cansat.control_Motor(0, cansat.get_speed());
s1200058 5:ba883a4bddc3 367 } else {
s1200058 5:ba883a4bddc3 368 switch(robot_Action(cansat.get_robot_angle(), cansat.get_target_angle())) {
s1200058 5:ba883a4bddc3 369 case 'f': //前進
s1200058 5:ba883a4bddc3 370 cansat.control_Motor(0, cansat.get_speed());
s1200058 5:ba883a4bddc3 371 break;
s1200058 5:ba883a4bddc3 372 case 'l':
s1200058 5:ba883a4bddc3 373 cansat.control_Motor(2, cansat.get_speed());
s1200058 5:ba883a4bddc3 374 break;
s1200058 5:ba883a4bddc3 375 case 'r':
s1200058 5:ba883a4bddc3 376 cansat.control_Motor(3, cansat.get_speed());
s1200058 5:ba883a4bddc3 377 break;
s1200058 5:ba883a4bddc3 378 }
s1200058 5:ba883a4bddc3 379 }
s1200058 5:ba883a4bddc3 380
s1200058 7:db6b436c0baa 381 if(cansat.get_target_distance() <= 1){
s1200058 6:23bb3b018f44 382 mode = 100;
s1200058 6:23bb3b018f44 383 }
kityann 0:1fcc61be1dcf 384 }
kityann 0:1fcc61be1dcf 385
kityann 0:1fcc61be1dcf 386 /******************************
kityann 0:1fcc61be1dcf 387 停止モード
kityann 0:1fcc61be1dcf 388 ******************************/
kityann 0:1fcc61be1dcf 389 void stopping(){
kityann 0:1fcc61be1dcf 390
s1200058 6:23bb3b018f44 391 cansat.control_Motor(0, cansat.get_speed());
kityann 0:1fcc61be1dcf 392
kityann 0:1fcc61be1dcf 393 }
kityann 0:1fcc61be1dcf 394
kityann 0:1fcc61be1dcf 395 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 396 //
kityann 0:1fcc61be1dcf 397 //Main Processing
kityann 0:1fcc61be1dcf 398 //
kityann 0:1fcc61be1dcf 399 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 400 int main() {
kityann 0:1fcc61be1dcf 401 //start up time
kityann 0:1fcc61be1dcf 402 wait(3);
kityann 0:1fcc61be1dcf 403 //set pc frequency to 57600bps
kityann 0:1fcc61be1dcf 404 pc.baud(PC_BAUD_RATE);
kityann 0:1fcc61be1dcf 405 //set xbee frequency to 57600bps
kityann 0:1fcc61be1dcf 406 xbee.begin(XBEE_BAUD_RATE);
kityann 0:1fcc61be1dcf 407
s1210160 10:ce253d8a5f2c 408 //Compass setting
s1210160 10:ce253d8a5f2c 409 compass.init();
kityann 0:1fcc61be1dcf 410
kityann 0:1fcc61be1dcf 411 //GPS setting
kityann 0:1fcc61be1dcf 412 gps_Serial = new Serial(p28,p27);
kityann 0:1fcc61be1dcf 413 Adafruit_GPS myGPS(gps_Serial);
kityann 0:1fcc61be1dcf 414 Timer refresh_Timer;
kityann 0:1fcc61be1dcf 415 const int refresh_Time = 1000; //refresh time in ms
kityann 0:1fcc61be1dcf 416 int count = 0;
kityann 0:1fcc61be1dcf 417
kityann 0:1fcc61be1dcf 418 myGPS.begin(GPS_BAUD_RATE);
kityann 0:1fcc61be1dcf 419
kityann 0:1fcc61be1dcf 420 //GPS Send Command
kityann 0:1fcc61be1dcf 421 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:1fcc61be1dcf 422 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:1fcc61be1dcf 423 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:1fcc61be1dcf 424
kityann 0:1fcc61be1dcf 425 wait_ms(2000);
kityann 0:1fcc61be1dcf 426
kityann 0:1fcc61be1dcf 427 //interrupt start
kityann 0:1fcc61be1dcf 428 refresh_Timer.start();
s1200058 11:19091694455e 429
s1200058 11:19091694455e 430 cansat.set_target(target_x, target_y);
s1200058 11:19091694455e 431 wait_ms(10000);
s1200058 11:19091694455e 432
s1200058 11:19091694455e 433 compass_interrupt.attach(&Compass_intrpt, 0.5);
kityann 0:1fcc61be1dcf 434
kityann 0:1fcc61be1dcf 435 printf("start\n");
kityann 0:1fcc61be1dcf 436
s1200058 5:ba883a4bddc3 437 // int mode = -1;
s1200058 5:ba883a4bddc3 438 short_out = 1; //ショートピンの出力:high
s1200058 5:ba883a4bddc3 439 nic.output();
s1200058 5:ba883a4bddc3 440 nic = 0;
s1200058 7:db6b436c0baa 441 int log = 0;
kityann 0:1fcc61be1dcf 442
kityann 0:1fcc61be1dcf 443 while (true) {
kityann 0:1fcc61be1dcf 444
kityann 0:1fcc61be1dcf 445 switch(mode){
kityann 0:1fcc61be1dcf 446 //スタートモード：パラシュートが開くまではこのモードを実行
kityann 0:1fcc61be1dcf 447 case -1:
kityann 0:1fcc61be1dcf 448 standby();
kityann 0:1fcc61be1dcf 449 break;
kityann 0:1fcc61be1dcf 450 //落下モード：落下時はこのモード。気圧計または、時間でロボットとパラシュートを分離する
kityann 0:1fcc61be1dcf 451 case 1:
kityann 0:1fcc61be1dcf 452 falling();
kityann 0:1fcc61be1dcf 453 break;
kityann 0:1fcc61be1dcf 454 //走行モード：ターゲットにむかって走行を行う
kityann 0:1fcc61be1dcf 455 case 2:
kityann 0:1fcc61be1dcf 456 running();
kityann 0:1fcc61be1dcf 457 break;
kityann 0:1fcc61be1dcf 458 //停止モード：ターゲット
kityann 0:1fcc61be1dcf 459 case 100:
kityann 0:1fcc61be1dcf 460 stopping();
kityann 0:1fcc61be1dcf 461 break;
kityann 0:1fcc61be1dcf 462 }
s1210160 10:ce253d8a5f2c 463
s1200058 11:19091694455e 464 /* while(1){
s1210160 10:ce253d8a5f2c 465
s1210160 10:ce253d8a5f2c 466 printf("compass x : %i, compass y : %i, compass z : %i\n", raw[0], raw[1], raw[2]);
s1210160 10:ce253d8a5f2c 467 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 468 printf("compass angle : %f\n", headingLPF);
s1210160 10:ce253d8a5f2c 469 printf("set compass angle : %f\n", cansat.get_compass_angle());
s1210160 10:ce253d8a5f2c 470 printf("robot angle : %d\n", calc_angle(headingLPF));
s1210160 10:ce253d8a5f2c 471 printf("set robot angle : %d\n", cansat.get_robot_angle());
s1210160 10:ce253d8a5f2c 472 }
s1200058 11:19091694455e 473 */
kityann 0:1fcc61be1dcf 474 myGPS.read();
kityann 0:1fcc61be1dcf 475 //recive gps module
kityann 0:1fcc61be1dcf 476 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:1fcc61be1dcf 477 if ( myGPS.newNMEAreceived() ) {
kityann 0:1fcc61be1dcf 478 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:1fcc61be1dcf 479 continue;
kityann 0:1fcc61be1dcf 480 }
kityann 0:1fcc61be1dcf 481 else{
kityann 0:1fcc61be1dcf 482 count++;
kityann 0:1fcc61be1dcf 483 }
kityann 0:1fcc61be1dcf 484 }
kityann 0:1fcc61be1dcf 485 //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
kityann 0:1fcc61be1dcf 486 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:1fcc61be1dcf 487 refresh_Timer.reset();
kityann 0:1fcc61be1dcf 488 //print_gps(count);
kityann 0:1fcc61be1dcf 489 Get_GPS(&myGPS);
s1200058 7:db6b436c0baa 490 log++;
s1200058 11:19091694455e 491 pc.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
s1200058 11:19091694455e 492 pc.printf("moter command: %c\n", cansat.motor_command);
s1200058 11:19091694455e 493 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 494 }
kityann 0:1fcc61be1dcf 495 }
kityann 0:1fcc61be1dcf 496
kityann 0:1fcc61be1dcf 497 }