File content as of revision 3:8d904225adfd:

/*
 * MAG3110 Sensor Library for mbed
 * TODO: Add proper header
 */


#include "mbed.h"
#include "MotionSensor.h"
#include "MAG3110.h"
#include "math.h"
//#include "SDFileSystem.h"

//SDFileSystem sd(p5, p6, p7, p8, "sd");
Serial pc(USBTX,USBRX);
//Serial pc(p9,p10);

int16_t con_x,con_y,con_z;
LocalFileSystem local("local");
//MAG3110 mag(p9,p10);
MAG3110 mag(p28,p27);
Serial gps(p13,p14);

DigitalIn local_on(p17);
//DigitalIn local_del(p19);
//DigitalIn cds(p29);
Timer ActiveTime; //タイマー
//DigitalOut trg(p29);
//DigitalOut sig(p28);
//InterruptIn distance(p27);

DigitalOut bit1(LED4);
DigitalOut bit2(LED3);
DigitalOut bit3(LED2);
DigitalOut bit4(LED1);
//DigitalOut bit5(p16);
DigitalIn local_on2(p16);
DigitalOut stby(p11);
DigitalOut motor_io(p12);
PwmOut servo(p24);
PwmOut motor_r(p21);
PwmOut motor_l(p22);
DigitalOut muda(p15);
InterruptIn cds(p29);
//DigitalIn cds(p29);

char gps_s[256];
char gps_c[256];
float palse=0.02;
float servo_palse=0.002;
float motor_palse=0.02;
float sig_time;
float offset_x,offset_y;
float cds_on,x_max,y_max,x_min,y_min;
float count_offset,count_gps;
int a,b,gp,gga,rmc,gll,flag,count2,count_move,quadrant;
float x_target,y_target,d_target,sita,angle_caly,angle_calx,direction_target;
float longitude_target = 36.115986;//36.11707
float latitude_target = 139.456207;//139.4720
float utc_time,latitude1,latitude2,longitude1,longitude2,speed,course,utc_date,gps_magnetic;
char pos_status;
float direction,x_dat,y_dat,longitudegosa,latitudegosa;
char angle_c;


void init()
{
    muda=0;
    cds_on = 0;
    count_move = 0;
    stby = 1;
    motor_io = 1;
    motor_r.period(0.02);
    motor_l.period(0.02);
    servo.period(0.02);
    x_max = -1000.0;//x,yの最大値を初期化
    y_max = -1000.0;
    x_min = 1000.0;
    y_min = 1000.0;

}

void cal_l()
{
    latitude1 = latitude1/100;
    latitude2 = (fmod(latitude1,1)/60)*100;
    latitude1 = floor(latitude1);
    latitude1 = latitude1+latitude2;
    
    longitude1 = longitude1/100;
    longitude2 = fmod(longitude1,1)/60*100;
    longitude1 = floor(longitude1);
    longitude1 = longitude1+longitude2;

}

void gps_read()
{
    __disable_irq();//割り込み禁止
    
    bit4=1;
    pc.printf("gps_read\n");
    for(count2=0; count2 < 200 ; count2++) {
        gps_s[count2] = gps.getc();
        pc.printf("%c",gps_s[count2]);
    }
    bit4=0;
    if(count_move==0 || count_move==3)
     {__enable_irq();}//割り込み許可
}

int gps_save()
{
    a=0;
    gp = 2;//1回目の＄は無視（flug=1を無視）
    count_gps = 0;

    pc.printf("\ngps_save\n");

    while(a <= 250 && count_gps < 4) {

        for(a=0,b=0,flag=0;  flag <= gp && flag < 4 && gp < 4 && a < 200;  a++,b++)//"lag<4" "gp<4" "a<200"は無限ループ対策
         {
            bit3=1;
            gps_c[a] = gps_s[b];
            pc.printf("%d %c \n",a,gps_c[a]);

            if(gps_c[a] == '$')
              {
                gps_c[0] = '$';
                a=0;
                flag++;//1回目の＄は無視（flug=1を無視）
                pc.printf("%d %c  flag:%d gp:%d\n",a,gps_c[a],flag,gp);
              }
                 
            else if(gps_c[a-1] == '\r' && gps_c[a] == '\n' && flag == gp)
              {
                gps_c[a+1]='\0';
                flag = 10;
              }
              
              
            else if(a > 256)//aが256を超えたとき、ループを強制脱出(whileから)
              {   
                count_gps = 5;
                flag = 10;
              }

        }

        bit3=0;
        bit2=1;
        wait(0.5);

        if(memcmp(gps_c,"$GPRMC",6) == 0) {
            sscanf(gps_c,"$GPRMC,%f,%c,%f,N,%f,E,%f,%f,%f,%f,W",&utc_time,&pos_status,&latitude1,&longitude1,&speed,&course,&utc_date,&gps_magnetic);
            //latitude = strncpy(latitude1,latitude1,2);
            cal_l();
            pc.printf("$GPRMC,time[%f],%c,%f,N,%f,E,%f,%f,%f,%f,W \n",utc_time,pos_status,latitude1,longitude1,speed,course,utc_date,gps_magnetic);
            gp++;

            if(latitude1!=0) {
                bit1=1;
            }
            //mkdir("/sd/gps", 0777);

            if(local_on == 1) {
                FILE *fp = fopen("/local/gps.txt", "a");
                if(fp == NULL) {
                    error("Could not open file for write\n");
                }

                fprintf(fp,"$GPRMC,%f,%c,%f,N,%f,E,%f,%f,%f,%f,W \n",utc_time,pos_status,latitude1,longitude1,speed,course,utc_date,gps_magnetic);
                fclose(fp);
            }
            bit1=0;
            count_gps++;
            pc.printf("count_gps:%.1f\n",count_gps);
        }

        else if(memcmp(gps_c,"$GPGGA",6) == 0) {
            sscanf(gps_c,"$GPGGA,%f,%f,N,%f,E",&utc_time,&latitude1,&longitude1);
            cal_l();
            pc.printf("$GPGGA,time[%f],%f,N,%f,E \n",utc_time,latitude1,longitude1);
            gp++;

            if(latitude1!=0) {
                bit1=1;
            }
            // mkdir("/sd/gps", 0777);
            if(local_on == 1) {
                FILE *fp = fopen("/local/gps.txt", "a");
                if(fp == NULL) {
                    error("Could not open file for write\n");
                }

                fprintf(fp,"$GPGGA,%f,%f,N,%f,E \n",utc_time,latitude1,longitude1);
                fclose(fp);
            }
            bit1=0;
            count_gps++;
            pc.printf("count_gps:%.1f\n",count_gps);
        }

        else if(memcmp(gps_c,"$GPGLL",6) == 0) {
            sscanf(gps_c,"$GPGLL,%f,N,%f,E \n",&latitude1,&longitude1);
            cal_l();
            pc.printf("$GPGLL,%f,N,%f,E \n",latitude1,longitude1);
            gp++;

            if(latitude1!=0) {
                bit1=1;
            }
            //mkdir("/sd/gps", 0777);

            if(local_on == 1) {
                FILE *fp = fopen("/local/gps.txt", "a");
                if(fp == NULL) {
                    error("Could not open file for write\n");
                }

                fprintf(fp,"$GPGLL,%f,N,%f,E \n",latitude1,longitude1);
                fclose(fp);
            }
            bit1=0;
            count_gps++;
            pc.printf("count_gps:%.1f\n",count_gps);
        }
        
        else
        {
         count_gps++;
         gp++;
         pc.printf("gps_c is not match count_gps:%.1f\n",count_gps);
        }
        bit2=0;
    }


    return 0;
}

int offset(float x_rd,float y_rd)
{

    if(x_max == 0 || y_max == 0 || x_min == 0 || y_min == 0) {
        x_max = -1000.0;//x,yの最大値を初期化 +-32500
        y_max = -1000.0;
        x_min = 1000.0;
        y_min = 1000.0;
    }//最大値、最小値に0が入った時初期化

    if(x_max<x_rd && x_rd != 0) {
        x_max=x_rd;
    }

    if(y_max<y_rd && y_rd != 0) {
        y_max=y_rd;
    }

    if(x_min>x_rd && x_rd != 0) {
        x_min=x_rd;
    }

    if(y_min>y_rd && y_rd != 0) {
        y_min=y_rd;
    }
    
    if(count_offset > 300)
    {
    offset_x = (x_min+x_max)/2;
    offset_y = (y_min+y_max)/2;
    
    if(0 < offset_x)
     { x_dat = con_x + offset_x; }
    else if(0 > offset_x)
     { x_dat = con_x - offset_x; }
           
    if(0 < offset_y)
     { y_dat = con_y - offset_y; }
    else if(0 > offset_y)
     { y_dat = con_y + offset_y; }
    
    /*
    if(0 < offset_x)
     { x_dat = con_x + offset_x*1.3; }
    else if(0 > offset_x)
     { x_dat = con_x - offset_x*1.3; }
           
    if(0 < offset_y)
     { y_dat = con_y + offset_y*1.3; }
    else if(0 > offset_y)
     { y_dat = con_y - offset_y*1.3; }*/
    }
    
    pc.printf("x: %d , y: %d , x_dat: %f , y_dat: %f , x_max: %.5f , x_min: %.5f , direction: %f \n",con_x, con_y, x_rd, y_rd, x_max, x_min, direction);
    return 0;
}

void cal_gps()
{
    /*if(latitude_target > latitude1 && longitude_target > longitude1)//第１象限
     {quadrant = 1;}
    if(latitude_target > latitude1 && longitude_target < longitude1)
     {quadrant = 2;}
    if(latitude_target < latitude1 && longitude_target < longitude1)
     {quadrant = 3;}
    if(latitude_target < latitude1 && longitude_target < longitude1)
     {quadrant = 4;}*/
     
    y_target = (latitude_target-latitude1)*111319.49;
    x_target = (longitude_target-longitude1)*cos(latitude1*(3.1415926535/180))*111319.49;
    d_target = sqrt(pow(con_x,2.0)*pow(con_y,2.0));//目的地までの距離
    
    angle_caly = cos(latitude_target)*sin(longitude_target-longitude1);
    angle_calx = cos(latitude1)*(sin(latitude_target)-sin(latitude1))*cos(latitude_target)*cos(longitude_target-longitude1);
    sita = atan2(angle_caly,angle_calx);
    if(sita < 0)
     {sita = sita + 2*3.1415926535;}
    direction_target = sita*57.29578;
}

void cal_con()
{

    direction = atan2(y_dat,x_dat);
    if(direction < 0)
     {direction = direction + 2*3.1415926535;}
    direction = direction*57.29578; //ラジアンに変換
    //direction = (atan(y_cal/x_cal))*57.29578;

    if(0 < direction && 89 > direction){
        angle_c = 'N';}

    else if(90 < direction && 179 > direction){
        angle_c = 'E';}

    else if(180 < direction && 269 > direction || -180 < direction && -91 > direction){
        angle_c = 'S';}

    else if(270 < direction && 359 > direction || -90 < direction && -1 > direction){
        angle_c = 'W';}

    else{ angle_c = '?';}

}

void trigger()
{
    //trg = 1;
    bit2 = 1;
    wait_us(10);
    //trg = 0;
    bit2 = 0;
}

void rise_echo()//立ち上がり割り込み
{   
    //sig=1;
    ActiveTime.start();
    __disable_irq();   
}

void fall_echo()//落ち込み割り込み
{
    //sig=0;
    ActiveTime.stop();
    if(ActiveTime.read_us() > 0.00002){
                   sig_time = ActiveTime.read_us()/6.169;}
    ActiveTime.reset();
    
    if(local_on == 1){
        bit1=1;
        FILE *fp = fopen("/local/ultra.txt","a");
                   fprintf(fp,"%f\n",sig_time);
                   fclose(fp);
        bit1=0;}
}


void first_sequence()
{
    if(count_move==0)
     {count_move=1;}
}

void turn_sequence()
{   
    __disable_irq();
    pc.printf("************turn_sequence*************\n");
    pc.printf("direction: %f  direction_target: %f\n",direction,direction_target);
    longitudegosa = longitude_target-longitude1;
    latitudegosa = latitude_target-latitude1;
                
    if(direction > direction_target+2.0)
      { while(direction > direction_target+2.0)
        {
          pc.printf("turn clockwise \ndirection: %f  direction_target: %f\n",direction,direction_target);
                   
          FILE *fp = fopen("/local/turn.txt","a");
          fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
          fclose(fp);
                   
          mag.getX(&con_x);//コンパス読み込み
          mag.getY(&con_y);
          offset(con_x,con_y);//x,y オフセット計算
          cal_con();
          
          motor_l.pulsewidth(motor_palse*0.7);
          motor_r.pulsewidth(motor_palse*0.35);
          }}
                   
   else if(direction < direction_target-2.0)
     { while(direction < direction_target-2.0)
       {
          pc.printf("turn anti clockwise \ndirection: %f  direction_target: %f\n",direction,direction_target);
                   
          FILE *fp = fopen("/local/turn2.txt","a");
          fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
          fclose(fp);
                   
          mag.getX(&con_x);//コンパス読み込み
          mag.getY(&con_y);
          offset(con_x,con_y);//x,y オフセット計算
          cal_con();
          
          motor_r.pulsewidth(motor_palse*0.7);
          motor_l.pulsewidth(motor_palse*0.35);
          }}
    
        motor_r.pulsewidth(motor_palse*0.8);
        motor_l.pulsewidth(motor_palse*0.8);
                 
        pc.printf("direction: %f  direction_target: %f\n",direction,direction_target);

        pc.printf("************turn_sequence end*************\n");
    
}

void move()
{
    switch(count_move)
    {
        case 1: 
                
                pc.printf("*************case1**************\n");
                motor_l.pulsewidth(0.01);  //パルス幅
                motor_r.pulsewidth(0.01);
                wait(10);//落下時間
                
                servo.pulsewidth(servo_palse);
                   
                /*if(direction <= direction_target+3 && direction >= direction_target-3)
                 {count_move++;}*/
                count_move++;
                pc.printf("*************case1 end**************\n"); 
                break;
                
        case 2:
                __disable_irq();
                pc.printf("*************case2**************\n");
                pc.printf("direction: %f  direction_target: %f\n",direction,direction_target);
                longitudegosa = longitude_target-longitude1;
                latitudegosa = latitude_target-latitude1;
                
                if(direction > direction_target+2.0)
                 { while(direction > direction_target+2.0)
                    {
                   pc.printf("turn clockwise \ndirection: %f  direction_target: %f\n",direction,direction_target);
                   
                   FILE *fp = fopen("/local/turn.txt","a");
                   fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
                   fclose(fp);
                   
                   mag.getX(&con_x);//コンパス読み込み
                   mag.getY(&con_y);
                   offset(con_x,con_y);//x,y オフセット計算
                   cal_con();
                   
                   /*gps_read();
                   gps_save();
                   cal_gps();回転しすぎる*/
                   motor_l.pulsewidth(motor_palse*0.7);
                   motor_r.pulsewidth(motor_palse*0.35);}
                   motor_r.pulsewidth(motor_palse);
                   motor_l.pulsewidth(motor_palse);
                   }
                   
                else if(direction < direction_target-2.0)
                 { while(direction < direction_target-2.0)
                    {
                   pc.printf("turn anti clockwise \ndirection: %f  direction_target: %f\n",direction,direction_target);
                   
                   FILE *fp = fopen("/local/turn2.txt","a");
                   fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
                   fclose(fp);
                   
                   mag.getX(&con_x);//コンパス読み込み
                   mag.getY(&con_y);
                   offset(con_x,con_y);//x,y オフセット計算
                   cal_con();
                   
                   /*gps_read();
                   gps_save();
                   cal_gps();回転しすぎる*/
                   motor_r.pulsewidth(motor_palse*0.7);
                   motor_l.pulsewidth(motor_palse*0.35);}
                   motor_r.pulsewidth(motor_palse);
                   motor_l.pulsewidth(motor_palse);
                   }
                   
                if(direction <= direction_target+2.0 && direction >= direction_target-2.0)
                 { pc.printf("front \n");
                   motor_l.pulsewidth(motor_palse);  //パルス幅
                   motor_r.pulsewidth(motor_palse);
                   count_move++;}
                 
                pc.printf("direction: %f  direction_target: %f\n",direction,direction_target);

                pc.printf("*************case2 end**************\n");
                
                break;
        case 3:
                motor_l.pulsewidth(0.0);  //パルス幅
                motor_r.pulsewidth(0.0);
                
                gps_read();
                gps_save();
                cal_gps();
                
                mag.getX(&con_x);//コンパス読み込み
                mag.getY(&con_y);
                offset(con_x,con_y);//x,y オフセット計算
                cal_con();
                
                turn_sequence();
                
                motor_l.pulsewidth(motor_palse*0.8);  //パルス幅
                motor_r.pulsewidth(motor_palse*0.8);
                
        
                longitudegosa = longitude_target-longitude1;
                latitudegosa = latitude_target-latitude1;
                
                if( abs(longitudegosa)<=0.1 && abs(latitudegosa)<=0.1 )
                 { pc.printf("front \n");
                   count_move++; }
                  
                break;        
        case 4:
                pc.printf("*************case3**************\n");
                
                __enable_irq();
                trigger();
                //distance.rise(rise_echo);
                //distance.fall(fall_echo);
                
                pc.printf("sig_time: %f\n",sig_time);
                
                if(sig_time <= 100.0)//100mm以下に近づいたとき停止
                 { pc.printf("sig_time is 100mm or under: %f\n",sig_time);
                   count_move++;}
                else
                 {
                  turn_sequence();
                  motor_l.pulsewidth(motor_palse/2);  //パルス幅
                  motor_r.pulsewidth(motor_palse/2);
                 }
                 
                break;
                
        default:
                motor_l.pulsewidth(0.0);
                motor_r.pulsewidth(0.0);
                
                break;
        
    }
    
}


void con_save()
{

    bit1=1;//led4を1にしてデータを保存
    FILE *fp = fopen("/local/commpas_new.txt","a");
    fprintf(fp,"%c,%f,%f,%f,%f\n",angle_c,d_target,x_dat,y_dat,direction);
    //fprintf(fp,"%d,%d,%d,%d,%d\n",x,y,z,offset_x,offset_y);
    fclose(fp);
    bit1=0;
}

void dele()
{

    bit2=1;//led3を1

    remove("/local/commpas_new.txt");
    remove("/local/gps.txt");

    bit2=0;
}

int main()
{
    bit1 = 1;
    pc.printf("start\n");
    //__disable_irq();//割り込み禁止
    init(); //初期化
    //cds.rise(first_sequence);//cds立ち上がり割り込み
    cds.fall(&first_sequence);//cds落ち込み割り込み
    mag.sampleRate(0x40);
    mag.enable();//コンパス有効化
    mag.getX(&con_x);//コンパス読み込み
    mag.getY(&con_y);
    mag.getZ(&con_z);
    
    motor_r.pulsewidth(motor_palse*0.7);
    motor_l.pulsewidth(motor_palse*0.35);
    
    for(count_offset=0; count_offset < 300; count_offset++)//300個分のサンプ取得
     {
      mag.getX(&con_x);//コンパス読み込み
      mag.getY(&con_y);
      mag.getZ(&con_z);
      offset(con_x,con_y);
      cal_con();
      
      if(local_on2==1)
        {
           FILE *fp = fopen("/local/first_con.txt","a");
                      fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
                      fclose(fp);
        }
      }
    motor_r.pulsewidth(motor_palse*0.0);
    motor_l.pulsewidth(motor_palse*0.0);
    bit1 = 0;
    
    while(1) {
        
                __enable_irq();//割り込み許可
                mag.getX(&con_x);//コンパス読み込み
                mag.getY(&con_y);
                offset(con_x,con_y);//x,y オフセット計算
                cal_con();
                
                gps_read();
                gps_save();
                cal_gps();
                
                /*if(cds == 0)
                 {first_sequence();
                  bit5=1;}*/

                  move();
                
                if(local_on == 1)
                 { con_save(); }  //pin20がhighの時、データ保存
                /*if(local_del == 1)
                   { dele(); }*/  //pin19がhighの時、保存したデータを消去
                
             }
}