File content as of revision 7:59613b7a1631:

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

#define MOTOR_PPR 300
#define ENCODER_PPR 1024

#define ENC_QUADRATURE_TYPE  4
#define MOT_QUADRATURE_TYPE  2
#define OUR_PI  3.141592653589793
#define DATA_COLS 7
#define BUFFER_SIZE 4200
#define MAX_VOLTAGE 3.3
#define VOLTS_PER_AMP 0.14
#define PROGRAM_RUNTIME 15.0

Serial pc(USBTX, USBRX);

QEI encoder(p29, p30, NC, ENCODER_PPR, QEI::X4_ENCODING);
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");
float theta1, theta2, dtheta1, dtheta2;
float mCurrent = 0.0;
float inputVoltage = 0.0;
//int pulses0 = 0;
//int deltaPulses;
float t0 = 0.0;
float t = 0.0, dt;

//Controller gains - Full-state Feedback
//float k1 = -0.0316, k2 = 9.7076, k3 = -0.4095, k4 = 1.2340, k5 = 0.0410;
float k1 = -0.3162, k2 = 18.278, k3 = -0.8964, k4 = 2.4441, k5 = 0.1843;

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

float* buffer;
float lambda1 = 30, lambda2 = 30, lambda3 = 15;
int index;
int pulsesPend, pulsesMot;
bool flag = 0;

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

void setVoltage(float inputVoltage)
{
    if(inputVoltage<0.0) {
        inputVoltage = -inputVoltage;
        dOut1=0;
        dOut2=1;
    } else {
        dOut1=1;
        dOut2=0;
    }
    float dutyCycle = inputVoltage/MAX_VOLTAGE;
    dutyCycle = (dutyCycle > 1.0)? 1.0 : dutyCycle;
    pwmOut.write(dutyCycle);
}

void computing(void const *args) {
    float z1 = 0.0, z2 = 0.0, dz1 = 0.0, dz2 = 0.0, z3 = 0.0, dz3 = 0.0; 
    
    while (true ) {
        t = T.read();
        
        //set pwm 
        // ADD A SANITY CHECK ON THETA
        if (cos(theta2) < 0.98) {
            flag = 0;
            inputVoltage = 0.0;
        } else {
            flag = 1;
            inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent);
        }
        setVoltage(inputVoltage);
        
        //read current
        mCurrent = aIn.read()*MAX_VOLTAGE/VOLTS_PER_AMP; 
        if(dOut1 == 0)
            mCurrent = -mCurrent;
        pulsesPend = -encoder.getPulses();
        pulsesMot = motor.getPulses();
        
        dt = t - t0; //time difference
        theta2 = float(pulsesPend)*encoder_conv + OUR_PI;
        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();
    pwmOut.period(0.0001); 
    
    Thread thrd2(computing,NULL,osPriorityRealtime);
    pc.printf("started computing thread!\r\n");
    Thread thrd3(saving,NULL,osPriorityNormal);
    pc.printf("started saving thread!\r\n");
   
    
    pc.printf("Start!\r\n");
    pc.printf("Time: %f\r\n", t);
    while (t < PROGRAM_RUNTIME) 
    {
        //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();
}