File content as of revision 0:fb6e494a7656:

#include "mbed.h"
#include "params.h"
#include "matrix.h"
#include "kinematic.h"
#include "pathPlanning.h"

DigitalIn sw2(PC_13);
Serial pc(USBTX, USBRX);
//DigitalOut led_green(LED_GREEN);

matrix path;
matrix p0;
matrix pf;

matrix p;
matrix q;

Timer t;

void check_sw2(void)
{
    if (sw2 == 0) {
        //pc.printf("SW2 button pressed. \n");
        //led_green = 0;
        t.start();
        for(int k = 0; k < 1 ; k++) {
            for(int j = 0; j < path.col; j++) {
                for(int i = 0; i < 6; i++) {
                    p.element[i][0] = path.element[i][j];
                    //pc.printf("%.4f    ", p.element[i][0]);
                }

                //pc.printf("\n");
                IPKF(&p, &q);
                
                pc.printf("%f    ", q.element[0][0]);
                pc.printf("\n");
                
                //for(int i = 0; i < 6; i++) {
                //    pc.printf("%.4f    ", q.element[i][0]);
                //}
                //pc.printf("\n");

                //DPKF(&q,&p);

                //for(int i = 0; i < 6; i++) {
                //    pc.printf("%.4f    ", p.element[i][0]);
                //}
                //pc.printf("\nj=%d\n", j);
            }
        }
        wait(2);
        pc.printf("%f", 0.0);
        wait(2);
        t.stop();
        //pc.printf("%f s\n", t.read());
        t.reset();
    }
    //led_green = 1;
    return;
}

int main()
{
    //led_green = 1;
    pc.baud(115200);

    InitMatrix(&path, 6, 100);
    InitMatrix(&p0, 6, 1);
    InitMatrix(&pf, 6, 1);

    InitMatrix(&p, 6, 1);
    InitMatrix(&q, 6, 1);

    p0.element[0][0]= 0.4;
    p0.element[1][0]= 0;
    p0.element[2][0]= 0.4;
    p0.element[3][0]= 0;
    p0.element[4][0]= 0;
    p0.element[5][0]= 0;

    pf.element[0][0]= 0.7;
    pf.element[1][0]= 0.4;
    pf.element[2][0]= -0.1;
    pf.element[3][0]= PI/4;
    pf.element[4][0]= PI/6;
    pf.element[5][0]= -PI/3;

    GeneratePath(&p0,&pf,&path);
    while(true) {
        check_sw2();
        wait(0.1);
    }
}