File content as of revision 4:1defb279922a:

#include "mbed.h"
#include "Robot.h"
#include "Communication.h"
#include "control.h"
//#include <Serial.h>

Serial pc(USBTX, USBRX);
//PwmOut rplidar_motor(D3);

int main() {
    //pc.baud(115200);
    //init_communication(&pc);
    Matrix occupationGrid;
    occupationGrid = create_occupation_grid();
    float xObj = 99.9;
    float yObj = poseY;
    float error = 0;
    float intError = 0;
    float objAheadX = poseX;
    float objAheadY = poseY;
    float objCellCenter[2] = {floor(xObj / CELL_SIZE) * CELL_SIZE + (CELL_SIZE / 2),
                              floor(yObj / CELL_SIZE) * CELL_SIZE + (CELL_SIZE / 2)};

    do{
        updatePose();
        followPath(objAheadX, objAheadY, intError, objCellCenter, occupationGrid, pc);
        error = sqrt((xObj - poseX)*(xObj - poseX) + (yObj - poseY)*(yObj - poseY));
        //printf("error = %f   ", error);
        printf("pose = [%.1f, %.1f]\n", poseX, poseY);
        wait_us(TIMESTEP*1000000);
    } while(error > CELL_SIZE);

    setSpeeds(0, 0);
}