File content as of revision 0:86efb6256f31:

#include "mbed.h"
#include "SDFileSystem.h"
#include "ADXL345_I2C.h"
#include "QEI.h"

#include <stdio.h>
#include <stdlib.h>

void savedata(void);
void flipB(void);

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

SDFileSystem sd(p11, p12, p13, p14, "sd");
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;


static char i2c_addr = 0x80;



int main()
{



//    int i;

    char filename[80];
    char a[200][250];
    char rotate[80];
    int i,j;
    int num;
    double C;
    unsigned short read_item;
    char p_addr = 0x00;
    char p_addr_i = 0x04;
    char I[2];
    float duty_output;
    int loop_break;

    double F = 0;
    double F_1 = 0;
    double F_2 = 0;
    double F_3 = 0;

    double a_device;
    double a_output;


    double ka = 100;
    double kv = 100;

    int readings[3] = {0, 0, 0};
//    int16_t a[3] = {0,0,0};
    double a_;
    double v;
    
    myled1 = 1;

//---ADXL345setting---
//    debug.printf("Starting ADXL345 test...\n");
//    debug.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());

    //Go into standby mode to configure the device.
    accelerometer.setPowerControl(0x00);

    //Full resolution, +/-16g, 4mg/LSB.
    accelerometer.setDataFormatControl(0x0B);

    //3.2kHz data rate.
    accelerometer.setDataRate(ADXL345_3200HZ);

    //Measurement mode.
    accelerometer.setPowerControl(0x08);

    accelerometer.setOffset (0, 0);

    myled2 = 1;

//    printf("Hello World!\n");

//---SDsetting--
    mkdir("/sd/mydir", 0777);
    for(num=1; num<=256; num=num+1) {
        sprintf(filename,"/sd/mydir/data%d.csv",num);
        fp = fopen(filename, "r");
        if(fp == NULL) {
            fp = fopen(filename,"w");
            break;
        }
        fclose(fp);
    }
//    fprintf(fp, "Hello fun SD Card World!\n");
    myled3 = 1;
//---ammeter setting---
    i2c.start();
    if(i2c.write(i2c_addr | 0,&p_addr,sizeof(p_addr)) == 0) {
        debug.printf("DA-ME-DA-YO!\n");
    }
    debug.printf("OK!\n");

    //rawWrite(0x05,0x0A00);


    char write_item[3];
    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");
    }

    
    short Calibration_set = 0x0A00;
    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");
    }

    myled4 = 1;
    wait(3.0);
    myled1 = 0;
    myled2 = 0;
    myled3 = 0;
    myled4 = 0;
    
    
//---ammeter setting end---
    duty_output = 0.0;
    duty.period(0.00005);
//    NVIC_SetPriority(0, 1);


/////////////////////
//---main routen---
/////////////////////
    while(1) {
        //debug.printf("enter the loop\n");
        t.reset();
        i = 0;
        loop_break = 0;
        debug.printf("writing OK!\n");
//---position reset---
        dir = 0;
        duty = 0.50f;
        while(!sensorD) {
//            debug.printf("loop sensorD\n");
        }
//        debug.printf("loop end\n");
        duty = 0.0f;
        wait(2.0);

//---sensing start---

//        wheel.reset();
        flipB_ = 0;
//        __enable_irq();
        dir = 1;
//        duty.write(duty_);
//        debug.printf("motor start\n");
        
//        debug.printf("interrupt start!\n");
//        sensorB_.rise(&flipB);
        while (1) {
            accelerometer.getOutput(readings);
//            debug.printf("%i\n", abs((int16_t)readings[0]));

            if( abs((int16_t)readings[0]) > 30) {
                myled3 = 1;
                break;
            }
        }
        save.attach(&savedata, 0.01 );
        t.start();
        
        
        while(t.read() <= 0.50) {
//            debug.printf("loop sensorB\n");
            if(flag) {
                flag = 0;

//---reading acceleration and control---
                accelerometer.getOutput(readings);
                //a[0] = (int16_t)readings[0];
                a_device = (int16_t)readings[0] * 0.0383;
                v = v + a_device * 0.01;
                if( a_device < 0) {
                    if(abs(v) > 0) {
                        F_1 = -1 * ( ka * a_device) + (kv * v);
                        F = 0.35 * F_1 + 0.4 * F_2 + 0.25 * F_3;
                        a_output = F / 5.0;
                        duty_output = a_output / 118 + duty_output;
                        if(duty_output > 0.65) {
                            duty_output = 0.65;
                            myled4 = 1;
                        }
                        duty.write(duty_output);
                        F_3 = F_2;
                        F_2 = F;
                        debug.printf("%i,%f,%f\n",  (int16_t)readings[0],v,duty_output);
                    }
                }

//---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 */;
/*                if(1) {
                    sprintf(a[i],"%f10.3,%f10.2,%d,%f10.3,%f10.3,%f10.3,%f10.3\n",t.read(),C,wheel.getPulses(),a_device,v,F,duty_output);
                    if( i > 299) {
                        loop_break = 1;
                        break;
                    }
                    i = i + 1;
                }*/

            }
            if(loop_break) {
                break;
            }

        }
        myled2 = 1;
        t.stop();


        duty = 0.0f;

        for( j = 0; j < i; j++) {
            fprintf(fp,a[j]);
        }

//        sprintf(rotate,"%f,%d\n",t.read(),wheel.getPulses());
        fprintf(fp,rotate);


        wait(3.0);

        save.detach();
        while(1) {
            myled2 = 1;
            wait(0.2);
            myled2 = 0;
            wait(0.2);
        }
    }

}




void savedata(void)
{
    flag = 1;
//    debug.printf("interrupt!\n");
}

void flipB(void)
{
    flipB_ = 1;
    duty = 0.0f;
    debug.printf("flip sensorB!");
    __disable_irq();
}