File content as of revision 22:5cbebf097600:

//計器プログラム

#include "mbed.h"
#include "rtos.h"
#include "Fusokukei.h"
#include "MPU6050.h"
#include "SDFileSystem.h"
#include "BufferedSoftSerial.h"
#include "Cadence.h"

#define KX_VALUE_MIN 0.4
#define KX_VALUE_MAX 0.8
#define SOUDA_DATAS_NUM 12
#define WRITE_DATAS_NUM 20
#define MPU_LOOP_TIME 0.01
#define AIR_LOOP_TIME 0.01
#define WRITE_DATAS_LOOP_TIME 1
#define ROLL_R_MAX_DEG 2
#define ROLL_L_MAX_DEG 2
#define SD_WRITE_NUM 10
#define INIT_SERVO_PERIOD_MS 20

Cadence cadence(p13,p14);
//Ticker cadenceTicker;

RawSerial pc(USBTX,USBRX);
RawSerial Android(p13,p14);
BufferedSoftSerial twe(p9,p10);
BufferedSoftSerial soudaSerial(p17,p18);
//Ticker writeDatasTicker;
Timer writeTimer;

InterruptIn FusokukeiPin(p21);
Ticker FusokukeiTicker;
Fusokukei air;
volatile int air_kaitensu= 0;

MPU6050 mpu6050;
Timer t;
Ticker mpu6050Ticker;

AnalogIn kx_X(p17);
AnalogIn kx_Y(p16);
AnalogIn kx_Z(p15);
float KX_X,KX_Y,KX_Z;

DigitalOut RollAlarmR(p20);
DigitalOut RollAlarmL(p19);
DigitalOut led(LED1);
DigitalOut led2(LED2);

SDFileSystem sd(p5, p6, p7, p8, "sd");
FILE* fp;

PwmOut kisokuServo(p26);
PwmOut geikakuServo(p22);

char soudaDatas[SOUDA_DATAS_NUM];
float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
volatile int write_datas_index = 0;

void cadenceDataReceive();
void air_countUp();
void call_calcAirSpeed();
void init();
void FusokukeiInit();
void SdInit();
void SDprintf(void const *argument);
void DataReceiveFromSouda();
void WriteDatas();
float calcAttackAngle();
float calcKXdeg(float x);

void cadenceDataReceive(){
//    cadence.readData();
}

void air_countUp(){
    air_kaitensu++;
}

void call_calcAirSpeed(){
    air.calcAirSpeed(air_kaitensu);
    air_kaitensu = 0;
}

void init(){
    twe.baud(19200);
    Android.baud(9600);
    soudaSerial.baud(9600);
    kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
    geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
    FusokukeiInit();
    mpu6050.MPUInit(t); 
    SdInit();
}

void FusokukeiInit(){
    FusokukeiPin.rise(air_countUp);
    FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
}

double calcPulse(int deg){
    return (0.0006+(deg/180.0)*(0.00235-0.00045));
}

void SdInit(){
    mkdir("/sd/mydir", 0777);
    fp = fopen("/sd/mydir/sdtest2.csv", "w");
    if(fp == NULL) {
        error("Could not open file for write\n");
    }
    fprintf(fp, "Hello fun SD Card World!\n\r");
    fclose(fp);
    Thread sd_thread(&SDprintf);
}

void DataReceiveFromSouda(){
    led2 = !led2;
    for(int i = 0; i < SOUDA_DATAS_NUM; i++){
        if(soudaSerial.readable()) {
            soudaDatas[i] = (char)soudaSerial.getc();
            if(soudaDatas[i]==';') i=-1;
        }else i--;
    }
}

void SDprintf(void const *argument){
    while(1){
    if(write_datas_index == SD_WRITE_NUM-1){
        fp = fopen("/sd/mydir/sdtest.csv", "a");
        if(fp == NULL) {
            error("Could not open file for write\n");
        }
        for(int i = 0; i < SD_WRITE_NUM; i++){
            for(int j = 0; j < WRITE_DATAS_NUM; j++){
                fprintf(fp,"%f,", writeDatas[i][j]);
            }
        }
        fprintf(fp,"\n\r");
        fclose(fp);
        write_datas_index=0;
    }
    Thread::wait(1);
    }
}

void WriteDatas(){
    int i;
    for(i = 0; i < SOUDA_DATAS_NUM; i++){
        //writeDatas[write_datas_index][i] = 0.0;    
        writeDatas[write_datas_index][i] = (float)soudaDatas[i];
    }
//    writeDatas[write_datas_index][i++] = cadence.cadence;
    writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read());
    writeDatas[write_datas_index][i++] = calcKXdeg(kx_Y.read());
    writeDatas[write_datas_index][i++] = calcKXdeg(kx_Z.read());
    writeDatas[write_datas_index][i++] = pitch;
    writeDatas[write_datas_index][i++] = roll;
    writeDatas[write_datas_index][i++] = yaw;
    writeDatas[write_datas_index][i++] = airSpeed;
    //writeDatas[write_datas_index][i++] = writeTimer.read();
    //for(i = 0; i < WRITE_DATAS_NUM; i++){
//        pc.printf("%f   ", writeDatas[write_datas_index][i]);
//        twe.printf("%f,", writeDatas[write_datas_index][i]);
//    }
//    pc.printf("\n\r");
//    twe.printf("\n\r");
    if(write_datas_index == SD_WRITE_NUM-1){
//        SDprintf();
        write_datas_index=0;
    }
    else{
        write_datas_index++;
    } 
    for(int i = 0; i < SOUDA_DATAS_NUM; i++){
        pc.printf("%i   ",soudaDatas[i]);
        twe.printf("%i,",soudaDatas[i]);
    }
//    twe.printf("%f\n\r",cadence.cadence);
//    pc.printf("%f\n\r",cadence.cadence);
    //pc.printf("\n\r");

if(Android.writeable()){
    Android.printf("%f,%f,%f",airSpeed,roll,0);
}
    twe.printf("%f,%f,%f,",pitch,roll,yaw);
    twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
    twe.printf("%f,\r\n",airSpeed); 
    pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
    //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
    pc.printf("%f\n\r",airSpeed);
    //SDprintf();
}

void WriteDatasF(){
    pc.printf("airSpeed:%f\n\r",airSpeed);
}

float calcKXdeg(float x){
    return -310.54*x+156.65;
}

float calcAttackAngle(){
    return pitch-calcKXdeg(kx_Z.read());
}

void RollAlarm(){ 
    if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
        RollAlarmL = 1;
    }
    else{
        RollAlarmL = 0;
    }
    if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){
        RollAlarmR = 1;
    }
    else{
        RollAlarmR = 0;
    }
}

void WriteServo(){
    kisokuServo.pulsewidth(calcPulse(9*airSpeed));
    if(pitch<0){
        geikakuServo.pulsewidth(calcPulse(0));
    }
    else{
        geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0)));
    }
}

int main(){
    init();
    while(1){
        pc.printf("test\n\r");
        mpu6050.mpuProcessing(t);
        RollAlarm();
        DataReceiveFromSouda();
//        cadence.readData();
        WriteDatas();
        WriteServo();
    }
}