File content as of revision 0:5fef60d1a47e:

#include "mbed.h"
#include "HeptaGPS.h"
#include "MPU9250.h"
#include "ta7291p.h"

#define MORTOR_LOW_SPEED 0.6
#define MORTOR_HIGH_SPEED 1.0

#define LATITUDE 35.72593
#define LONGITUDE 140.05737

#define MAGNET_CALIB_X 3.227
#define MAGNET_CALIB_Y -14.7

#define EQUATORIAL_RADIUS 6738137
 
Serial pc(USBTX, USBRX);//(tx,rx)
Serial xbee(p9,p10);
SPI spi(p5, p6, p7);
mpu9250_spi imu(spi,p8);  
HeptaGPS gps(p13,p14);//(tx,rx)
ta7291p mortor1(p25,p24,p26);
ta7291p mortor2(p22,p21,p23);

int flag;
void Cmd_rx(){     
     char cmd;
     
     xbee.printf("interrupt!!\r"); 
     cmd = xbee.getc();
     
     if(cmd == 's'){
         flag = 1;
     }
     else if(cmd == 'q'){
        flag = 2;
     }
}//割り込み関数
    
int Azimuthcal(float lat1,float log1){
    float dlog,dlat,dx,dy,azimuth;
       dlog=(LONGITUDE - log1)*3.1415/180;
       dlat=(LATITUDE - lat1)*3.1415/180;
       dx=EQUATORIAL_RADIUS*dlog*cos(lat1*3.1415/180);
       dy=EQUATORIAL_RADIUS*dlat;
       azimuth = atan2(dx,dy)*180/3.1415;
            return azimuth;                                                          
     }//GPSからの方位角
     
int readCompas(float mx,float my){
    float MagBear,ma,mb;
    imu.read_all();
    ma = (mx - MAGNET_CALIB_X)*cos(7*3.1415/180);
    mb = (my - MAGNET_CALIB_Y)*cos(7*3.1415/180);
    MagBear = atan2(mb,ma)*180/3.1415;
    return MagBear;
    } //地磁気センサーからの磁方位角    

void setup_MPU9250(void){
        if(imu.init(1,BITS_DLPF_CFG_188HZ)){  //INIT the mpu9250
        printf("\nCouldn't initialize MPU9250 via SPI!");
    }    
    imu.whoami(); //output the I2C address to know if SPI is working, it should be 104
    wait(1);    
    imu.set_gyro_scale(BITS_FS_2000DPS);    //Set full scale range for gyros
    wait(1);  
    imu.set_acc_scale(BITS_FS_16G);          //Set full scale range for accs
    wait(1);
    imu.AK8963_whoami();
    wait(0.1);  
    imu.AK8963_calib_Magnetometer();
    imu.calib_acc();
      }//9軸センサーの前処理


void straight(void){
      mortor1.rotf(MORTOR_HIGH_SPEED);
      mortor2.rotf(MORTOR_HIGH_SPEED);
      }

void turn_left(void){
      mortor1.rotf(MORTOR_LOW_SPEED);
      mortor2.rotf(MORTOR_HIGH_SPEED);
      }
      
void turn_right(void){
      mortor1.rotf(MORTOR_HIGH_SPEED);
      mortor2.rotf(MORTOR_LOW_SPEED);
      }

void stop_mortor(void){
     mortor1.rotstop();
     mortor2.rotstop();
     }
     
int calc_distance(float lat1_s,float log1_s ){
    float x1=(LONGITUDE - log1_s)*3.1415/180;
    float y1=(LATITUDE - lat1_s)*3.1415/180;
    float dx1=EQUATORIAL_RADIUS*x1*cos(lat1_s*3.1415/180);
    float dy1=EQUATORIAL_RADIUS*y1;
    float distance = sqrt(dx1*dx1 + dy1*dy1);
    
    if((distance > 0)&&(distance < 1)){
       return flag = 2;
        }
        else{
           return flag =1;
        }
    }//距離計算  
void setup_GPS(float lat2,float log2){
    xbee.printf("lat %2.5f,log %2.5f\r",lat2,log2);
    }
      
                        
int main() {
    float lat,log;
    float magx,magy;
    int Az,Com,attitudeAngle;
    
    setup_MPU9250();
    xbee.attach(Cmd_rx,Serial::RxIrq); 
    while(1){
        gps.sensing(&lat,&log);
        setup_GPS(lat,log);
        if(flag == 1){
            break;
            }
        }
   while(1){ 
             gps.sensing(&lat,&log);
             xbee.printf("lat=%2.5f,log=%2.5f\r\n",lat,log);
             Az = Azimuthcal(lat,log);
             calc_distance(lat,log);   
             
             magx = imu.Magnetometer[0];
             magy = imu.Magnetometer[1];
             Com = readCompas(magx,magy);
             
             attitudeAngle = Az + Com;
         
             if(flag==1){          
                    if((attitudeAngle >= -30 )&&(attitudeAngle <= 30)){
                        straight();
                    }
                    else if(attitudeAngle > 30){
                        turn_right();
                    }else{
                        turn_left();
                        }
            }
            else if(flag == 2){
                xbee.printf("flag=2\r");
                stop_mortor();
            }
                    else{
                        xbee.printf("stop\r");
                        stop_mortor();
                        }   
       }         
                                                             
       }