pyonta cansat / Mbed 2 deprecated FM_integration

k

Dependencies:   Hm MPL MPU60580 MU2Class SDFileSystem mbed

main.cpp@1:8a25883c423c, 2017-09-13 (annotated)

Committer:
pyonta2017
Date:
Wed Sep 13 09:44:08 2017 +0000
Revision:
1:8a25883c423c
Parent:
0:6ddf1386e71d 
aa

Who changed what in which revision?

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