File content as of revision 8:9059f95fc498:

#include "mbed.h"
#include "ADXL345_I2C.h"
#include <stdio.h>
#include <stdlib.h>

void savedata(void);
void flipB(void);
void ADXL_config(void);
void read_current(double *C);
void controller(double *v,float *duty_output,double *acce_device, double *acce_output, double *F);
void makefile(void);
void weight_init(void);
void INA226config(void);

DigitalOut dir(p25);
PwmOut duty(p24);
I2C i2c(p28, p27);

ADXL345_I2C accelerometer(p9, p10);
Serial debug(USBTX,USBRX);
//QEI wheel (p21, p22, p23, 1000);

DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);
DigitalIn  sensorA(p17);
DigitalIn  sensorB(p18);
DigitalIn  sensorC(p19);
DigitalIn  sensorD(p20);
InterruptIn sensorB_(p18);

int flag = 0;
int flipB_ = 0;
Ticker save;
FILE *fp;
Timer t;
int intrrupt_cnt = 0;

static char i2c_addr = 0x80;



int main()
{
    char buffer[100][150];
    int i,j;
    double C;
    float duty_output;
    int loop_break;
    double acce_device;
    double acce_output;
    int readings[3] = {0, 0, 0};
    double v;
    double F_output;

    myled1 = 1; //起動の確認
    ADXL_config();
    myled2 = 1;
//    makefile();
    myled3 = 1;
    INA226config();
    myled4 = 1;
    wait(3.0);
    myled1 = 0;
    myled2 = 0;
    myled3 = 0;
    myled4 = 0;


//---PWNの周期を設定---
    duty_output = 0.6;
    duty.period(0.00005);
//    NVIC_SetPriority(0, 1);


/////////////////////
//---制御開始---
/////////////////////
    while(1) {
        t.reset();
        i = 0;
        loop_break = 0;
        debug.printf("writing OK!\n");

        weight_init(); //慣性力発生用おもりを初期位置に移動させる

//---加速度を感知したら移動開始---
        flipB_ = 0;
        dir = 1;

        while (1) {
            accelerometer.getOutput(readings);
            if( abs((int16_t)readings[0]) > 25) {
                myled3 = 1;
                break;
            }
        }
        save.attach(&savedata, 0.01 ); //割り込み開始
        t.start();
        
        duty.write(duty_output);


        while(t.read() <= 0.80) { //内部タイマーが0.80秒に達するまで制御
            if(flag) {
                flag = 0;
                controller( &v, &duty_output, &acce_device, &acce_output, &F_output);
                read_current( &C);
                if(1) {
                    sprintf(buffer[i],"%f10,%f10,%f10,%f10,%f10,%f10",t.read(),C,acce_device,C,duty_output,F_output); //バッファにデータを溜めこみ終了時に吐き出す．
                    if( i > 299) {
                        loop_break = 1;
                        break;
                    }
                    i = i + 1;
                }

            }
            if(loop_break) {
                break;
            }

        }
        
        myled2 = 1; //0.80秒経過したらLED2を光らせる
        t.stop();
        save.detach();


        duty = 0.0f;

        myled1 = 1;
        for( j = 0; j < i; j++) {
            debug.printf("%s\n",buffer[j]);//シリアル通信でバッファにあるデータを相手に送信する
        }
        
        while(1) {
            myled1 = 1;
            wait(0.2);
            myled1 = 0;
            wait(0.2);
        }
    }
}




void savedata(void)//割り込み時の処理
{
    flag = 1;
    intrrupt_cnt++;

}


void ADXL_config(void)//ADXL345の初期設定
{
    //初期設定開始
    accelerometer.setPowerControl(0x00);

    //精度
    accelerometer.setDataFormatControl(0x0B);

    //3.2kHz
    accelerometer.setDataRate(ADXL345_3200HZ);

    //計測開始
    accelerometer.setPowerControl(0x08);

    accelerometer.setOffset (0, 0);

}

void INA226config(void) //INA226の設定
{

    char p_addr = 0x00;
    char p_addr_i = 0x04;
    short Calibration_set = 0x0A00;
    char write_item[3];
//---ammeter setting---
    i2c.start();
    if(i2c.write(i2c_addr | 0,&p_addr,sizeof(p_addr)) == 0) {
        debug.printf("ERROR_write\n");
    }
    debug.printf("OK!\n");

    //rawWrite(0x05,0x0A00);

    short Config_set = 0x444F;//0x4A4F;//0x4E97;//0x4C97;
    write_item[0] = 0x00;
    write_item[1] = static_cast<char>((Config_set >> 8) & 0x00ff);
    write_item[2] = static_cast<char>(Config_set & 0x00ff);
    if(i2c.write(i2c_addr | 0,write_item,3) == 0) {
        debug.printf("OK!!\n");
    } else {
        debug.printf("OH!!\n");
    }



    write_item[0] = 0x05;
    write_item[1] = static_cast<char>((Calibration_set >> 8) & 0x00ff);
    write_item[2] = static_cast<char>(Calibration_set & 0x00ff);
    if(i2c.write(i2c_addr | 0,write_item,3) == 0) {
        debug.printf("OK!!\n");
    } else {
        debug.printf("OH!!\n");
    }
}

void weight_init(void) //慣性力発生用のおもりの位置の初期化
{
//---position reset---
    dir = 0;
    duty = 0.50f;
    while(!sensorB);
    duty = 0.0f;
    wait(2.0);
    myled1 = 1;
}

void controller(double *v,float *duty_output,double *acce_device, double *acce_output, double *F)//制御部
{

    double ka = 100;
    double kv = 100;

    int readings[3] = {0,0,0};
//    static double F;
    static double F_1;
    static double F_2;
    static double F_3;
    static int intrrupt_checker = 1;


//---加速度を読み取り変換する---
    accelerometer.getOutput(readings);
    *acce_device = (int16_t)readings[0] * 0.0383;
    *v = *v + *acce_device * 0.01;
    if( *acce_device < 0) {
        if(abs(*v) > 0) {
//---必要な力の算出（今回は不使用）---
            F_1 = -1 * ( ka * *acce_device) + (kv * *v);
            *F = 0.35 * F_1 + 0.4 * F_2 + 0.25 * F_3;
            *acce_output = *F / 8.5;
            if(*duty_output > 1.0) {
                *duty_output = 1.0;
                myled4 = 1;
            }
//---加速度を検知したら慣性力発生用おもりを加速させる---
            if(intrrupt_checker <= intrrupt_cnt){
                *duty_output = *duty_output + 0.04;
                intrrupt_checker = intrrupt_cnt + 1;
            }
            
            
            duty.write(*duty_output);

            F_3 = F_2;
            F_2 = *F;
        }
    }
}

void read_current(double *C) //電流量を測る
{
    char I[2];
    unsigned short read_item;
    char p_addr = 0x00;
    char p_addr_i = 0x04;
//---reading current---
    while(i2c.write(i2c_addr | 0,&p_addr_i,sizeof(p_addr_i)) != 0);
    while(i2c.read(i2c_addr | 0x01,I,sizeof(I)) != 0);
    read_item = static_cast<unsigned short>(I[0]);
    read_item = (read_item << 8) | static_cast<unsigned short>(I[1]);

    char *s_p = reinterpret_cast<char *>(&read_item);
    short d_s;
    char *d_p = reinterpret_cast<char *>(&d_s);
    *(d_p + 0) = *(s_p + 0);
    *(d_p + 1) = *(s_p + 1);
    *C = static_cast<double>(d_s) /* * 1.25 */;
}