k
Dependencies: Hm MPL MPU60580 MU2Class SDFileSystem mbed
Diff: main.cpp
- Revision:
- 0:6ddf1386e71d
- Child:
- 1:8a25883c423c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Sep 11 10:05:39 2017 +0000 @@ -0,0 +1,809 @@ +#include "mbed.h" +#include "xprintf.h" +#include "MPL3115A2.h" +#include "HMC5883L.h" +#include "MPU6050.h" +#include "SDFileSystem.h" +#include "EthernetPowerControl.h" +#define M_PI 3.141592 + +//hmc calibration mode +#define STOP 0 //initial +#define CAL 1 //calibration +#define RUN 2 //run + +//Sequence phase +#define Preparing 1 +#define Rising 2 +#define Descending 3 +#define Landing_Fusing 4 +#define Moving 5 + +//Moving step +#define get_goaldirection 0 +#define attitude_determination 1 +#define calculate_direction 2 +#define direction_control 3 +#define jump 4 + +DigitalOut myled1(LED1); +DigitalOut myled2(LED2); +DigitalOut myled3(LED3); +DigitalOut myled4(LED4); + +//ピンアサイン確認 +Serial gps(p13, p14); +Serial Mu2(p28, p27); +I2C i2c(p9, p10); // sda, scl +Serial mp(USBTX, USBRX); +SDFileSystem sd(p5, p6, p7, p8, "sd"); +LocalFileSystem local("local"); +DigitalOut controlmotor1(p22),controlmotor2(p23);//方向制御モーター +PwmOut pin21(p21); +DigitalOut jumpmotor(p24); + +//溶断 +DigitalOut para(p18); +DigitalOut marker(p19); +DigitalOut jumparm(p20); +// +DigitalOut Flightpin(p26); + +Timer timer; +int val; +int val_total; +unsigned char timer_set; +unsigned char phase; +unsigned char step; + +//気圧/////// +MPL3115A2 APsensor(&i2c, &mp); +int alt; +int ini_alt; +int min_alt; +int min_alt5; +int max_alt; +int rela_max; +int rela_ini; +int flag1; +int flag2; +int flag3; +unsigned char alttimes; +void getAltitude(){ + //wait_ms(300); + Altitude a; + Temperature t; + Pressure p; + APsensor.readAltitude(&a); + APsensor.readTemperature(&t); + APsensor.setModeStandby(); + APsensor.setModeBarometer(); + APsensor.setModeActive(); + APsensor.readPressure(&p); + alt = a; + //気球試験用 + if(phase == Preparing){ + rela_ini = alt - ini_alt; + if(rela_ini > 50)flag1 = flag1+1; + } + + if(phase == Rising){ + if(max_alt < alt){ + max_alt = alt; + } + else{ + rela_max = max_alt -alt; + if(rela_max > 50)flag2 = flag2+1; + } + } + //着地判定 + if(phase == Descending){ + if (min_alt > alt){ + min_alt = alt; + min_alt5 = min_alt + 5; + flag3 -= 1; + } + else { + if( alt < min_alt5){ + flag3 +=2; + } + } + } + mp.printf("alt:%d,max:%d,ini:%d,phase:%d\r\n",alt,max_alt,ini_alt,phase); + mp.printf("rela_ini:%d,rela_max:%d\r\n",rela_ini,rela_max); + mp.printf("flag1:%d,flag2:%d,flag3:%d\r\n",flag1,flag2,flag3); + //mp.printf("OFF_H: 0x%X, OFF_T: 0x%X, OFF_P: 0x%X\r\n", APsensor.offsetAltitude(), APsensor.offsetTemperature(), APsensor.offsetPressure()); + APsensor.setModeStandby(); + APsensor.setModeAltimeter(); + APsensor.setModeActive(); +} +///// + +//GPS +int i,rlock,mode; +char gps_data[256]; +char ns,ew; +float w_time,hokui,tokei; +float g_hokui,g_tokei; +float d_hokui,m_hokui,d_tokei,m_tokei; +unsigned char c; +//目標経度,緯度 +float goal_tokei,goal_hokui; +float goal_direction; +float direction_y; +float direction_x; +float angular_difference; + +void getGPS(){ + //目的地 ブラックロック修正 + goal_tokei = 118.9833; + goal_hokui = 40.92493; + c = gps.getc(); + if( c=='$' || i == 256){ + mode = 0; + i = 0; + for(int j=0; j<256; j++){ + gps_data[j]=NULL; + } + } + if(mode==0){ + if((gps_data[i]=c) != '\r'){ + i++; + }else{ + gps_data[i]='\0'; + + if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){ + if(rlock==1){ + //mp.printf("Status:Lock(%d)\n\r",rlock); + //logitude + d_tokei= int(tokei/100); + m_tokei= (tokei-d_tokei*100)/60; + g_tokei= d_tokei+m_tokei; + //Latitude + d_hokui=int(hokui/100); + m_hokui=(hokui-d_hokui*100)/60; + g_hokui=d_hokui+m_hokui; + mp.printf("Lon:%.6f, Lat:%.6f\n\r",g_tokei, g_hokui); + Mu2.printf("@DT14%f,%f\r\n",g_tokei, g_hokui); + float longitudinalDifference =-(goal_tokei - g_tokei); + float latitudinalDifference = goal_hokui - g_hokui; + + //球面三角法 + direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180); + 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); + goal_direction = atan2f(direction_y,direction_x); + if(goal_direction < 0)goal_direction += 2*M_PI; + + //磁北に対する角度に調節 ネバダ東に14度 + goal_direction -= 0.244346; + if (goal_direction < 0) goal_direction += 2*M_PI; + mp.printf("goal:%f\n\r",goal_direction*180/M_PI); + //mp.printf("longitudinalDifference:%f",longitudinalDifference); + //mp.printf("latitudinalDifference:%f",latitudinalDifference); + if(phase == Descending)getAltitude(); + if(phase == Moving)step = attitude_determination; + } + else{ + mp.printf("\n\rStatus:unLock(%d)\n\r",rlock); + mp.printf("%s",gps_data); + Mu2.printf("@DT02No\r\n"); + if(phase == Descending)getAltitude(); + //if(phase == Moving)test_get_direction(); + } + sprintf(gps_data, ""); + } + } + } +} + +void MU2initialize(){ + Mu2.baud(19200); + Mu2.printf("@EI34\r\n"); + wait(1); + Mu2.printf("@DI25\r\n"); + wait(1); + Mu2.printf("@CH0F\r\n"); + wait(1); + Mu2.printf("@GI34\r\n"); + wait(1); + } + +///////////////////// +//地磁気 +HMC5883L compass(p9, p10); +Ticker interrupt; +double heading0 = 0.0; +double heading1 = 0.0; +double heading2 = 0.0; +double heading3 = 0.0; +double headingLPF = 0.0; +double initHeading; +double tgtHeading; +double preHeading = 0.0; +unsigned char hmc_mode; + +int maxX, minX, maxY, minY; +int ofsX = 0; //calibration x +int ofsY = 0; //calibration y +int counter = 0; +int ofsXset = 1122; +int ofsYset = -466; +int ofsZset = -522; + +//地磁気キャリブレーション用 +void intrpt() { + int16_t raw[3]; + compass.getXYZ(raw); + //double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2] + double heading = atan2(static_cast<double>(raw[2]-ofsYset), static_cast<double>(raw[0]-ofsXset)); + if(heading < 0)heading += 2*M_PI; + if(heading > 2*M_PI)heading -= 2*M_PI; + heading3 = heading2; + heading2 = heading1; + heading1 = heading0; + heading0 = heading; + headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter + + switch(hmc_mode) { + case STOP: + if(counter == 100) { + initHeading = headingLPF; + hmc_mode = CAL; + maxX = raw[0]; + minX = raw[0]; + maxY = raw[2]; + minY = raw[2]; + counter = 0; + //mp.printf("STOP end\n\r"); + } + break; + case CAL: + if(raw[0] > maxX)maxX = raw[0]; + if(raw[0] < minX)minX = raw[0]; + if(raw[2] > maxY)maxY = raw[2]; + if(raw[2] < minY)minY = raw[2]; + if((counter > 100) + && (headingLPF > (initHeading-0.01)) + && (headingLPF < (initHeading+0.01))) { + ofsX = (maxX + minX)/2; + ofsY = (maxY + minY)/2; + //hmc_mode = RUN; + counter = 0; + //mp.printf("ofs=%d,%d\n\r",ofsX,ofsY); + } + break; + case RUN: + mp.printf("heading=%f,x=%d,y=%d\n\r",headingLPF*180/M_PI,raw[0]-ofsXset,raw[2]-ofsYset); + //mp.printf("%d,%d,%d\r\n",raw[0],raw[2],raw[1]); + //step = calculate_direction; + break; + + } + counter++; +} + +//加速度 +MPU6050 mpu; +int16_t ax, ay, az; +int16_t gx, gy, gz; +int32_t axa, aya, aza; +int32_t gxa, gya, gza; + +//オフセット値 +double axoffset = 586.711; +double ayoffset = -174.847; +double azoffset = -2195.34375; + +//センサの出力あたりの加速度[m/s^2] +double axrate = 0.000601615; +double ayrate = 0.000598334; +double azrate = 0.0005845; + +//3軸キャリブレーション +double phi; +double theta; +double heading3axis; + +double bfy; +double bfx; + +//3軸からheading算出 +void getheading3axis() { + //加速度算出 + mpu.getAcceleration(&ax, &ay, &az); + double axcal = -axrate * (static_cast<double>(ax) - axoffset); + double aycal = -ayrate * (static_cast<double>(ay) - ayoffset); + double azcal = -azrate * (static_cast<double>(az) - azoffset); + + phi = atan2(aycal,azcal); + //phi += M_PI; + theta = atan2(-axcal,aycal*sin(phi)+azcal*cos(phi)); + //theta += M_PI; + + //地磁気算出 + int16_t raw[3]; + compass.getXYZ(raw); + bfy = -(static_cast<double>(raw[1]-ofsZset))*sin(phi) + (static_cast<double>(raw[2]-ofsYset))*cos(phi); + bfx = (static_cast<double>(raw[0]-ofsXset))*cos(theta) + + (static_cast<double>(raw[2]-ofsYset))*sin(theta)*sin(phi) + + (static_cast<double>(raw[1]-ofsZset))*sin(theta)*cos(phi); + double heading = atan2(-bfy, bfx); + if(heading < 0)heading += 2*M_PI; + if(heading > 2*M_PI)heading -= 2*M_PI; + heading3 = heading2; + heading2 = heading1; + heading1 = heading0; + heading0 = heading; + headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter + //headingLPF += 0.349066; + if(headingLPF > 2*M_PI) headingLPF -= 2*M_PI; + //mp.printf("ax:%f,ay:%f,az:%f\r\n",axcal,aycal,azcal); + mp.printf("heading:%f,phi:%f,theta:%f\r\n",headingLPF*180/M_PI,phi*180/M_PI,theta*180/M_PI); + } + +//ADXL375 +const int addr = 0xA6; +char adxl[6]; +short int axout = 0; +short int ayout = 0; +short int azout = 0; + +void adxl_init(){ + char fifo[2] = {0x38,0x80}; + i2c.write(addr,fifo,2); //FIFO_Mode -> Stream + char power[2] = {0x2D,0x08}; + i2c.write(addr,power,2); //measurebit -> 1 +} + +void adxl_get(){ + adxl[0] = 0x32; + i2c.write(addr,adxl,1); + i2c.read(0xA7,adxl,6); + axout = (((int16_t )adxl[1]) << 8) | adxl[0]; + ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; + azout = (((int16_t )adxl[5]) << 8) | adxl[4]; +} + + +//モーター駆動方向制御 +void motor_drive(){ + controlmotor1.write(1); + controlmotor2.write(0); + pin21.write(0.8); + //if(pin1)mp.printf("Yes!\r\n"); +} + +//溶断プログラム +void burning(){ + wait(5); + myled1 = 1; + para = 1; + wait(1); + myled1 = 0; + para = 0; + wait(5); + myled2 = 1; + marker = 1; + wait(1); + myled2 = 0; + marker = 0; + wait(5); + myled3 = 1; + jumparm = 1; + wait(1); + myled3 = 0; + jumparm = 0; +} +//跳躍モーター駆動まいまいかわいい +void jumping(){ + timer.start(); + //モーター駆動時間調整 + while(val < 4){ + val = timer.read(); + jumpmotor = 1; + myled4 != myled4; + } + timer.reset(); + timer.stop(); + jumpmotor = 0; + val = 0; +} + +///test +//GPSなし方向制御,跳躍テスト +float test_tokei,test_hokui; +void test_get_direction(){ + //目的地 大岡山駅 + goal_tokei = 139.686234; + goal_hokui = 35.614330; + + //テスト場所石川台一号館 + test_tokei = 139.681529; + test_hokui = 35.605263; + + float longitudinalDifference = goal_tokei - test_tokei; + float latitudinalDifference = goal_hokui - test_hokui; + //球面三角法 + direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180); + 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); + goal_direction = atan2f(direction_y,direction_x); + if(goal_direction < 0)goal_direction += 2*M_PI; + + //磁北に対する角度に調節 東京7度 + goal_direction += 0.122173; + //goal_direction2 += 0.122173; + if (goal_direction < 0) goal_direction += 2*M_PI; +} + +/////////////////////////main//////////////////////////////////// +int main(){ + PHY_PowerDown();//省電力 + wait(2); + //センサ初期化 + Mu2.baud(19200); + Mu2.printf("@EI34\r\n"); + wait(1); + Mu2.printf("@DI25\r\n"); + wait(1); + Mu2.printf("@CH0F\r\n"); + wait(1); + Mu2.printf("@GI34\r\n"); + wait(1); + APsensor.init(); + wait(0.2); + hmc_mode = RUN; + mpu.initialize(); + wait(0.2); + compass.init(); + wait(0.2); + adxl_init(); + wait(0.2); + APsensor.setOffsetAltitude(100); + APsensor.setOffsetTemperature(10); + APsensor.setOffsetPressure(-32); + mkdir("/sd/mydir", 0777); + min_alt = 4000.0; + Flightpin = 1; + //高度初期値 + for(int h = 0; h < 10; h++){ + getAltitude(); + ini_alt = ini_alt + alt; + wait(0.5); + } + ini_alt = ini_alt/10; + mp.printf("ini_alt:%d\r\n",ini_alt); + int jump_count; + phase = Preparing; + wait(900);//開始15分待機 + //phase = Moving; + /////////////////////////////単機能//////// + + //wait(5); + + //while(1){ + //myled1 = 1; + //Flightpin = 1; + //GPS_MU2 + //getGPS(); + //方向制御用モーター + /* + controlmotor1.write(1); + controlmotor2.write(0); + pin21.write(0.8); + wait(3); + controlmotor1.write(0); + controlmotor2.write(0); + pin21.write(0.0); + */ + //跳躍用モーター + //jumpmotor = 1; + //wait(5); + //jumpmotor = 0; + //溶断 + //burning(); + //気圧 + //getAltitude(); + //mpu + /* + mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); + double axcal = -axrate * (static_cast<double>(ax) - axoffset); + double aycal = -ayrate * (static_cast<double>(ay) - ayoffset); + double azcal = -azrate * (static_cast<double>(az) - azoffset); + mp.printf("x:%f,y:%f,z:%f",axcal,aycal,azcal); + */ + //hmc + /* + hmc_mode = RUN; + intrpt(); + wait(0.5); + */ + //hmc_mpu + //getheading3axis() ; + //wait(0.3); + //adxl_SDカード 跳躍高さ測定 + /* + mkdir("/sd/mydir", 0777); + wait(1); + FILE *fp = fopen("/sd/mydir/height1.txt", "a"); + wait(1); + if(fp == NULL) { + error("Could not open file for write\n"); + } + timer.reset(); + timer.start(); + val = 0; + while(val < 5000){ + jumpmotor = 1; + val = timer.read_ms(); + adxl[0] = 0x32; + i2c.write(addr,adxl,1); + i2c.read(0xA7,adxl,6); + axout = (((int16_t )adxl[1]) << 8) | adxl[0]; + ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; + azout = (((int16_t )adxl[5]) << 8) | adxl[4]; + fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); + wait_ms(2); + myled1 !=myled1; + } + myled1 = 0; + jumpmotor = 0; + wait(1); + fclose(fp); + */ + // + + //} + ///////////////////////// + + + /* + //////////////////////////安全試験用///////// + //振動試験 + myled1 = 0; + //wait(5);//test + wait(900);//end to end + //静荷重 + FILE *sl = fopen("/local/static2.txt", "a"); + wait(10); + myled1 = 1; + wait(5); + timer.reset(); + timer.start(); + //while(val < 30000){ //test + while(val < 600000){ //end to end + val = timer.read_ms(); + adxl[0] = 0x32; + i2c.write(addr,adxl,1); + i2c.read(0xA7,adxl,6); + axout = (((int16_t )adxl[1]) << 8) | adxl[0]; + ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; + azout = (((int16_t )adxl[5]) << 8) | adxl[4]; + //pc.printf("time:%d,X:%d,Y:%d,Z:%d\r\n",val,axout,ayout,azout); + fprintf(sl, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); + wait_ms(200); + //wait必要、SDがbusyの状態で送っちゃってるんじゃない? + } + fclose(sl); + timer.reset(); + timer.stop(); + val = 0; + myled1 = 0; + //wait(5); //test + wait(960); //end to end + + //開傘衝撃 + FILE *im = fopen("/local/impact2.txt", "a"); + wait(10); + myled2 = 1; + timer.reset(); + timer.start(); + //while(val < 10000){ //test + while(val < 30000){ //end to end + val = timer.read_ms(); + adxl[0] = 0x32; + i2c.write(addr,adxl,1); + i2c.read(0xA7,adxl,6); + axout = (((int16_t )adxl[1]) << 8) | adxl[0]; + ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; + azout = (((int16_t )adxl[5]) << 8) | adxl[4]; + fprintf(im, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); + wait_ms(3); + //wait必要、SDがbusyの状態で送っちゃってるんじゃない? + } + fclose(im); + timer.reset(); + timer.stop(); + val = 0; + myled2 = 0; + //wait(30); //test + wait(600); //end to end + /////////////////////////// + */ + //着地検知用timer + timer.reset(); + timer.start(); + val = 0; + while(1){ + switch(phase){ + case Preparing: + myled1 = 1; + getAltitude(); + wait(0.5); + val = timer.read(); + if(flag1 > 20) phase = Rising; + if( val > 600) phase = Rising; //計25分 + break; + case Rising: + myled1 = 0; + myled2 = 1; + getAltitude(); + wait(0.5); + val = timer.read(); + if(flag2 > 10) phase = Descending; + if(val > 900) phase = Descending; + break; + case Descending: + val = timer.read(); + myled2 = 0; + myled3 = 1; + getGPS(); + + if ( val >1500){ + timer.reset(); + val = 0; + timer_set = 1; + } + if (timer_set == 1){ + val_total = 1500 + val; + } + if ( flag3 >= 300){ + if( val_total > 1800){ + phase = Landing_Fusing; + } + } + if( val_total > 2400) phase = Landing_Fusing;//電源onから55分 + break; + case Landing_Fusing: + timer.reset(); + timer.stop(); + val = 0; + myled3 = 0; + myled4 = 1; + wait(30); + burning(); + wait(5); + Mu2.printf("@DT04FIRE\r\n"); + for ( int gp; gp <= 10; gp++){ + getGPS(); + } + wait(5); + jumpmotor = 1; + wait(15); + jumpmotor = 0; + phase = Moving; + break; + case Moving: + myled2 = 0; + myled1 = 0; + myled3 = 0; + myled4 = 0; + wait(1); + switch(step){ + case get_goaldirection: + myled1 = 1; + getGPS(); + //test_get_direction(); + step = attitude_determination; + wait(1); + break; + case attitude_determination: + myled1 = 0; + myled2 = 1; + timer.reset(); + timer.start(); + val = 0; + while(val < 3){ + val = timer.read(); + getheading3axis(); + wait(0.5); + } + getheading3axis(); + step = calculate_direction; + wait(1); + break; + case calculate_direction: + myled2 = 0; + myled3 = 1; + if(headingLPF < M_PI){ + if(goal_direction < headingLPF+M_PI){ + angular_difference = headingLPF - goal_direction; + } + else{ + angular_difference = 2*M_PI + (headingLPF - goal_direction); + } + } + else{ + if(goal_direction < headingLPF - M_PI){ + angular_difference = headingLPF - goal_direction - 2*M_PI; + } + else{ + angular_difference = headingLPF - goal_direction; + } + } + mp.printf("heading:%f,goal_direction%f\n\r",headingLPF*180/M_PI,goal_direction*180/M_PI); + //mp.printf("rangular_difference = %f\n\r" , angular_difference*180/M_PI); + //40度以下でジャンプ + if ((angular_difference < 0.698132) && (angular_difference > -0.698132)){ + step = jump; + } + else { + step = direction_control; + } + wait(1); + break; + + case direction_control: + myled1 = 0; + myled3 = 0; + myled4 = 1; + //int drive_time; + timer.reset(); + timer.stop(); + timer.start(); + val = 0; + while(val < 2){ + val = timer.read(); + motor_drive(); + } + controlmotor1.write(0); + controlmotor2.write(0); + pin21.write(0.0); + timer.reset(); + timer.stop(); + val = 0; + //step = jump; + if(jump_count == 3){ + step = jump; + } + else{ + step = attitude_determination; + } + jump_count = jump_count+1; + break; + + case jump: + //ログファイル + myled1 = 1; + myled2 = 1; + wait(2); + FILE *fp = fopen("/sd/mydir/height1.txt", "a"); + wait(1); + timer.reset(); + timer.start(); + val = 0; + jumpmotor = 1; + Mu2.printf("@DT04JUMP\r\n"); + while(val < 4000){ + val = timer.read_ms(); + adxl[0] = 0x32; + i2c.write(addr,adxl,1); + i2c.read(0xA7,adxl,6); + axout = (((int16_t )adxl[1]) << 8) | adxl[0]; + ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; + azout = (((int16_t )adxl[5]) << 8) | adxl[4]; + fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); + wait_ms(2); + } + jumpmotor = 0; + wait(1); + fclose(fp); + myled3 = 1; + jump_count = 0; + wait(3); + step = get_goaldirection; + //step = direction_control; + break; + } + } + } +}