File content as of revision 2:011e6115c77a:

#include "mbed.h"
#include "rtos.h"
#include "QEI.h"
#include <fstream>
#include <iomanip>

#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.

//Motor direction and PWM
DigitalOut dOut1(p5);
DigitalOut dOut2(p7);
PwmOut pwmOut(p21);

// 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 lambda1 = 30, lambda2 = 30, lambda3 = 15;
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, z3 = 0.0, dz3 = 0.0;
    float freq = 1.0, dutyCycle;
    int pulsesPend, pulsesMot;
    while (true) {
        t = T.read();
        
        //set pwm 
        dutyCycle = (sin(freq*t));
        dutyCycle = dutyCycle*dutyCycle;
        //pc.printf("Duty%f\n\r",dutyCycle);
        pwmOut.write(dutyCycle);
        
        //read current
        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 = - lambda1 * z1 + lambda1 * theta1;
        z1 = z1 + dz1 * dt;
        dtheta1 = dz1;
        
        //calculate dtheta2 
        dz2 = - lambda2 * z2 + lambda2 * theta2;
        z2 = z2 + dz2 * dt;
        dtheta2 = dz2;
        
        //filter current 
        dz3 = -lambda3 * z3 + lambda3 * mCurrent;
        z3 = z3 + dz3 * dt;
        mCurrent = z3;
        
        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);
    
    //Run forward
    pwmOut.period(0.0001); 
    dOut1=1;
    dOut2=0;
    
    
    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 at Index: %d\r\n",index);
    pwmOut.write(0.0);
    thrd2.terminate();
    thrd3.terminate();
    saveToFile();
}