pyonta cansat / Mbed 2 deprecated FM_integration_copy

test

Dependencies:   HMC5883L MPL3115 MPU6050 MU2Class SDFileSystem mbed

main.cpp@0:1f956fab4d28, 2017-09-06 (annotated)

Committer:
pyonta2017
Date:
Wed Sep 06 15:13:08 2017 +0000
Revision:
0:1f956fab4d28
?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pyonta2017 0:1f956fab4d28 1 #include "mbed.h"
pyonta2017 0:1f956fab4d28 2 #include "xprintf.h"
pyonta2017 0:1f956fab4d28 3 #include "MPL3115A2.h"
pyonta2017 0:1f956fab4d28 4 #include "HMC5883L.h"
pyonta2017 0:1f956fab4d28 5 #include "MPU6050.h"
pyonta2017 0:1f956fab4d28 6 #include "SDFileSystem.h"
pyonta2017 0:1f956fab4d28 7 #include "EthernetPowerControl.h"
pyonta2017 0:1f956fab4d28 8 #define M_PI 3.141592
pyonta2017 0:1f956fab4d28 9
pyonta2017 0:1f956fab4d28 10 //hmc calibration mode
pyonta2017 0:1f956fab4d28 11 #define STOP 0 //initial
pyonta2017 0:1f956fab4d28 12 #define CAL 1 //calibration
pyonta2017 0:1f956fab4d28 13 #define RUN 2 //run
pyonta2017 0:1f956fab4d28 14
pyonta2017 0:1f956fab4d28 15 //Sequence phase
pyonta2017 0:1f956fab4d28 16 #define Preparing 1
pyonta2017 0:1f956fab4d28 17 #define Rising 2
pyonta2017 0:1f956fab4d28 18 #define Descending 3
pyonta2017 0:1f956fab4d28 19 #define Landing_Fusing 4
pyonta2017 0:1f956fab4d28 20 #define Moving 5
pyonta2017 0:1f956fab4d28 21
pyonta2017 0:1f956fab4d28 22 //Moving step
pyonta2017 0:1f956fab4d28 23 #define get_goaldirection 0
pyonta2017 0:1f956fab4d28 24 #define attitude_determination 1
pyonta2017 0:1f956fab4d28 25 #define calculate_direction 2
pyonta2017 0:1f956fab4d28 26 #define direction_control 3
pyonta2017 0:1f956fab4d28 27 #define jump 4
pyonta2017 0:1f956fab4d28 28
pyonta2017 0:1f956fab4d28 29 DigitalOut myled1(LED1);
pyonta2017 0:1f956fab4d28 30 DigitalOut myled2(LED2);
pyonta2017 0:1f956fab4d28 31 DigitalOut myled3(LED3);
pyonta2017 0:1f956fab4d28 32 DigitalOut myled4(LED4);
pyonta2017 0:1f956fab4d28 33
pyonta2017 0:1f956fab4d28 34 //ピンアサイン確認
pyonta2017 0:1f956fab4d28 35 Serial gps(p13, p14);
pyonta2017 0:1f956fab4d28 36 Serial Mu2(p28, p27);
pyonta2017 0:1f956fab4d28 37 I2C i2c(p9, p10); // sda, scl
pyonta2017 0:1f956fab4d28 38 Serial mp(USBTX, USBRX);
pyonta2017 0:1f956fab4d28 39 SDFileSystem sd(p5, p6, p7, p8, "sd");
pyonta2017 0:1f956fab4d28 40 LocalFileSystem local("local");
pyonta2017 0:1f956fab4d28 41 DigitalOut controlmotor1(p22),controlmotor2(p23);//方向制御モーター
pyonta2017 0:1f956fab4d28 42 PwmOut pin21(p21);
pyonta2017 0:1f956fab4d28 43 DigitalOut jumpmotor(p24);
pyonta2017 0:1f956fab4d28 44
pyonta2017 0:1f956fab4d28 45 //溶断
pyonta2017 0:1f956fab4d28 46 DigitalOut para(p18);
pyonta2017 0:1f956fab4d28 47 DigitalOut marker(p19);
pyonta2017 0:1f956fab4d28 48 DigitalOut jumparm(p20);
pyonta2017 0:1f956fab4d28 49 //
pyonta2017 0:1f956fab4d28 50 DigitalOut Flightpin(p26);
pyonta2017 0:1f956fab4d28 51
pyonta2017 0:1f956fab4d28 52 Timer timer;
pyonta2017 0:1f956fab4d28 53 int val;
pyonta2017 0:1f956fab4d28 54 unsigned char phase;
pyonta2017 0:1f956fab4d28 55 unsigned char step;
pyonta2017 0:1f956fab4d28 56
pyonta2017 0:1f956fab4d28 57 //気圧///////
pyonta2017 0:1f956fab4d28 58 MPL3115A2 APsensor(&i2c, &mp);
pyonta2017 0:1f956fab4d28 59 int alt;
pyonta2017 0:1f956fab4d28 60 int ini_alt;
pyonta2017 0:1f956fab4d28 61 int min_alt;
pyonta2017 0:1f956fab4d28 62 int min_alt5;
pyonta2017 0:1f956fab4d28 63 int max_alt;
pyonta2017 0:1f956fab4d28 64 int rela_max;
pyonta2017 0:1f956fab4d28 65 int rela_ini;
pyonta2017 0:1f956fab4d28 66 int flag1;
pyonta2017 0:1f956fab4d28 67 int flag2;
pyonta2017 0:1f956fab4d28 68 int flag3;
pyonta2017 0:1f956fab4d28 69 unsigned char alttimes;
pyonta2017 0:1f956fab4d28 70 void getAltitude(){
pyonta2017 0:1f956fab4d28 71 //wait_ms(300);
pyonta2017 0:1f956fab4d28 72 Altitude a;
pyonta2017 0:1f956fab4d28 73 Temperature t;
pyonta2017 0:1f956fab4d28 74 Pressure p;
pyonta2017 0:1f956fab4d28 75 APsensor.readAltitude(&a);
pyonta2017 0:1f956fab4d28 76 APsensor.readTemperature(&t);
pyonta2017 0:1f956fab4d28 77 APsensor.setModeStandby();
pyonta2017 0:1f956fab4d28 78 APsensor.setModeBarometer();
pyonta2017 0:1f956fab4d28 79 APsensor.setModeActive();
pyonta2017 0:1f956fab4d28 80 APsensor.readPressure(&p);
pyonta2017 0:1f956fab4d28 81 alt = a;
pyonta2017 0:1f956fab4d28 82 //気球試験用
pyonta2017 0:1f956fab4d28 83 if(phase == Preparing){
pyonta2017 0:1f956fab4d28 84 rela_ini = alt - ini_alt;
pyonta2017 0:1f956fab4d28 85 if(rela_ini > 7)flag1 = flag1+1;
pyonta2017 0:1f956fab4d28 86 }
pyonta2017 0:1f956fab4d28 87
pyonta2017 0:1f956fab4d28 88 if(phase == Rising){
pyonta2017 0:1f956fab4d28 89 if(max_alt < alt){
pyonta2017 0:1f956fab4d28 90 max_alt = alt;
pyonta2017 0:1f956fab4d28 91 }
pyonta2017 0:1f956fab4d28 92 else{
pyonta2017 0:1f956fab4d28 93 rela_max = max_alt -alt;
pyonta2017 0:1f956fab4d28 94 if(rela_max > 7)flag2 = flag2+1;
pyonta2017 0:1f956fab4d28 95 }
pyonta2017 0:1f956fab4d28 96 }
pyonta2017 0:1f956fab4d28 97 //着地判定
pyonta2017 0:1f956fab4d28 98 if(phase == Descending){
pyonta2017 0:1f956fab4d28 99 if (min_alt > alt){
pyonta2017 0:1f956fab4d28 100 min_alt = alt;
pyonta2017 0:1f956fab4d28 101 min_alt5 = min_alt + 5;
pyonta2017 0:1f956fab4d28 102 flag3 -= 1;
pyonta2017 0:1f956fab4d28 103 }
pyonta2017 0:1f956fab4d28 104 else {
pyonta2017 0:1f956fab4d28 105 if( alt < min_alt5){
pyonta2017 0:1f956fab4d28 106 flag3 +=2;
pyonta2017 0:1f956fab4d28 107 }
pyonta2017 0:1f956fab4d28 108 }
pyonta2017 0:1f956fab4d28 109 }
pyonta2017 0:1f956fab4d28 110 mp.printf("alt:%d,max:%d,ini:%d,phase:%d\r\n",alt,max_alt,ini_alt,phase);
pyonta2017 0:1f956fab4d28 111 mp.printf("rela_ini:%d,rela_max:%d\r\n",rela_ini,rela_max);
pyonta2017 0:1f956fab4d28 112 mp.printf("flag1:%d,flag2:%d,flag3:%d\r\n",flag1,flag2,flag3);
pyonta2017 0:1f956fab4d28 113 //mp.printf("OFF_H: 0x%X, OFF_T: 0x%X, OFF_P: 0x%X\r\n", APsensor.offsetAltitude(), APsensor.offsetTemperature(), APsensor.offsetPressure());
pyonta2017 0:1f956fab4d28 114 APsensor.setModeStandby();
pyonta2017 0:1f956fab4d28 115 APsensor.setModeAltimeter();
pyonta2017 0:1f956fab4d28 116 APsensor.setModeActive();
pyonta2017 0:1f956fab4d28 117 }
pyonta2017 0:1f956fab4d28 118 /////
pyonta2017 0:1f956fab4d28 119
pyonta2017 0:1f956fab4d28 120 //GPS
pyonta2017 0:1f956fab4d28 121 int i,rlock,mode;
pyonta2017 0:1f956fab4d28 122 char gps_data[256];
pyonta2017 0:1f956fab4d28 123 char ns,ew;
pyonta2017 0:1f956fab4d28 124 float w_time,hokui,tokei;
pyonta2017 0:1f956fab4d28 125 float g_hokui,g_tokei;
pyonta2017 0:1f956fab4d28 126 float d_hokui,m_hokui,d_tokei,m_tokei;
pyonta2017 0:1f956fab4d28 127 unsigned char c;
pyonta2017 0:1f956fab4d28 128 //目標経度，緯度
pyonta2017 0:1f956fab4d28 129 float goal_tokei,goal_hokui;
pyonta2017 0:1f956fab4d28 130 float goal_direction;
pyonta2017 0:1f956fab4d28 131 float direction_y;
pyonta2017 0:1f956fab4d28 132 float direction_x;
pyonta2017 0:1f956fab4d28 133 float angular_difference;
pyonta2017 0:1f956fab4d28 134
pyonta2017 0:1f956fab4d28 135 void getGPS(){
pyonta2017 0:1f956fab4d28 136 //目的地 大岡山駅
pyonta2017 0:1f956fab4d28 137 goal_tokei = 139.686234;
pyonta2017 0:1f956fab4d28 138 goal_hokui = 35.614330;
pyonta2017 0:1f956fab4d28 139 c = gps.getc();
pyonta2017 0:1f956fab4d28 140 if( c=='$' || i == 256){
pyonta2017 0:1f956fab4d28 141 mode = 0;
pyonta2017 0:1f956fab4d28 142 i = 0;
pyonta2017 0:1f956fab4d28 143 for(int j=0; j<256; j++){
pyonta2017 0:1f956fab4d28 144 gps_data[j]=NULL;
pyonta2017 0:1f956fab4d28 145 }
pyonta2017 0:1f956fab4d28 146 }
pyonta2017 0:1f956fab4d28 147 if(mode==0){
pyonta2017 0:1f956fab4d28 148 if((gps_data[i]=c) != '\r'){
pyonta2017 0:1f956fab4d28 149 i++;
pyonta2017 0:1f956fab4d28 150 }else{
pyonta2017 0:1f956fab4d28 151 gps_data[i]='\0';
pyonta2017 0:1f956fab4d28 152
pyonta2017 0:1f956fab4d28 153 if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){
pyonta2017 0:1f956fab4d28 154 if(rlock==1){
pyonta2017 0:1f956fab4d28 155 //mp.printf("Status:Lock(%d)\n\r",rlock);
pyonta2017 0:1f956fab4d28 156 //logitude
pyonta2017 0:1f956fab4d28 157 d_tokei= int(tokei/100);
pyonta2017 0:1f956fab4d28 158 m_tokei= (tokei-d_tokei*100)/60;
pyonta2017 0:1f956fab4d28 159 g_tokei= d_tokei+m_tokei;
pyonta2017 0:1f956fab4d28 160 //Latitude
pyonta2017 0:1f956fab4d28 161 d_hokui=int(hokui/100);
pyonta2017 0:1f956fab4d28 162 m_hokui=(hokui-d_hokui*100)/60;
pyonta2017 0:1f956fab4d28 163 g_hokui=d_hokui+m_hokui;
pyonta2017 0:1f956fab4d28 164 mp.printf("Lon:%.6f, Lat:%.6f\n\r",g_tokei, g_hokui);
pyonta2017 0:1f956fab4d28 165 Mu2.printf("@DT14%f,%f\r\n",g_tokei, g_hokui);
pyonta2017 0:1f956fab4d28 166 float longitudinalDifference = goal_tokei - g_tokei;
pyonta2017 0:1f956fab4d28 167 float latitudinalDifference = goal_hokui - g_hokui;
pyonta2017 0:1f956fab4d28 168
pyonta2017 0:1f956fab4d28 169 //球面三角法
pyonta2017 0:1f956fab4d28 170 direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180);
pyonta2017 0:1f956fab4d28 171 direction_x = cos(g_hokui*M_PI/180)*sin(goal_hokui*M_PI/180) - sin(g_hokui*M_PI/180)*cos(goal_hokui*M_PI/180)*cos(longitudinalDifference*M_PI/180);
pyonta2017 0:1f956fab4d28 172 goal_direction = atan2f(direction_y,direction_x);
pyonta2017 0:1f956fab4d28 173 if(goal_direction < 0)goal_direction += 2*M_PI;
pyonta2017 0:1f956fab4d28 174
pyonta2017 0:1f956fab4d28 175 //磁北に対する角度に調節 東京7度
pyonta2017 0:1f956fab4d28 176 goal_direction += 0.122173;
pyonta2017 0:1f956fab4d28 177 //goal_direction2 += 0.122173;
pyonta2017 0:1f956fab4d28 178 if (goal_direction < 0) goal_direction += 2*M_PI;
pyonta2017 0:1f956fab4d28 179 //mp.printf("goal:%f\n\r",goal_direction*180/M_PI);
pyonta2017 0:1f956fab4d28 180 //mp.printf("longitudinalDifference:%f",longitudinalDifference);
pyonta2017 0:1f956fab4d28 181 //mp.printf("latitudinalDifference:%f",latitudinalDifference);
pyonta2017 0:1f956fab4d28 182 if(phase == Descending)getAltitude();
pyonta2017 0:1f956fab4d28 183 if(phase == Moving)step = attitude_determination;
pyonta2017 0:1f956fab4d28 184 }
pyonta2017 0:1f956fab4d28 185 else{
pyonta2017 0:1f956fab4d28 186 //mp.printf("\n\rStatus:unLock(%d)\n\r",rlock);
pyonta2017 0:1f956fab4d28 187 //mp.printf("%s",gps_data);
pyonta2017 0:1f956fab4d28 188 Mu2.printf("@DT02No\r\n");
pyonta2017 0:1f956fab4d28 189 if(phase == Descending)getAltitude();
pyonta2017 0:1f956fab4d28 190 //if(phase == Moving)test_get_direction();
pyonta2017 0:1f956fab4d28 191 }
pyonta2017 0:1f956fab4d28 192 sprintf(gps_data, "");
pyonta2017 0:1f956fab4d28 193 }
pyonta2017 0:1f956fab4d28 194 }
pyonta2017 0:1f956fab4d28 195 }
pyonta2017 0:1f956fab4d28 196 }
pyonta2017 0:1f956fab4d28 197
pyonta2017 0:1f956fab4d28 198 void MU2initialize(){
pyonta2017 0:1f956fab4d28 199 Mu2.baud(19200);
pyonta2017 0:1f956fab4d28 200 Mu2.printf("@EI34\r\n");
pyonta2017 0:1f956fab4d28 201 wait(1);
pyonta2017 0:1f956fab4d28 202 Mu2.printf("@DI25\r\n");
pyonta2017 0:1f956fab4d28 203 wait(1);
pyonta2017 0:1f956fab4d28 204 Mu2.printf("@CH0F\r\n");
pyonta2017 0:1f956fab4d28 205 wait(1);
pyonta2017 0:1f956fab4d28 206 Mu2.printf("@GI34\r\n");
pyonta2017 0:1f956fab4d28 207 wait(1);
pyonta2017 0:1f956fab4d28 208 }
pyonta2017 0:1f956fab4d28 209
pyonta2017 0:1f956fab4d28 210 /////////////////////
pyonta2017 0:1f956fab4d28 211 //地磁気
pyonta2017 0:1f956fab4d28 212 HMC5883L compass(p9, p10);
pyonta2017 0:1f956fab4d28 213 Ticker interrupt;
pyonta2017 0:1f956fab4d28 214 double heading0 = 0.0;
pyonta2017 0:1f956fab4d28 215 double heading1 = 0.0;
pyonta2017 0:1f956fab4d28 216 double heading2 = 0.0;
pyonta2017 0:1f956fab4d28 217 double heading3 = 0.0;
pyonta2017 0:1f956fab4d28 218 double headingLPF = 0.0;
pyonta2017 0:1f956fab4d28 219 double initHeading;
pyonta2017 0:1f956fab4d28 220 double tgtHeading;
pyonta2017 0:1f956fab4d28 221 double preHeading = 0.0;
pyonta2017 0:1f956fab4d28 222 unsigned char hmc_mode;
pyonta2017 0:1f956fab4d28 223
pyonta2017 0:1f956fab4d28 224 int maxX, minX, maxY, minY;
pyonta2017 0:1f956fab4d28 225 int ofsX = 0; //calibration x
pyonta2017 0:1f956fab4d28 226 int ofsY = 0; //calibration y
pyonta2017 0:1f956fab4d28 227 int counter = 0;
pyonta2017 0:1f956fab4d28 228 int ofsXset = 1328;
pyonta2017 0:1f956fab4d28 229 int ofsYset = -554;
pyonta2017 0:1f956fab4d28 230 int ofsZset = -750;
pyonta2017 0:1f956fab4d28 231
pyonta2017 0:1f956fab4d28 232 //地磁気キャリブレーション用
pyonta2017 0:1f956fab4d28 233 void intrpt() {
pyonta2017 0:1f956fab4d28 234 int16_t raw[3];
pyonta2017 0:1f956fab4d28 235 compass.getXYZ(raw);
pyonta2017 0:1f956fab4d28 236 //double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2]
pyonta2017 0:1f956fab4d28 237 double heading = atan2(static_cast<double>(raw[2]-ofsYset), static_cast<double>(raw[0]-ofsXset));
pyonta2017 0:1f956fab4d28 238 if(heading < 0)heading += 2*M_PI;
pyonta2017 0:1f956fab4d28 239 if(heading > 2*M_PI)heading -= 2*M_PI;
pyonta2017 0:1f956fab4d28 240 heading3 = heading2;
pyonta2017 0:1f956fab4d28 241 heading2 = heading1;
pyonta2017 0:1f956fab4d28 242 heading1 = heading0;
pyonta2017 0:1f956fab4d28 243 heading0 = heading;
pyonta2017 0:1f956fab4d28 244 headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter
pyonta2017 0:1f956fab4d28 245
pyonta2017 0:1f956fab4d28 246 switch(hmc_mode) {
pyonta2017 0:1f956fab4d28 247 case STOP:
pyonta2017 0:1f956fab4d28 248 if(counter == 100) {
pyonta2017 0:1f956fab4d28 249 initHeading = headingLPF;
pyonta2017 0:1f956fab4d28 250 hmc_mode = CAL;
pyonta2017 0:1f956fab4d28 251 maxX = raw[0];
pyonta2017 0:1f956fab4d28 252 minX = raw[0];
pyonta2017 0:1f956fab4d28 253 maxY = raw[2];
pyonta2017 0:1f956fab4d28 254 minY = raw[2];
pyonta2017 0:1f956fab4d28 255 counter = 0;
pyonta2017 0:1f956fab4d28 256 //mp.printf("STOP end\n\r");
pyonta2017 0:1f956fab4d28 257 }
pyonta2017 0:1f956fab4d28 258 break;
pyonta2017 0:1f956fab4d28 259 case CAL:
pyonta2017 0:1f956fab4d28 260 if(raw[0] > maxX)maxX = raw[0];
pyonta2017 0:1f956fab4d28 261 if(raw[0] < minX)minX = raw[0];
pyonta2017 0:1f956fab4d28 262 if(raw[2] > maxY)maxY = raw[2];
pyonta2017 0:1f956fab4d28 263 if(raw[2] < minY)minY = raw[2];
pyonta2017 0:1f956fab4d28 264 if((counter > 100)
pyonta2017 0:1f956fab4d28 265 && (headingLPF > (initHeading-0.01))
pyonta2017 0:1f956fab4d28 266 && (headingLPF < (initHeading+0.01))) {
pyonta2017 0:1f956fab4d28 267 ofsX = (maxX + minX)/2;
pyonta2017 0:1f956fab4d28 268 ofsY = (maxY + minY)/2;
pyonta2017 0:1f956fab4d28 269 //hmc_mode = RUN;
pyonta2017 0:1f956fab4d28 270 counter = 0;
pyonta2017 0:1f956fab4d28 271 //mp.printf("ofs=%d,%d\n\r",ofsX,ofsY);
pyonta2017 0:1f956fab4d28 272 }
pyonta2017 0:1f956fab4d28 273 break;
pyonta2017 0:1f956fab4d28 274 case RUN:
pyonta2017 0:1f956fab4d28 275 mp.printf("heading=%f,x=%d,y=%d\n\r",headingLPF*180/M_PI,raw[0]-ofsXset,raw[2]-ofsYset);
pyonta2017 0:1f956fab4d28 276 //mp.printf("%d,%d,%d\r\n",raw[0],raw[2],raw[1]);
pyonta2017 0:1f956fab4d28 277 //step = calculate_direction;
pyonta2017 0:1f956fab4d28 278 break;
pyonta2017 0:1f956fab4d28 279
pyonta2017 0:1f956fab4d28 280 }
pyonta2017 0:1f956fab4d28 281 counter++;
pyonta2017 0:1f956fab4d28 282 }
pyonta2017 0:1f956fab4d28 283
pyonta2017 0:1f956fab4d28 284 //加速度
pyonta2017 0:1f956fab4d28 285 MPU6050 mpu;
pyonta2017 0:1f956fab4d28 286 int16_t ax, ay, az;
pyonta2017 0:1f956fab4d28 287 int16_t gx, gy, gz;
pyonta2017 0:1f956fab4d28 288 int32_t axa, aya, aza;
pyonta2017 0:1f956fab4d28 289 int32_t gxa, gya, gza;
pyonta2017 0:1f956fab4d28 290
pyonta2017 0:1f956fab4d28 291 //オフセット値
pyonta2017 0:1f956fab4d28 292 double axoffset = 586.711;
pyonta2017 0:1f956fab4d28 293 double ayoffset = -174.847;
pyonta2017 0:1f956fab4d28 294 double azoffset = -2195.34375;
pyonta2017 0:1f956fab4d28 295
pyonta2017 0:1f956fab4d28 296 //センサの出力あたりの加速度[m/s^2]
pyonta2017 0:1f956fab4d28 297 double axrate = 0.000601615;
pyonta2017 0:1f956fab4d28 298 double ayrate = 0.000598334;
pyonta2017 0:1f956fab4d28 299 double azrate = 0.0005845;
pyonta2017 0:1f956fab4d28 300
pyonta2017 0:1f956fab4d28 301 //3軸キャリブレーション
pyonta2017 0:1f956fab4d28 302 double phi;
pyonta2017 0:1f956fab4d28 303 double theta;
pyonta2017 0:1f956fab4d28 304 double heading3axis;
pyonta2017 0:1f956fab4d28 305
pyonta2017 0:1f956fab4d28 306 double bfy;
pyonta2017 0:1f956fab4d28 307 double bfx;
pyonta2017 0:1f956fab4d28 308
pyonta2017 0:1f956fab4d28 309 //3軸からheading算出
pyonta2017 0:1f956fab4d28 310 void getheading3axis() {
pyonta2017 0:1f956fab4d28 311 //加速度算出
pyonta2017 0:1f956fab4d28 312 mpu.getAcceleration(&ax, &ay, &az);
pyonta2017 0:1f956fab4d28 313 double axcal = -axrate * (static_cast<double>(ax) - axoffset);
pyonta2017 0:1f956fab4d28 314 double aycal = -ayrate * (static_cast<double>(ay) - ayoffset);
pyonta2017 0:1f956fab4d28 315 double azcal = -azrate * (static_cast<double>(az) - azoffset);
pyonta2017 0:1f956fab4d28 316
pyonta2017 0:1f956fab4d28 317 phi = atan2(aycal,azcal);
pyonta2017 0:1f956fab4d28 318 //phi += M_PI;
pyonta2017 0:1f956fab4d28 319 theta = atan2(-axcal,aycal*sin(phi)+azcal*cos(phi));
pyonta2017 0:1f956fab4d28 320 //theta += M_PI;
pyonta2017 0:1f956fab4d28 321
pyonta2017 0:1f956fab4d28 322 //地磁気算出
pyonta2017 0:1f956fab4d28 323 int16_t raw[3];
pyonta2017 0:1f956fab4d28 324 compass.getXYZ(raw);
pyonta2017 0:1f956fab4d28 325 bfy = -(static_cast<double>(raw[1]-ofsZset))*sin(phi) + (static_cast<double>(raw[2]-ofsYset))*cos(phi);
pyonta2017 0:1f956fab4d28 326 bfx = (static_cast<double>(raw[0]-ofsXset))*cos(theta)
pyonta2017 0:1f956fab4d28 327 + (static_cast<double>(raw[2]-ofsYset))*sin(theta)*sin(phi)
pyonta2017 0:1f956fab4d28 328 + (static_cast<double>(raw[1]-ofsZset))*sin(theta)*cos(phi);
pyonta2017 0:1f956fab4d28 329 double heading = atan2(-bfy, bfx);
pyonta2017 0:1f956fab4d28 330 if(heading < 0)heading += 2*M_PI;
pyonta2017 0:1f956fab4d28 331 if(heading > 2*M_PI)heading -= 2*M_PI;
pyonta2017 0:1f956fab4d28 332 heading3 = heading2;
pyonta2017 0:1f956fab4d28 333 heading2 = heading1;
pyonta2017 0:1f956fab4d28 334 heading1 = heading0;
pyonta2017 0:1f956fab4d28 335 heading0 = heading;
pyonta2017 0:1f956fab4d28 336 headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter
pyonta2017 0:1f956fab4d28 337 //headingLPF += 0.349066;
pyonta2017 0:1f956fab4d28 338 if(headingLPF > 2*M_PI) headingLPF -= 2*M_PI;
pyonta2017 0:1f956fab4d28 339 //mp.printf("ax:%f,ay:%f,az:%f\r\n",axcal,aycal,azcal);
pyonta2017 0:1f956fab4d28 340 mp.printf("heading:%f,phi:%f,theta:%f\r\n",headingLPF*180/M_PI,phi*180/M_PI,theta*180/M_PI);
pyonta2017 0:1f956fab4d28 341 }
pyonta2017 0:1f956fab4d28 342
pyonta2017 0:1f956fab4d28 343 //ADXL375
pyonta2017 0:1f956fab4d28 344 const int addr = 0xA6;
pyonta2017 0:1f956fab4d28 345 char adxl[6];
pyonta2017 0:1f956fab4d28 346 short int axout = 0;
pyonta2017 0:1f956fab4d28 347 short int ayout = 0;
pyonta2017 0:1f956fab4d28 348 short int azout = 0;
pyonta2017 0:1f956fab4d28 349
pyonta2017 0:1f956fab4d28 350 void adxl_init(){
pyonta2017 0:1f956fab4d28 351 char fifo[2] = {0x38,0x80};
pyonta2017 0:1f956fab4d28 352 i2c.write(addr,fifo,2); //FIFO_Mode -> Stream
pyonta2017 0:1f956fab4d28 353 char power[2] = {0x2D,0x08};
pyonta2017 0:1f956fab4d28 354 i2c.write(addr,power,2); //measurebit -> 1
pyonta2017 0:1f956fab4d28 355 }
pyonta2017 0:1f956fab4d28 356
pyonta2017 0:1f956fab4d28 357 void adxl_get(){
pyonta2017 0:1f956fab4d28 358 adxl[0] = 0x32;
pyonta2017 0:1f956fab4d28 359 i2c.write(addr,adxl,1);
pyonta2017 0:1f956fab4d28 360 i2c.read(0xA7,adxl,6);
pyonta2017 0:1f956fab4d28 361 axout = (((int16_t )adxl[1]) << 8) | adxl[0];
pyonta2017 0:1f956fab4d28 362 ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
pyonta2017 0:1f956fab4d28 363 azout = (((int16_t )adxl[5]) << 8) | adxl[4];
pyonta2017 0:1f956fab4d28 364 }
pyonta2017 0:1f956fab4d28 365
pyonta2017 0:1f956fab4d28 366
pyonta2017 0:1f956fab4d28 367 //モーター駆動方向制御
pyonta2017 0:1f956fab4d28 368 void motor_drive(){
pyonta2017 0:1f956fab4d28 369 controlmotor1.write(1);
pyonta2017 0:1f956fab4d28 370 controlmotor2.write(0);
pyonta2017 0:1f956fab4d28 371 pin21.write(0.8);
pyonta2017 0:1f956fab4d28 372 //if(pin1)mp.printf("Yes!\r\n");
pyonta2017 0:1f956fab4d28 373 }
pyonta2017 0:1f956fab4d28 374
pyonta2017 0:1f956fab4d28 375 //溶断プログラム
pyonta2017 0:1f956fab4d28 376 void burning(){
pyonta2017 0:1f956fab4d28 377 wait(5);
pyonta2017 0:1f956fab4d28 378 myled1 = 1;
pyonta2017 0:1f956fab4d28 379 //para = 1;
pyonta2017 0:1f956fab4d28 380 wait(1);
pyonta2017 0:1f956fab4d28 381 myled1 = 0;
pyonta2017 0:1f956fab4d28 382 //para = 0;
pyonta2017 0:1f956fab4d28 383 wait(5);
pyonta2017 0:1f956fab4d28 384 myled2 = 1;
pyonta2017 0:1f956fab4d28 385 //marker = 1;
pyonta2017 0:1f956fab4d28 386 wait(1);
pyonta2017 0:1f956fab4d28 387 myled2 = 0;
pyonta2017 0:1f956fab4d28 388 //marker = 0;
pyonta2017 0:1f956fab4d28 389 wait(5);
pyonta2017 0:1f956fab4d28 390 myled3 = 1;
pyonta2017 0:1f956fab4d28 391 //jumparm = 1;
pyonta2017 0:1f956fab4d28 392 wait(1);
pyonta2017 0:1f956fab4d28 393 myled3 = 0;
pyonta2017 0:1f956fab4d28 394 //jumparm = 0;
pyonta2017 0:1f956fab4d28 395 }
pyonta2017 0:1f956fab4d28 396 //跳躍モーター駆動まいまいかわいい
pyonta2017 0:1f956fab4d28 397 void jumping(){
pyonta2017 0:1f956fab4d28 398 timer.start();
pyonta2017 0:1f956fab4d28 399 //モーター駆動時間調整
pyonta2017 0:1f956fab4d28 400 while(val < 4){
pyonta2017 0:1f956fab4d28 401 val = timer.read();
pyonta2017 0:1f956fab4d28 402 jumpmotor = 1;
pyonta2017 0:1f956fab4d28 403 myled4 != myled4;
pyonta2017 0:1f956fab4d28 404 }
pyonta2017 0:1f956fab4d28 405 timer.reset();
pyonta2017 0:1f956fab4d28 406 timer.stop();
pyonta2017 0:1f956fab4d28 407 jumpmotor = 0;
pyonta2017 0:1f956fab4d28 408 val = 0;
pyonta2017 0:1f956fab4d28 409 }
pyonta2017 0:1f956fab4d28 410
pyonta2017 0:1f956fab4d28 411 ///test
pyonta2017 0:1f956fab4d28 412 //GPSなし方向制御，跳躍テスト
pyonta2017 0:1f956fab4d28 413 float test_tokei,test_hokui;
pyonta2017 0:1f956fab4d28 414 void test_get_direction(){
pyonta2017 0:1f956fab4d28 415 //目的地 大岡山駅
pyonta2017 0:1f956fab4d28 416 goal_tokei = 139.686234;
pyonta2017 0:1f956fab4d28 417 goal_hokui = 35.614330;
pyonta2017 0:1f956fab4d28 418
pyonta2017 0:1f956fab4d28 419 //テスト場所石川台一号館
pyonta2017 0:1f956fab4d28 420 test_tokei = 139.681529;
pyonta2017 0:1f956fab4d28 421 test_hokui = 35.605263;
pyonta2017 0:1f956fab4d28 422
pyonta2017 0:1f956fab4d28 423 float longitudinalDifference = goal_tokei - test_tokei;
pyonta2017 0:1f956fab4d28 424 float latitudinalDifference = goal_hokui - test_hokui;
pyonta2017 0:1f956fab4d28 425 //球面三角法
pyonta2017 0:1f956fab4d28 426 direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180);
pyonta2017 0:1f956fab4d28 427 direction_x = cos(test_hokui*M_PI/180)*sin(goal_hokui*M_PI/180) - sin(test_hokui*M_PI/180)*cos(goal_hokui*M_PI/180)*cos(longitudinalDifference*M_PI/180);
pyonta2017 0:1f956fab4d28 428 goal_direction = atan2f(direction_y,direction_x);
pyonta2017 0:1f956fab4d28 429 if(goal_direction < 0)goal_direction += 2*M_PI;
pyonta2017 0:1f956fab4d28 430
pyonta2017 0:1f956fab4d28 431 //磁北に対する角度に調節 東京7度
pyonta2017 0:1f956fab4d28 432 goal_direction += 0.122173;
pyonta2017 0:1f956fab4d28 433 //goal_direction2 += 0.122173;
pyonta2017 0:1f956fab4d28 434 if (goal_direction < 0) goal_direction += 2*M_PI;
pyonta2017 0:1f956fab4d28 435 }
pyonta2017 0:1f956fab4d28 436
pyonta2017 0:1f956fab4d28 437 /////////////////////////main////////////////////////////////////
pyonta2017 0:1f956fab4d28 438 int main(){
pyonta2017 0:1f956fab4d28 439 PHY_PowerDown();//省電力
pyonta2017 0:1f956fab4d28 440 wait(2);
pyonta2017 0:1f956fab4d28 441 //センサ初期化
pyonta2017 0:1f956fab4d28 442 Mu2.baud(19200);
pyonta2017 0:1f956fab4d28 443 Mu2.printf("@EI34\r\n");
pyonta2017 0:1f956fab4d28 444 wait(1);
pyonta2017 0:1f956fab4d28 445 Mu2.printf("@DI25\r\n");
pyonta2017 0:1f956fab4d28 446 wait(1);
pyonta2017 0:1f956fab4d28 447 Mu2.printf("@CH0F\r\n");
pyonta2017 0:1f956fab4d28 448 wait(1);
pyonta2017 0:1f956fab4d28 449 Mu2.printf("@GI34\r\n");
pyonta2017 0:1f956fab4d28 450 wait(1);
pyonta2017 0:1f956fab4d28 451 APsensor.init();
pyonta2017 0:1f956fab4d28 452 wait(0.2);
pyonta2017 0:1f956fab4d28 453 hmc_mode = RUN;
pyonta2017 0:1f956fab4d28 454 mpu.initialize();
pyonta2017 0:1f956fab4d28 455 wait(0.2);
pyonta2017 0:1f956fab4d28 456 compass.init();
pyonta2017 0:1f956fab4d28 457 wait(0.2);
pyonta2017 0:1f956fab4d28 458 adxl_init();
pyonta2017 0:1f956fab4d28 459 wait(0.2);
pyonta2017 0:1f956fab4d28 460 APsensor.setOffsetAltitude(100);
pyonta2017 0:1f956fab4d28 461 APsensor.setOffsetTemperature(10);
pyonta2017 0:1f956fab4d28 462 APsensor.setOffsetPressure(-32);
pyonta2017 0:1f956fab4d28 463 mkdir("/sd/mydir", 0777);
pyonta2017 0:1f956fab4d28 464 min_alt = 4000.0;
pyonta2017 0:1f956fab4d28 465 Flightpin = 1;
pyonta2017 0:1f956fab4d28 466 //高度初期値
pyonta2017 0:1f956fab4d28 467 for(int h = 0; h < 10; h++){
pyonta2017 0:1f956fab4d28 468 getAltitude();
pyonta2017 0:1f956fab4d28 469 ini_alt = ini_alt + alt;
pyonta2017 0:1f956fab4d28 470 wait(0.5);
pyonta2017 0:1f956fab4d28 471 }
pyonta2017 0:1f956fab4d28 472 ini_alt = ini_alt/10;
pyonta2017 0:1f956fab4d28 473 mp.printf("ini_alt:%d\r\n",ini_alt);
pyonta2017 0:1f956fab4d28 474 int jump_count;
pyonta2017 0:1f956fab4d28 475 phase = Preparing;
pyonta2017 0:1f956fab4d28 476 //phase = Moving;
pyonta2017 0:1f956fab4d28 477 /////////////////////////////単機能////////
pyonta2017 0:1f956fab4d28 478
pyonta2017 0:1f956fab4d28 479 //wait(5);
pyonta2017 0:1f956fab4d28 480
pyonta2017 0:1f956fab4d28 481 //while(1){
pyonta2017 0:1f956fab4d28 482 //myled1 = 1;
pyonta2017 0:1f956fab4d28 483 //Flightpin = 1;
pyonta2017 0:1f956fab4d28 484 //GPS_MU2
pyonta2017 0:1f956fab4d28 485 //getGPS();
pyonta2017 0:1f956fab4d28 486 //方向制御用モーター
pyonta2017 0:1f956fab4d28 487 /*
pyonta2017 0:1f956fab4d28 488 controlmotor1.write(1);
pyonta2017 0:1f956fab4d28 489 controlmotor2.write(0);
pyonta2017 0:1f956fab4d28 490 pin21.write(0.8);
pyonta2017 0:1f956fab4d28 491 wait(3);
pyonta2017 0:1f956fab4d28 492 controlmotor1.write(0);
pyonta2017 0:1f956fab4d28 493 controlmotor2.write(0);
pyonta2017 0:1f956fab4d28 494 pin21.write(0.0);
pyonta2017 0:1f956fab4d28 495 */
pyonta2017 0:1f956fab4d28 496 //跳躍用モーター
pyonta2017 0:1f956fab4d28 497 //jumpmotor = 1;
pyonta2017 0:1f956fab4d28 498 //wait(5);
pyonta2017 0:1f956fab4d28 499 //jumpmotor = 0;
pyonta2017 0:1f956fab4d28 500 //溶断
pyonta2017 0:1f956fab4d28 501 //burning();
pyonta2017 0:1f956fab4d28 502 //気圧
pyonta2017 0:1f956fab4d28 503 //getAltitude();
pyonta2017 0:1f956fab4d28 504 //mpu
pyonta2017 0:1f956fab4d28 505 /*
pyonta2017 0:1f956fab4d28 506 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
pyonta2017 0:1f956fab4d28 507 double axcal = -axrate * (static_cast<double>(ax) - axoffset);
pyonta2017 0:1f956fab4d28 508 double aycal = -ayrate * (static_cast<double>(ay) - ayoffset);
pyonta2017 0:1f956fab4d28 509 double azcal = -azrate * (static_cast<double>(az) - azoffset);
pyonta2017 0:1f956fab4d28 510 mp.printf("x:%f,y:%f,z:%f",axcal,aycal,azcal);
pyonta2017 0:1f956fab4d28 511 */
pyonta2017 0:1f956fab4d28 512 //hmc
pyonta2017 0:1f956fab4d28 513 /*
pyonta2017 0:1f956fab4d28 514 hmc_mode = RUN;
pyonta2017 0:1f956fab4d28 515 intrpt();
pyonta2017 0:1f956fab4d28 516 wait(0.5);
pyonta2017 0:1f956fab4d28 517 */
pyonta2017 0:1f956fab4d28 518 //hmc_mpu
pyonta2017 0:1f956fab4d28 519 //getheading3axis() ;
pyonta2017 0:1f956fab4d28 520 //wait(0.3);
pyonta2017 0:1f956fab4d28 521 //adxl_SDカード 跳躍高さ測定
pyonta2017 0:1f956fab4d28 522 /*
pyonta2017 0:1f956fab4d28 523 mkdir("/sd/mydir", 0777);
pyonta2017 0:1f956fab4d28 524 wait(1);
pyonta2017 0:1f956fab4d28 525 FILE *fp = fopen("/sd/mydir/height1.txt", "a");
pyonta2017 0:1f956fab4d28 526 wait(1);
pyonta2017 0:1f956fab4d28 527 if(fp == NULL) {
pyonta2017 0:1f956fab4d28 528 error("Could not open file for write\n");
pyonta2017 0:1f956fab4d28 529 }
pyonta2017 0:1f956fab4d28 530 timer.reset();
pyonta2017 0:1f956fab4d28 531 timer.start();
pyonta2017 0:1f956fab4d28 532 val = 0;
pyonta2017 0:1f956fab4d28 533 while(val < 5000){
pyonta2017 0:1f956fab4d28 534 jumpmotor = 1;
pyonta2017 0:1f956fab4d28 535 val = timer.read_ms();
pyonta2017 0:1f956fab4d28 536 adxl[0] = 0x32;
pyonta2017 0:1f956fab4d28 537 i2c.write(addr,adxl,1);
pyonta2017 0:1f956fab4d28 538 i2c.read(0xA7,adxl,6);
pyonta2017 0:1f956fab4d28 539 axout = (((int16_t )adxl[1]) << 8) | adxl[0];
pyonta2017 0:1f956fab4d28 540 ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
pyonta2017 0:1f956fab4d28 541 azout = (((int16_t )adxl[5]) << 8) | adxl[4];
pyonta2017 0:1f956fab4d28 542 fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
pyonta2017 0:1f956fab4d28 543 wait_ms(2);
pyonta2017 0:1f956fab4d28 544 myled1 !=myled1;
pyonta2017 0:1f956fab4d28 545 }
pyonta2017 0:1f956fab4d28 546 myled1 = 0;
pyonta2017 0:1f956fab4d28 547 jumpmotor = 0;
pyonta2017 0:1f956fab4d28 548 wait(1);
pyonta2017 0:1f956fab4d28 549 fclose(fp);
pyonta2017 0:1f956fab4d28 550 */
pyonta2017 0:1f956fab4d28 551 //
pyonta2017 0:1f956fab4d28 552
pyonta2017 0:1f956fab4d28 553 //}
pyonta2017 0:1f956fab4d28 554 /////////////////////////
pyonta2017 0:1f956fab4d28 555
pyonta2017 0:1f956fab4d28 556
pyonta2017 0:1f956fab4d28 557 /*
pyonta2017 0:1f956fab4d28 558 //////////////////////////安全試験用/////////
pyonta2017 0:1f956fab4d28 559 //振動試験
pyonta2017 0:1f956fab4d28 560 myled1 = 0;
pyonta2017 0:1f956fab4d28 561 //wait(5);//test
pyonta2017 0:1f956fab4d28 562 wait(900);//end to end
pyonta2017 0:1f956fab4d28 563 //静荷重
pyonta2017 0:1f956fab4d28 564 FILE *sl = fopen("/local/static2.txt", "a");
pyonta2017 0:1f956fab4d28 565 wait(10);
pyonta2017 0:1f956fab4d28 566 myled1 = 1;
pyonta2017 0:1f956fab4d28 567 wait(5);
pyonta2017 0:1f956fab4d28 568 timer.reset();
pyonta2017 0:1f956fab4d28 569 timer.start();
pyonta2017 0:1f956fab4d28 570 //while(val < 30000){ //test
pyonta2017 0:1f956fab4d28 571 while(val < 600000){ //end to end
pyonta2017 0:1f956fab4d28 572 val = timer.read_ms();
pyonta2017 0:1f956fab4d28 573 adxl[0] = 0x32;
pyonta2017 0:1f956fab4d28 574 i2c.write(addr,adxl,1);
pyonta2017 0:1f956fab4d28 575 i2c.read(0xA7,adxl,6);
pyonta2017 0:1f956fab4d28 576 axout = (((int16_t )adxl[1]) << 8) | adxl[0];
pyonta2017 0:1f956fab4d28 577 ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
pyonta2017 0:1f956fab4d28 578 azout = (((int16_t )adxl[5]) << 8) | adxl[4];
pyonta2017 0:1f956fab4d28 579 //pc.printf("time:%d,X:%d,Y:%d,Z:%d\r\n",val,axout,ayout,azout);
pyonta2017 0:1f956fab4d28 580 fprintf(sl, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
pyonta2017 0:1f956fab4d28 581 wait_ms(200);
pyonta2017 0:1f956fab4d28 582 //wait必要、ＳＤがbusyの状態で送っちゃってるんじゃない？
pyonta2017 0:1f956fab4d28 583 }
pyonta2017 0:1f956fab4d28 584 fclose(sl);
pyonta2017 0:1f956fab4d28 585 timer.reset();
pyonta2017 0:1f956fab4d28 586 timer.stop();
pyonta2017 0:1f956fab4d28 587 val = 0;
pyonta2017 0:1f956fab4d28 588 myled1 = 0;
pyonta2017 0:1f956fab4d28 589 //wait(5); //test
pyonta2017 0:1f956fab4d28 590 wait(960); //end to end
pyonta2017 0:1f956fab4d28 591
pyonta2017 0:1f956fab4d28 592 //開傘衝撃
pyonta2017 0:1f956fab4d28 593 FILE *im = fopen("/local/impact2.txt", "a");
pyonta2017 0:1f956fab4d28 594 wait(10);
pyonta2017 0:1f956fab4d28 595 myled2 = 1;
pyonta2017 0:1f956fab4d28 596 timer.reset();
pyonta2017 0:1f956fab4d28 597 timer.start();
pyonta2017 0:1f956fab4d28 598 //while(val < 10000){ //test
pyonta2017 0:1f956fab4d28 599 while(val < 30000){ //end to end
pyonta2017 0:1f956fab4d28 600 val = timer.read_ms();
pyonta2017 0:1f956fab4d28 601 adxl[0] = 0x32;
pyonta2017 0:1f956fab4d28 602 i2c.write(addr,adxl,1);
pyonta2017 0:1f956fab4d28 603 i2c.read(0xA7,adxl,6);
pyonta2017 0:1f956fab4d28 604 axout = (((int16_t )adxl[1]) << 8) | adxl[0];
pyonta2017 0:1f956fab4d28 605 ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
pyonta2017 0:1f956fab4d28 606 azout = (((int16_t )adxl[5]) << 8) | adxl[4];
pyonta2017 0:1f956fab4d28 607 fprintf(im, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
pyonta2017 0:1f956fab4d28 608 wait_ms(3);
pyonta2017 0:1f956fab4d28 609 //wait必要、ＳＤがbusyの状態で送っちゃってるんじゃない？
pyonta2017 0:1f956fab4d28 610 }
pyonta2017 0:1f956fab4d28 611 fclose(im);
pyonta2017 0:1f956fab4d28 612 timer.reset();
pyonta2017 0:1f956fab4d28 613 timer.stop();
pyonta2017 0:1f956fab4d28 614 val = 0;
pyonta2017 0:1f956fab4d28 615 myled2 = 0;
pyonta2017 0:1f956fab4d28 616 //wait(30); //test
pyonta2017 0:1f956fab4d28 617 wait(600); //end to end
pyonta2017 0:1f956fab4d28 618 ///////////////////////////
pyonta2017 0:1f956fab4d28 619 */
pyonta2017 0:1f956fab4d28 620 //着地検知用timer
pyonta2017 0:1f956fab4d28 621 timer.reset();
pyonta2017 0:1f956fab4d28 622 timer.start();
pyonta2017 0:1f956fab4d28 623 val = 0;
pyonta2017 0:1f956fab4d28 624 while(1){
pyonta2017 0:1f956fab4d28 625 switch(phase){
pyonta2017 0:1f956fab4d28 626 case Preparing:
pyonta2017 0:1f956fab4d28 627 myled1 = 1;
pyonta2017 0:1f956fab4d28 628 getAltitude();
pyonta2017 0:1f956fab4d28 629 wait(0.5);
pyonta2017 0:1f956fab4d28 630 val = timer.read();
pyonta2017 0:1f956fab4d28 631 if(flag1 > 10) phase = Rising;
pyonta2017 0:1f956fab4d28 632 if( val > 300) phase = Rising;
pyonta2017 0:1f956fab4d28 633 break;
pyonta2017 0:1f956fab4d28 634 case Rising:
pyonta2017 0:1f956fab4d28 635 myled1 = 0;
pyonta2017 0:1f956fab4d28 636 myled2 = 1;
pyonta2017 0:1f956fab4d28 637 getAltitude();
pyonta2017 0:1f956fab4d28 638 wait(0.5);
pyonta2017 0:1f956fab4d28 639 val = timer.read();
pyonta2017 0:1f956fab4d28 640 if(flag2 > 10) phase = Descending;
pyonta2017 0:1f956fab4d28 641 if(val > 600) phase = Descending;
pyonta2017 0:1f956fab4d28 642 break;
pyonta2017 0:1f956fab4d28 643 case Descending:
pyonta2017 0:1f956fab4d28 644 val = timer.read();
pyonta2017 0:1f956fab4d28 645 myled2 = 0;
pyonta2017 0:1f956fab4d28 646 myled3 = 1;
pyonta2017 0:1f956fab4d28 647 getGPS();
pyonta2017 0:1f956fab4d28 648 //wait(0.5);
pyonta2017 0:1f956fab4d28 649 if ( flag3 >= 100){
pyonta2017 0:1f956fab4d28 650 //mpuの条件もいれます
pyonta2017 0:1f956fab4d28 651 if( val > 180){
pyonta2017 0:1f956fab4d28 652 phase = Landing_Fusing;
pyonta2017 0:1f956fab4d28 653 }
pyonta2017 0:1f956fab4d28 654 }
pyonta2017 0:1f956fab4d28 655 if( val > 900) phase = Landing_Fusing;//タイマー検知15分
pyonta2017 0:1f956fab4d28 656 break;
pyonta2017 0:1f956fab4d28 657 case Landing_Fusing:
pyonta2017 0:1f956fab4d28 658 timer.reset();
pyonta2017 0:1f956fab4d28 659 timer.stop();
pyonta2017 0:1f956fab4d28 660 val = 0;
pyonta2017 0:1f956fab4d28 661 myled3 = 0;
pyonta2017 0:1f956fab4d28 662 myled4 = 1;
pyonta2017 0:1f956fab4d28 663 wait(30);
pyonta2017 0:1f956fab4d28 664 burning();
pyonta2017 0:1f956fab4d28 665 wait(5);
pyonta2017 0:1f956fab4d28 666 Mu2.printf("@DT04FIRE\r\n");
pyonta2017 0:1f956fab4d28 667 for ( int gp; gp <= 10; gp++){
pyonta2017 0:1f956fab4d28 668 getGPS();
pyonta2017 0:1f956fab4d28 669 }
pyonta2017 0:1f956fab4d28 670 wait(5);
pyonta2017 0:1f956fab4d28 671 jumpmotor = 1;
pyonta2017 0:1f956fab4d28 672 wait(15);
pyonta2017 0:1f956fab4d28 673 jumpmotor = 0;
pyonta2017 0:1f956fab4d28 674 phase = Moving;
pyonta2017 0:1f956fab4d28 675 break;
pyonta2017 0:1f956fab4d28 676 case Moving:
pyonta2017 0:1f956fab4d28 677 myled2 = 0;
pyonta2017 0:1f956fab4d28 678 myled1 = 0;
pyonta2017 0:1f956fab4d28 679 myled3 = 0;
pyonta2017 0:1f956fab4d28 680 myled4 = 0;
pyonta2017 0:1f956fab4d28 681 Mu2.printf("@DT04MOVE\r\n");
pyonta2017 0:1f956fab4d28 682 wait(1);
pyonta2017 0:1f956fab4d28 683 switch(step){
pyonta2017 0:1f956fab4d28 684 case get_goaldirection:
pyonta2017 0:1f956fab4d28 685 myled1 = 1;
pyonta2017 0:1f956fab4d28 686 //getGPS();
pyonta2017 0:1f956fab4d28 687 test_get_direction();
pyonta2017 0:1f956fab4d28 688 step = attitude_determination;
pyonta2017 0:1f956fab4d28 689 wait(1);
pyonta2017 0:1f956fab4d28 690 break;
pyonta2017 0:1f956fab4d28 691 case attitude_determination:
pyonta2017 0:1f956fab4d28 692 myled1 = 0;
pyonta2017 0:1f956fab4d28 693 myled2 = 1;
pyonta2017 0:1f956fab4d28 694 timer.reset();
pyonta2017 0:1f956fab4d28 695 timer.start();
pyonta2017 0:1f956fab4d28 696 val = 0;
pyonta2017 0:1f956fab4d28 697 while(val < 5){
pyonta2017 0:1f956fab4d28 698 val = timer.read();
pyonta2017 0:1f956fab4d28 699 getheading3axis();
pyonta2017 0:1f956fab4d28 700 wait(0.5);
pyonta2017 0:1f956fab4d28 701 }
pyonta2017 0:1f956fab4d28 702 getheading3axis();
pyonta2017 0:1f956fab4d28 703 step = calculate_direction;
pyonta2017 0:1f956fab4d28 704 wait(1);
pyonta2017 0:1f956fab4d28 705 break;
pyonta2017 0:1f956fab4d28 706 case calculate_direction:
pyonta2017 0:1f956fab4d28 707 myled2 = 0;
pyonta2017 0:1f956fab4d28 708 myled3 = 1;
pyonta2017 0:1f956fab4d28 709 if(headingLPF < M_PI){
pyonta2017 0:1f956fab4d28 710 if(goal_direction < headingLPF+M_PI){
pyonta2017 0:1f956fab4d28 711 angular_difference = headingLPF - goal_direction;
pyonta2017 0:1f956fab4d28 712 }
pyonta2017 0:1f956fab4d28 713 else{
pyonta2017 0:1f956fab4d28 714 angular_difference = 2*M_PI + (headingLPF - goal_direction);
pyonta2017 0:1f956fab4d28 715 }
pyonta2017 0:1f956fab4d28 716 }
pyonta2017 0:1f956fab4d28 717 else{
pyonta2017 0:1f956fab4d28 718 if(goal_direction < headingLPF - M_PI){
pyonta2017 0:1f956fab4d28 719 angular_difference = headingLPF - goal_direction - 2*M_PI;
pyonta2017 0:1f956fab4d28 720 }
pyonta2017 0:1f956fab4d28 721 else{
pyonta2017 0:1f956fab4d28 722 angular_difference = headingLPF - goal_direction;
pyonta2017 0:1f956fab4d28 723 }
pyonta2017 0:1f956fab4d28 724 }
pyonta2017 0:1f956fab4d28 725 mp.printf("heading:%f,goal_direction%f\n\r",headingLPF*180/M_PI,goal_direction*180/M_PI);
pyonta2017 0:1f956fab4d28 726 //mp.printf("rangular_difference = %f\n\r" , angular_difference*180/M_PI);
pyonta2017 0:1f956fab4d28 727 //40度以下でジャンプ
pyonta2017 0:1f956fab4d28 728 if ((angular_difference < 0.698132) && (angular_difference > -0.698132)){
pyonta2017 0:1f956fab4d28 729 step = jump;
pyonta2017 0:1f956fab4d28 730 }
pyonta2017 0:1f956fab4d28 731 else {
pyonta2017 0:1f956fab4d28 732 step = direction_control;
pyonta2017 0:1f956fab4d28 733 }
pyonta2017 0:1f956fab4d28 734 wait(1);
pyonta2017 0:1f956fab4d28 735 break;
pyonta2017 0:1f956fab4d28 736
pyonta2017 0:1f956fab4d28 737 case direction_control:
pyonta2017 0:1f956fab4d28 738 myled1 = 0;
pyonta2017 0:1f956fab4d28 739 myled3 = 0;
pyonta2017 0:1f956fab4d28 740 myled4 = 1;
pyonta2017 0:1f956fab4d28 741 //int drive_time;
pyonta2017 0:1f956fab4d28 742 timer.reset();
pyonta2017 0:1f956fab4d28 743 timer.stop();
pyonta2017 0:1f956fab4d28 744 timer.start();
pyonta2017 0:1f956fab4d28 745 val = 0;
pyonta2017 0:1f956fab4d28 746 while(val < 2){
pyonta2017 0:1f956fab4d28 747 val = timer.read();
pyonta2017 0:1f956fab4d28 748 motor_drive();
pyonta2017 0:1f956fab4d28 749 }
pyonta2017 0:1f956fab4d28 750 controlmotor1.write(0);
pyonta2017 0:1f956fab4d28 751 controlmotor2.write(0);
pyonta2017 0:1f956fab4d28 752 pin21.write(0.0);
pyonta2017 0:1f956fab4d28 753 timer.reset();
pyonta2017 0:1f956fab4d28 754 timer.stop();
pyonta2017 0:1f956fab4d28 755 val = 0;
pyonta2017 0:1f956fab4d28 756 //step = jump;
pyonta2017 0:1f956fab4d28 757 if(jump_count == 3){
pyonta2017 0:1f956fab4d28 758 step = jump;
pyonta2017 0:1f956fab4d28 759 }
pyonta2017 0:1f956fab4d28 760 else{
pyonta2017 0:1f956fab4d28 761 step = attitude_determination;
pyonta2017 0:1f956fab4d28 762 }
pyonta2017 0:1f956fab4d28 763 jump_count = jump_count+1;
pyonta2017 0:1f956fab4d28 764 break;
pyonta2017 0:1f956fab4d28 765
pyonta2017 0:1f956fab4d28 766 case jump:
pyonta2017 0:1f956fab4d28 767 //ログファイル
pyonta2017 0:1f956fab4d28 768 myled1 = 1;
pyonta2017 0:1f956fab4d28 769 myled2 = 1;
pyonta2017 0:1f956fab4d28 770 wait(2);
pyonta2017 0:1f956fab4d28 771 FILE *fp = fopen("/sd/mydir/height1.txt", "a");
pyonta2017 0:1f956fab4d28 772 wait(1);
pyonta2017 0:1f956fab4d28 773 timer.reset();
pyonta2017 0:1f956fab4d28 774 timer.start();
pyonta2017 0:1f956fab4d28 775 val = 0;
pyonta2017 0:1f956fab4d28 776 jumpmotor = 1;
pyonta2017 0:1f956fab4d28 777 while(val < 4000){
pyonta2017 0:1f956fab4d28 778 val = timer.read_ms();
pyonta2017 0:1f956fab4d28 779 adxl[0] = 0x32;
pyonta2017 0:1f956fab4d28 780 i2c.write(addr,adxl,1);
pyonta2017 0:1f956fab4d28 781 i2c.read(0xA7,adxl,6);
pyonta2017 0:1f956fab4d28 782 axout = (((int16_t )adxl[1]) << 8) | adxl[0];
pyonta2017 0:1f956fab4d28 783 ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
pyonta2017 0:1f956fab4d28 784 azout = (((int16_t )adxl[5]) << 8) | adxl[4];
pyonta2017 0:1f956fab4d28 785 fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
pyonta2017 0:1f956fab4d28 786 wait_ms(2);
pyonta2017 0:1f956fab4d28 787 }
pyonta2017 0:1f956fab4d28 788 jumpmotor = 0;
pyonta2017 0:1f956fab4d28 789 wait(1);
pyonta2017 0:1f956fab4d28 790 fclose(fp);
pyonta2017 0:1f956fab4d28 791 myled3 = 1;
pyonta2017 0:1f956fab4d28 792 jump_count = 0;
pyonta2017 0:1f956fab4d28 793 wait(3);
pyonta2017 0:1f956fab4d28 794 step = get_goaldirection;
pyonta2017 0:1f956fab4d28 795 //step = direction_control;
pyonta2017 0:1f956fab4d28 796 break;
pyonta2017 0:1f956fab4d28 797 }
pyonta2017 0:1f956fab4d28 798 }
pyonta2017 0:1f956fab4d28 799 }
pyonta2017 0:1f956fab4d28 800 }