Joao Luiz Almeida de Souza Ramos / Mbed 2 deprecated furutacontroller

Dependencies:   QEI mbed-rtos mbed

main.cpp

Committer:
jaoramos
Date:
2013-12-01
Revision:
0:9f2b0ea63eac
Child:
1:5c05e0d08e61

File content as of revision 0:9f2b0ea63eac:

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

#define NR_SAMPLES 4000

Serial pc(USBTX, USBRX);

QEI encoder(p29, p30, NC, 1024);
Timer T;

// open a file for data logger
LocalFileSystem local("local");

float angle = 0.0;
int pulses0 = 0;
int deltaPulses;
float t0 = 0.0;
float t = 0.0, dt, vel;
int curPulses;
float conversion = 360.0/(1024.0*2.0);
float* buffer;

int index;

void saving(void const *args) {
    index = 0;
    while (true) {
        //fprintf(fp, "0.0\n");
        //fprintf(fp, "%f, %f, %f\n", angle, vel, t); 
        //printf("Vel is: %f, dt = %f\r\n", vel, dt);
        buffer[index] = angle;
        buffer[index+1] = t;
        index = index+2;
        Thread::wait(10);
    }
}

void computing(void const *args) {
    while (true) {
        t = T.read();
        dt = t - t0;
        curPulses = encoder.getPulses();
        angle = float(curPulses)*conversion;
        
        deltaPulses = curPulses - pulses0;
        vel = float(deltaPulses)/dt*conversion;
        
        pulses0 = curPulses;
        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+2) {
 
        fprintf(fp,"%f,%f\n", buffer[i],buffer[i+1]);
    }
    pc.printf("closing file\n\r");
    fclose(fp);
    wait(2.0);;
}

int main() {
    //allocate memory for the buffer
    buffer = new float[NR_SAMPLES];
    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();
}