File content as of revision 2:9f8fa9035357:

//GPS GT-720F Test05


#include "mbed.h"
#include "TextOLED.h"
#include "SDFileSystem.h"
#include "stdio.h"


#define ON 1
#define OFF 0


AnalogIn alt(p20);        //高度計読み取り
InterruptIn speedcount(p17); //機速用のフォトインタラプタ読み取り
InterruptIn rpmcount(p21);  //回転数用のフォトインタラプタ読み取り
InterruptIn Log_Switch(p26);//ログスイッチ読み取り


Serial pc(USBTX, USBRX);
Serial control_com(p28, p27);//制御基板との通信用
Serial android_com(p9, p10);               //Androidとの通信用txrx!!

Serial gps(p13,p14);// tx, rx

TextOLED pilot_oled(p11, p12 ,p30 ,p29 ,p15, p16); // RS, E, DB4, DB5, DB6, DB7  有機EL!!
SDFileSystem sd(p5, p6, p7, p8, "sd");//SDpinの番号 3,7,5,2 //1pin--Vout,4pin--Vout,6pin--GND,8pin--Vout


DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);


//音システム用
DigitalOut soundsystem1(p19);
DigitalOut soundsystem2(p24);
DigitalOut soundsystem3(p25);

DigitalOut option(p22);//toGPS and jailo


//タイマー割り込み宣言
Ticker ticker;//updateのタイマー割り込み
Timer t,t1;

int speedcounter = 0, rpmcounter = 0, min = 0, sec = 0, i;
int oldspeed = 0, oldrpm = 0;
int log_count = 0;
int SD_count = 0;
int up2=0,up1=0,down1=0,down2=0,E_neut=0;
int left1=0,left2=0,right1=0,right2=0,R_neut=0;
int send_compass=0;


double time_s = 0,time_ms=0,time1=0,time2=0, speed = 0, rpm = 0, height = 0,compass = 0;

char ele;
char rud;
char e_trim='e',r_trim='r';
char compass_array[4] = "000";
char data;
char send_buff1[24]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};


unsigned char Log_flag = OFF;
unsigned char SD_flag = OFF;
      
      

//プロトタイプ宣言
//void init();
//void myled(int a,int b,int c,int d);
void SD_log();
void OLEDdisplay_print();
void spd_f();
void rpm_f();
void spd_c();
void rpm_c();
void control_communication();
void android_communication();
void update();
void altitude_f();
void compass_f();
void Log_Switch_f();
void CGRAM_1();
void CGRAM_2();
void CGRAM_3();
void CGRAM_4();
void CGRAM_5();


//=======================================================================//



LocalFileSystem local("local");


FILE *fp;

int main() {

      char c;
      int i3,rlock,stn;
      char gps_data[256];
      char ns,ew;
      float time,hokui,tokei;
      float g_hokui,g_tokei;
      float d_hokui,m_hokui,d_tokei,m_tokei;
 
      int h_time=0,m_time=0,s_time=0;
      
 //mkdir("/sd/gps_data", 0777);//まずsdのフォルダをつくる。
 
      gps.baud(9600);

  pc.printf("Start TBT!!\n");

     led1=1;
    led2=1;
    led3=1;
    led4=1;
    wait(0.01);
    pilot_oled.cls();
    wait(0.001);
    CGRAM_1();
    CGRAM_2();
    CGRAM_3();
    CGRAM_4();
    CGRAM_5();
//    myled(1,1,0,0);
    wait(2);
   mkdir("/sd/mydir", 0777);
    android_com.baud(115200);   
    android_com.format(8,Serial::None,1); 
    
    control_com.attach(control_communication, Serial::RxIrq);
    control_com.baud(115200); 

    t.start();
    t1.start();

    Log_Switch.rise(&Log_Switch_f);
    speedcount.rise(&spd_c);
    rpmcount.rise(&rpm_c);
    ticker.attach(update,1);//1s更新
    //time1 = t1.read_ms();
    


    while (1) {
      
            
//        pc.printf("There is TBT!!\n");
        led3=!led3;
        if(min >=30) { //30分経過時、タイマーリセット
            t.reset();
        }
        if(Log_flag == OFF) { //log取るごとにリセット
            t.stop();
            t.reset();
        } else {
            t.start();
        }
        time_s = t.read();//時間取得
        min = (int)(time_s) / 60;
        sec = (int)time_s % 60;
        wait(0.0002);
    
    
      i3=0;
      while(gps.getc()!='$'){
      }
      
      while( (gps_data[i3]=gps.getc()) != '\r'){
        i3++;
        if(i3==256){
         
           i3=255;
           break;
         }
      }
      gps_data[i3]='\0';
      
      //test
      /* Test data
       rlock=1;
       hokui=3532.25024; //=>35.537502
       tokei=13751.86820;//=>137.864471
      */  
      if( sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d",&time,&hokui,&ns,&tokei,&ew,&rlock,&stn) >= 1){
        if(rlock >= 1){
          
          //time set
          h_time=int(time/10000);
          m_time=int((time-h_time*10000)/100);
          s_time=int(time-h_time*10000-m_time*100);
          h_time=h_time+9;//UTC =>JST
          
          //hokui
          d_hokui=int(hokui/100);
          m_hokui=(hokui-d_hokui*100)/60;
          g_hokui=d_hokui+m_hokui;
          //tokei
          d_tokei=int(tokei/100);
          m_tokei=(tokei-d_tokei*100)/60;
          g_tokei=d_tokei+m_tokei;
          //g_hokui=int(hokui/100)+(hokui-int(hokui/100))/60;
          //g_tokei=int(tokei/100)+(tokei-int(tokei/100))/60;
        
          
          //==================================================//
         
         pc.printf("%2d,%2d,%2d,%4.6f,,%3.6f,\r\n",h_time,m_time,s_time,g_tokei,g_hokui);
         
     //    fp = fopen("/sd/gps_data/log.txt", "a");
    
        //  fprintf(fp,"%2d,%2d,%2d,%4.6f,,%3.6f,\r\n",h_time,m_time,s_time,g_tokei,g_hokui);
       // fprintf(fp,"%4.6f,%3.6f,\r\n",g_tokei,g_hokui);
       //   fclose(fp);
          
          //==================================================//
          
          
         
        //}
        }
      }//if
    }//while
}//main


void SD_log()   //SDログ関数
{
    //  __disable_irq();
    led2 = 1;
    if (Log_flag == ON) {
        FILE *fp;
        /*
         fp = fopen("/sd/log.txt", "a");
    
         fprintf(fp,"%2d,%2d,%2d,%4.6f,,%3.6f,\r\n",h_time,m_time,s_time,g_tokei,g_hokui);
        fprintf(fp,"%4.6f,%3.6f,\r\n",g_tokei,g_hokui);
          fclose(fp);   
          */
          
        fp = fopen("/sd/mydir/TP.txt", "a");
        
        if (NULL == fp) {
            pilot_oled.locate(0, 0);
            pilot_oled.printf("There is no SD!!");
            pc.printf("There is no SD!!\n");
            error("Could not open file for write \n");
        } else {
            fprintf(fp, "NO:%d,%02d:%02d,%1.0f,%0.3f,%0.3f,%0.3f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",
                         SD_count, min, sec, rpm, speed, height,compass,up2,up1,E_neut,down1,down2,left2,left1,R_neut,right1,right2);
                   //      fprintf(fp,"%4.6f,;%3.6f,;\r\n",g_tokei,g_hokui);
            fclose(fp);     
                                                                                                                //操舵データ
        }
    }
    led2 = 0;
    //   __enable_irq();
}

void Log_Switch_f()//ログスイッチ
{
    log_count++;
    if (log_count == 1) {
        Log_flag = ON;
        SD_count++;
    } else if (log_count == 2) {
        Log_flag = OFF;
        log_count = 0;
    }
}

void OLEDdisplay_print()  //有機EL表示用関数
{
    led3!=led3;
    //__disable_irq();
/*    if(Log_flag == OFF) {
        wait_ms(0.7);
        pilot_oled.locate(0, 0);
        pilot_oled.printf("log");//時間表示
        pilot_oled.locate(3, 0);
        pilot_oled.putc(0xB5);//オ
        pilot_oled.locate(4, 0);
        pilot_oled.putc(0xBE);//セ
        pilot_oled.locate(5, 0);
        pilot_oled.putc(0x21);//！
    } else {
        pilot_oled.locate(0, 0);
        pilot_oled.printf("%02d:%02d ", min, sec);//時間表示
    }
    wait_ms(0.7);*/
    pilot_oled.locate(0, 1);
    pilot_oled.printf("r%3.0f s%4.1f h%4.1f", rpm,speed,height);//センサーの値
    wait_ms(0.7);

    pilot_oled.locate(4, 0);
    pilot_oled.printf("%1d", send_compass);
    pilot_oled.locate(11, 0);
    if (Log_flag == ON) pilot_oled.printf("ON ");
    else pilot_oled.printf("OFF");
    
//        pilot_oled.locate(0, 0);
  //      pilot_oled.printf("count:%d",rpmcounter);//時間表示
    wait_ms(0.7);
// __enable_irq();
}

void compass_f()//高度の変換
{
    compass=atoi(compass_array);
    
    if(compass<22.5&&compass>=0||compass<360&&compass>=337.5)send_compass=1;//北
    else if(compass<67.5&&compass>=22.5)send_compass=2;//
    else if(compass<112.5&&compass>=67.5)send_compass=3;//東
    else if(compass<157.5&&compass>=112.5)send_compass=4;//
    else if(compass<202.5&&compass>=157.5)send_compass=5;//南
    else if(compass<247.5&&compass>=202.5)send_compass=6;//
    else if(compass<292.5&&compass>=247.5)send_compass=7;//西
    else if(compass<337.5&&compass>=292.5)send_compass=8;//

        
    
}

void altitude_f()//高度の変換
{
    height=alt.read()*3.3*1000/1.6/100;
}

void rpm_c()  //回転数のフォトインタラプタ
{
    rpmcounter++;
}

void rpm_f()  //回転数計測
{
//    rpm = (double)(rpmcounter * 60000/time_ms/48);
    rpm=(double)(rpmcounter);
    rpmcounter = 0;
}

void spd_c()  //機速のフォトインタラプタ
{
    speedcounter++;
}

void spd_f()
{
    int x = 0;
    x = (double)(speedcounter * 60000/time_ms/8);
    speed = (double)(0.0014*x + 0.9815);
    //最初の機速校正時の式は、0.0015*x + 0.6063       
    if(speed<=0.9815) {
        speed = 0;
    }
    speedcounter = 0;
}


void control_communication()  //操舵基板との通信用
{
//    myled(9,9,1,9);

    // __disable_irq();
    // t1.stop();
    for (int i = 0; i <3; i++) {
        compass_array[i] = control_com.getc();//高度の受信
    }
    ele = control_com.getc();//舵角の受信
    rud = control_com.getc();//舵角の受信
    e_trim = control_com.getc();//trimの受信
    r_trim = control_com.getc();//trimの受信
    //////////////ER操舵データ表示////////////////////
    /*   pilot_oled.locate(6, 0);
       pilot_oled.printf("%c", ele);
       wait_ms(0.7);
       pilot_oled.locate(8, 0);
       pilot_oled.printf("%c", rud);
       wait_ms(0.7);*/
    pilot_oled.locate(0, 0);
    pilot_oled.printf("%1c%1c", e_trim,r_trim);
    pilot_oled.locate(14, 0);
    pilot_oled.printf("%2d", SD_count);
    wait_ms(0.7);
    ///////////エレベータログデータ条件分岐////////////////////////////
    switch(ele) {
        case 'U':
            pilot_oled.locate(6, 0);
            pilot_oled.putc(0x02);//
//            pilot_oled.printf("U");//
            up2 = 5 ;
            break;
        case 'u':
            pilot_oled.locate(6, 0);
            pilot_oled.putc(0x5E);//^
//            pilot_oled.printf("u");//
            up1 = 4 ;
            break;
        case 'n':
            pilot_oled.locate(6, 0);
            pilot_oled.putc(0x2D);//-
            E_neut = 3 ;
            break;
        case 'd':
            pilot_oled.locate(6, 0);
            pilot_oled.putc(0x00);//vみたいな文字
//            pilot_oled.printf("d");//
            down1 = 2 ;
            break;
        case 'D':
            pilot_oled.locate(6, 0);
            pilot_oled.putc(0x01);
//            pilot_oled.printf("D");//
            down2 = 1;
            break;
        default:
            break;
    }
    /////////ラダーログデータ条件分岐/////////////////////////////////////
    switch(rud) {
        case 'L':
            pilot_oled.locate(8, 0);
            pilot_oled.putc(0x03);
//            pilot_oled.printf("L");//
            left2 =1;
            break;
        case 'l':
            pilot_oled.locate(8, 0);
            pilot_oled.putc(0x3C);//<
            left1 =2;
            break;
        case 'N':
            pilot_oled.locate(8, 0);
            pilot_oled.putc(0x7C);//|
            R_neut=3;
            break;
        case 'r':
            pilot_oled.locate(8, 0);
            pilot_oled.putc(0x3E);//>
            right1=4;
            break;
        case 'R':
            pilot_oled.locate(8, 0);
            pilot_oled.putc(0x04);
//             pilot_oled.printf("R");//
           right2=5;
            break;
        default:
            break;
    }
    //  t1.start();
    //__enable_irq();
}

void android_communication()
{
//    myled(9,9,9,1);

    int srpm,sspeed,sheight;
    srpm=(int)rpm;
    sspeed=(int)(speed*10);
    sheight=(int)(height*10);    
 //   if(srpm<100)srpm+=900;
 //   if(sspeed<100)sspeed+=900;
//    if(sheight<100)sheight+=900;
    
    snprintf(send_buff1,10,"r%xh%xv%x",srpm,sheight,sspeed);    
    for(i=0;i<10;i++){
//        pc.putc(send_buff1[i]); 
        android_com.putc(send_buff1[i]);         
    }
}

void update()   //データ更新。最優先関数、割り込み禁止絶対入れろ。
{
//    myled(1,0,0,0);
    __disable_irq();
    //time2 = t1.read_ms();
    time_ms = t1.read_ms();//周期時間読み取り
    t1.stop();
    t1.reset();
    led4 =! led4;
    //time_ms =time2-time1;
    rpm_f();
    spd_f();
    altitude_f();
    compass_f();
   SD_log();
    OLEDdisplay_print();
    android_communication();
    pc.printf("NO:%d,%02d:%02d,R:%1.0f,S:%0.3f,H:%0.3f,%c%c%c,compass:%3.0f,%c%c,e:%c,r:%c,et:%c,rt:%c,%.3f\r\n", SD_count, min, sec, rpm, speed, height,send_buff1[4],send_buff1[5],send_buff1[6],compass,compass_array[0],compass_array[1], ele, rud,e_trim,r_trim,time_ms);
//    pc.printf("TBT\n");
    up2 = 0;
    up1 = 0;
    E_neut = 0;
    down1=0;
    down2=0;
    left1= 0;
    left2 =0;
    R_neut = 0;
    right1=0;
    right2=0;
    t1.start();
    // time1 = t1.read_ms();
    __enable_irq();
}


void CGRAM_1()
{
    // CGRAMアドレスの指定
    // CGRAM(1)にビット・パターンを登録します。
    pilot_oled.writeCommand(0x40) ;
    // 40μ秒待ちます。
    wait_us(40);          

    // ここから文字のビット・パターンを1行ずつ設定していきます。
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x11) ;//10001
    pilot_oled.writeData((int)0x0A) ;//01010
    pilot_oled.writeData((int)0x04) ;//00100
    wait_us(40);
    pilot_oled.cls();
}

void CGRAM_2()
{
    // CGRAMアドレスの指定
    // CGRAM(2)にビット・パターンを登録します。
    pilot_oled.writeCommand(0x48) ;
    // 40μ秒待ちます。
    wait_us(40);          

    // ここから文字のビット・パターンを1行ずつ設定していきます。
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x1F) ;//11111
    pilot_oled.writeData((int)0x0E) ;//01110
    pilot_oled.writeData((int)0x04) ;//00100
    wait_us(40);
    pilot_oled.cls();
}

void CGRAM_3()
{
    // CGRAMアドレスの指定
    // CGRAM(3)にビット・パターンを登録します。
    pilot_oled.writeCommand(0x50) ;
    // 40μ秒待ちます。
    wait_us(40);          

    // ここから文字のビット・パターンを1行ずつ設定していきます。
    pilot_oled.writeData((int)0x04) ;//00100
    pilot_oled.writeData((int)0x0E) ;//01110
    pilot_oled.writeData((int)0x1F) ;//11111
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x00) ;
    pilot_oled.writeData((int)0x00) ;
    wait_us(40);
    pilot_oled.cls();
}

void CGRAM_4()
{
    // CGRAMアドレスの指定
    // CGRAM(4)にビット・パターンを登録します。
    pilot_oled.writeCommand(0x58) ;
    // 40μ秒待ちます。
    wait_us(40);          

    // ここから文字のビット・パターンを1行ずつ設定していきます。
    pilot_oled.writeData((int)0x02) ;//00010
    pilot_oled.writeData((int)0x06) ;//00110
    pilot_oled.writeData((int)0x0E) ;//01110
    pilot_oled.writeData((int)0x1E) ;//11110
    pilot_oled.writeData((int)0x0E) ;//01110
    pilot_oled.writeData((int)0x06) ;//00110
    pilot_oled.writeData((int)0x02) ;//00010
    pilot_oled.writeData((int)0x00) ;//00000
    wait_us(40);
    pilot_oled.cls();
}

void CGRAM_5()
{
    // CGRAMアドレスの指定
    // CGRAM(5)にビット・パターンを登録します。
    pilot_oled.writeCommand(0x60) ;
    // 40μ秒待ちます。
    wait_us(40);          

    // ここから文字のビット・パターンを1行ずつ設定していきます。
    pilot_oled.writeData((int)0x08) ;//01000
    pilot_oled.writeData((int)0x0C) ;//01100
    pilot_oled.writeData((int)0x0E) ;//01110
    pilot_oled.writeData((int)0x0F) ;//01111
    pilot_oled.writeData((int)0x0E) ;//01110
    pilot_oled.writeData((int)0x0C) ;//01100
    pilot_oled.writeData((int)0x08) ;//01000
    pilot_oled.writeData((int)0x00) ;//00000
    wait_us(40);
    pilot_oled.cls();
}