File content as of revision 1:5c05e0d08e61:

#include "mbed.h"
#include "rtos.h"
#include "QEI.h"


#define MOTOR_PPR 1200
#define ENCODER_PPR 1024

#define QUADRATURE_TYPE  2
#define OUR_PI  3.141592653589793
#define DATA_COLS 6
#define NR_SAMPLES 1500
#define buffer_size 3500
#define MAX_VOLTAGE 3.3
#define VOLTS_PER_AMP 0.14


Serial pc(USBTX, USBRX);

QEI encoder(p29, p30, NC, ENCODER_PPR);
QEI motor(p25, p26, NC, MOTOR_PPR);
Timer T;

//Curent Measurement
AnalogIn aIn(p16);    //pin 15 set as analog input.  Pins 15-20 can be used as analog inputs.


// open a file for data logger
LocalFileSystem local("local");
//const int  buffer_size = DATA_COLS * NR_SAMPLES;
float theta1, theta2, dtheta1, dtheta2;
float mCurrent = 0.0;
//int pulses0 = 0;
//int deltaPulses;
float t0 = 0.0;
float t = 0.0, dt;

float encoder_conv = 2*OUR_PI/(float(ENCODER_PPR)*float(QUADRATURE_TYPE));
float motor_conv = 2*OUR_PI/(float(MOTOR_PPR)*float(QUADRATURE_TYPE));

float* buffer;
float lambda = 30;
int index;

void saving(void const *args) {
    index = 0;
    while (true) {
        buffer[index] = theta1;
        buffer[index+1] = theta2;
        buffer[index+2] = dtheta1;
        buffer[index+3] = dtheta2;
        buffer[index+4] = mCurrent;
        buffer[index+5] = t;
        
        index = index+DATA_COLS;
        Thread::wait(20);
    }
}

void computing(void const *args) {
    float z1 = 0.0, z2 = 0.0, dz1 = 0.0, dz2 = 0.0;
    
    int pulsesPend, pulsesMot;
    while (true) {
        t = T.read();
        
        mCurrent = aIn.read()*MAX_VOLTAGE/VOLTS_PER_AMP; 
        pulsesPend = -encoder.getPulses();
        pulsesMot = motor.getPulses();
        
        dt = t - t0; //time difference
        theta2 = float(pulsesPend)*encoder_conv;
        theta1 = float(pulsesMot)*motor_conv;
        
        //calculate dtheta1 
        dz1 = - lambda * z1 + lambda * theta1;
        z1 = z1 + dz1 * dt;
        dtheta1 = dz1;
        
        //calculate dtheta2 
        dz2 = - lambda * z2 + lambda * theta2;
        z2 = z2 + dz2 * dt;
        dtheta2 = dz2;
         
        t0 = t;
        Thread::wait(1);
    }
}

void saveToFile ()
{
    FILE *fp = fopen("/local/data.csv", "w");
    if (!fp) {
        fprintf(stderr, "File could not be openend \n\r");
        exit(1);
    }
 
    wait(2.0);
 
    for (int i=0; i < index; i=i+DATA_COLS) 
    {
        for (int j = 0; j< DATA_COLS; j++)
        {
            fprintf(fp,"%f,", buffer[i+j]);
        }
        fprintf(fp,"\n");
    }
    pc.printf("closing file\n\r");
    fclose(fp);
    wait(2.0);;
}

int main() {
    //allocate memory for the buffer
    pc.printf("creating buffer!\r\n");
    buffer = new float[buffer_size];
    pc.printf("done creating buffer!\r\n");
    T.start();
    Thread thrd2(computing,NULL,osPriorityRealtime);
    Thread thrd3(saving,NULL,osPriorityNormal);
    
    pc.printf("Start!\r\n");
    pc.printf("Time: %f\r\n", t);
    while (t < 10.0) 
    {
        pc.printf("Time: %f\r\n", t);
        Thread::wait(1000);
    }
    pc.printf("Done!\r\n");
    thrd2.terminate();
    thrd3.terminate();
    saveToFile();
}