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必要、ＳＤが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必要、ＳＤが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;
+            }
+        }                       
+    }
+}