File content as of revision 1:f37787740470:

#include "mbed.h"
#include "QEI.h" // for encoder
#include "USBHostMSD.h" // for USB memory
#include "AD5754.h" // for DA converter

#define PIEZO_PERIOD_US 1000 // 1kHz
#define ENCODER_PERIOD_US 1000 // 1kHz
//#define ENCODER_PERIOD_US 100000 // 10Hz
//#define VIBRATOR_PERIOD_US 200 // 5kHz
//#define ACC_PERIOD_US 200 //5kHz

//// State in main phase ////
#define INITIALISATION_STATE 0
#define BEFORE_IMPACT_STATE 1
#define AFTER_IMPACT_STATE 2

#define RECORD_PERIOD 1.0

#define SAVE_NUMBER 1024 // 5(kHz) * 0.2(s) = 1000

#define AVE_WINDOW 10
#define VEL_WINDOW 10

Serial pc(USBTX, USBRX);
 
AnalogIn piezo(p19);
QEI encoder(p21, p22, NC, 2000);  // encoder
AnalogOut vibrator(p18);
AD5754 dac(p5, NC, p7, p8); // spi_(mosi, miso, sck), nCS_(cs)


Ticker piezo_t;
Ticker encoder_t;
//Ticker vibrator_t;
//Ticker acc_t;

Timer t;

volatile bool piezo_flag = false;
void piezo_interrupt(){
    piezo_flag = true;
}

volatile bool encoder_flag = false;
void encoder_interrupt(){
    encoder_flag = true;
}

/*
volatile bool vibrator_flag = false;
void vibrator_interrupt(){
    vibrator_flag = true;
}
*/
/*
volatile bool acc_flag = false;
void acc_interrupt(){
    acc_flag = true;
}
*/

int main() {
    
    pc.baud(115200);
    
    USBHostMSD msd("usb");
    
    
    while(!msd.connect()){
        wait(1);
    }
    
    piezo_t.attach_us(&piezo_interrupt, PIEZO_PERIOD_US);
    encoder_t.attach_us(&encoder_interrupt, ENCODER_PERIOD_US);
    //vibrator_t.attach_us(&vibrator_interrupt, VIBRATOR_PERIOD_US);
    //acc_t.attach_us(&acc__interrupt, ACC_PERIOD_US);
    
    uint16_t DAdata[3] = {}; // 16bit; 0 ~ 32768 ~ 65535 (-5 ~0 ~ 5(V))
    int channel = 0;
    
    
    bool start_flag = true;
    bool main_flag = false;
    bool end_flag = false;
    
    short state = INITIALISATION_STATE;
    
    //// Time ////
    float start_time;
    
    //// Piezosensor ////
    float piezo_val = 0.0;
    //float piezo_val_buf[10] = {};
    
    //// Encoder ////
    int encoder_cnt = 0;
    int encoder_buf[AVE_WINDOW] = {};
    int encoder_ave = 0;
    int encoder_ave_buf[VEL_WINDOW] = {};
    int encoder_vel = 0;
    
    int i = 0;
    int j = 0;
    int buf[500] = {};
    int data = 0;
    int ret;
    
    /*
    //// Vibrator ////
    float vib_cmd = 0.5;
    */
    
    //// Accelerometer ////
    /*
    int acc_val[3] = {};
    float acc_fval[3] {};
    float acc_fval_xbuf[5] = {};
    float acc_fval_ybuf[5] = {};
    float acc_fval_zbuf[5] = {};
    */
    
    //// Data buff ////
    float save_data[SAVE_NUMBER] = {};
    int save_cnt;
    
    LocalFileSystem local("local"); 
    
    //// Option ////
    int print_cnt = 0;
    
    
    // Fix vibrator
    vibrator = 0.0;
    
    pc.printf("'S' key: Start state\n\n");
    //pc.printf("'E' key: End state\n\n");
    wait(1);
    
    dac.rangeSelect();
    dac.setPowerControl();
    
    
    while(start_flag){
        if(pc.readable()){
            if(pc.getc() == 's'){
                start_flag = false;
                pc.printf("\nHAPTIC test start\n");
                
                /*
                pc.printf("\nFile reading:\n");
                FILE *fp = fopen( "/usb/test.csv", "r");
                
                if ( fp == NULL )
                {
                    error("\nCould not open file\n");
                }
                */
                
                /*
                while( (ret=fscanf(fp, "%d", &data)) != EOF){
                    buf[i] = data;
                    i++;
                    pc.printf("\nError\n");
                }
                */
                
                
                //fclose(fp); 
                
                wait(0.5);
                pc.printf("Start initialization\nLift device\n\n");
                wait(0.5);
                main_flag = true;
            }
        }
    }
    
    t.start();

    while(main_flag){
        switch(state){
            case INITIALISATION_STATE:
                if(encoder_flag){
                    vibrator = 0.5;
                    encoder_flag = false;
                    encoder_cnt = encoder.getPulses();  // One of the encoders needs a sign change
                    
                    encoder_ave = encoder_cnt;
                    for(int i=0; i<AVE_WINDOW-1; i++){
                        encoder_buf[i+1] = encoder_buf[i];
                        encoder_ave += encoder_buf[i];
                    }
                    encoder_buf[0] = encoder_cnt;
                    encoder_ave = encoder_ave/AVE_WINDOW;
                    
                    if(encoder_ave < -350){
                        state = BEFORE_IMPACT_STATE;
                        pc.printf("Finish initialization\n\n");
                    }
                    
                    
                    if(print_cnt > 1000000/ENCODER_PERIOD_US){
                        printf("encoder = %d\n", encoder_ave);
                        print_cnt = 0;
                    }
                    print_cnt++;
                    
                    ////// DA converter //////
                    DAdata[0] = (uint16_t)((int16_t)(32768)); // 16bit; 0 ~ 32768 ~ 65535 (-5 ~0 ~ 5(V))
                    if(DAdata[0] > 65536){
                        DAdata[0] = 0;
                    }
                    channel = 0;
                    dac.send(channel, DAdata[0]);
                    //////
                    
                }
                break;
            case BEFORE_IMPACT_STATE:
                
                if(piezo_flag){
                    piezo_flag = false;
                    piezo_val = piezo; // 0.0(0.0V)~ 0.5(1.65V) ~ 1.0(3.3V)
                    /*
                    if(print_cnt > 1000000/PIEZO_PERIOD_US){
                        //printf("piezosensor = %0.3f\n", piezo_val);
                        //printf("time = %f\n", t.read());
                        print_cnt = 0;
                    }
                    print_cnt++;
                    */
                    
                    // Save piezo data in save_data[0] ~ [99]
                    /*
                    save_data[100] = piezo_val;
                    for(save_cnt=0; save_cnt<100; save_cnt++){
                        save_data[save_cnt] = save_data[save_cnt+1];
                    }
                    */
                    
                }
                if(encoder_flag){
                    encoder_flag = false;
                    encoder_cnt = encoder.getPulses();  // One of the encoders needs a sign change
                    
                    
                    encoder_ave = encoder_cnt;
                    for(int i=0; i<AVE_WINDOW-1; i++){
                        encoder_buf[i+1] = encoder_buf[i];
                        encoder_ave += encoder_buf[i];
                    }
                    encoder_buf[0] = encoder_cnt;
                    encoder_ave = encoder_ave/AVE_WINDOW;
                    
                    
                    
                    for(int i=VEL_WINDOW-2; i>=0; i--){
                        encoder_ave_buf[i+1] = encoder_ave_buf[i];
                    }
                    encoder_ave_buf[0] = encoder_ave;
                    encoder_vel = encoder_ave_buf[VEL_WINDOW-1] - encoder_ave_buf[0];
                    
                    if(print_cnt > 1000000/ENCODER_PERIOD_US){
                        printf("velocity = %d\n", encoder_vel);
                        print_cnt = 0;
                    }
                    print_cnt++;
                    
                    //if(encoder_cnt < 1 && encoder_vel >10){
                    if(encoder_cnt > -1 && encoder_vel == -16){ // 
                        state = AFTER_IMPACT_STATE;
                        pc.printf("Collision!!\n\n");
                        printf("Collision velocity = %d\n", encoder_vel);
                        start_time = t.read();
                        save_cnt = 100;
                        
                        DAdata[0] = (uint16_t)((int16_t)(50000)); // 16bit; 0 ~ 32768 ~ 65535 (-5 ~0 ~ 5(V))
                        if(DAdata[0] > 65536){
                            DAdata[0] = 0;
                        }
                        channel = 0;
                        dac.send(channel, DAdata[0]);
                    
                    }
                    
                    /*
                    if(print_cnt > 1000000/ENCODER_PERIOD_US){
                        printf("encoder = %d\n", encoder_cnt);
                        print_cnt = 0;
                    }
                    print_cnt++;
                    */
                }
                break;
            case AFTER_IMPACT_STATE:
                DAdata[0] = (uint16_t)((int16_t)(50000)); // 16bit; 0 ~ 32768 ~ 65535 (-5 ~0 ~ 5(V))
                if(DAdata[0] > 65536){
                    DAdata[0] = 0;
                }
                channel = 0;
                dac.send(channel, DAdata[0]);
                    
                /*
                if(vibrator_flag){
                    vibrator_flag = false;
                    //vibrator = vib_cmd;
                }
                */
                
                pc.printf("'R' key: Restart\n\n");
                while(state == AFTER_IMPACT_STATE) {
                    if(pc.readable()){
                        if(pc.getc() == 'r'){
                            state = INITIALISATION_STATE;
                        }
                    }
                }
                break;
            default:
                state = INITIALISATION_STATE;
                break;
        }
            
        /*
        if(acc_flag){
            acc_flag = false;
            accelerometer.getOutput(acc_val);
            acc_fval[0] = (float)(int16_t)acc_val[0];
            acc_fval[1] = (float)(int16_t)acc_val[1];
            acc_fval[2] = (float)(int16_t)acc_val[2];
            //printf("x = %6.2f y = %6.2f z = %6.2f\n", acc_fval[0]*200/4096, acc_fval[1]*200/4096, acc_fval[2]*200/4096);
        }
        */
        
        if(pc.readable()){
            if(pc.getc() == 'e'){
                main_flag = false;
                end_flag = true;
            }
        }
        
    }
    pc.printf("HAPTIC test stop\n\n\n");
    wait(1);
    
    while(end_flag){
        
        FILE *fp;
        fp = fopen("/usb/test.csv", "w"); 
        if(fp == NULL){
            error("Could not open file for write\n");
        }
        else{
            for(int i=0; i<SAVE_NUMBER-1; i++){
                fprintf(fp,"%f\n",save_data[i]);
            }
            fclose(fp);
        }
        
        end_flag = false;
    }
    printf("END Haptic test\n");
}