pyonta cansat / Mbed 2 deprecated FM_integration_off

a

Dependencies:   HMC5883 MPL3115A MPU605 MU2Class SDFileSystem mbed

main.cpp@0:ac7a5495d95c, 2017-09-09 (annotated)

Committer:
pyonta2017
Date:
Sat Sep 09 23:18:12 2017 +0000
Revision:
0:ac7a5495d95c
a

Who changed what in which revision?

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