File content as of revision 4:1defb279922a:

#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <math.h>
#include "Communication.h"
#include "control.h"
#include "mbed.h"
#include "Robot.h"
#include "Matrix.h"

const float TIMESTEP = 0.05;
const float CELL_SIZE = 5;
const int MAP_SIZE = 100;
const int GRID_SIZE = int(MAP_SIZE/CELL_SIZE);
const int WINDOW_SIZE = int(45/CELL_SIZE);
const float PI = 3.1415926;

Matrix create_occupation_grid(){
    Matrix occupationGrid = Matrix(GRID_SIZE,GRID_SIZE);
    for(int i = 1; i <= 20; i++)
        for(int j = 1;j <= 20; j++)
            if (i > 8 && i < 12 && j > 8 && j < 12)
                occupationGrid.add(i,j,1);

    return occupationGrid;
}
/*int occupationGrid[GRID_SIZE][GRID_SIZE] = {{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}};*/
/*int occupationGrid[GRID_SIZE][GRID_SIZE] = {{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}};*/

void moveToPoint(float xObj, float yObj){
    float kv = 0.2;
    float kw = 10;
    
    float d = sqrt((xObj - poseX)*(xObj - poseX) + (yObj - poseY)*(yObj - poseY));
    float angleError = atan2(yObj - poseY, xObj - poseX);
    //printf("ANGLE ERROR: %.1f    ", angleError*180/PI);
    angleError = atan2(sin(angleError - poseTheta), cos(angleError - poseTheta));   // equivalente a wrapToPi()

    float wheelSpeeds[2];

    robotVel2wheelVel(kv*d, kw*angleError, wheelSpeeds);

    wheelSpeeds[0] = mapSpeeds(wheelSpeeds[0]);
    wheelSpeeds[1] = mapSpeeds(wheelSpeeds[1]);

    //printf("wheel speeds: [%.0f, %.0f]\t", wheelSpeeds[0], wheelSpeeds[1]);

    setSpeeds(wheelSpeeds[0], wheelSpeeds[1]);
}

float vff(Matrix occupationGrid, int *xactive, int *yactive, int lenx, int leny, float *objCellCenter, Serial pc){
    //printf("i vff; ");
    float Fcr = 35;
    float Fca = 1.5;

    // não se sabe quantas células estão ocupadas então aloca-se espaço para todas para mais
    // à frente guardar apenas as ocupadas e apagar estes arrays
    //int *auxRows, *auxCols;

    //auxRows = (int *)calloc(lenx*leny, sizeof(int));
    //auxCols = (int *)calloc(lenx*leny, sizeof(int));

    //int occupiedCounter = 0;
    //int auxCounter = 0;

    // pesquisa a grelha de ocupação e guarda os índices da janela ativa que correspondem a 
    // células ocupadas
    float Frx = 0;
    float Fry = 0;

    float xCell, yCell;
    float value;

    for(int i = 0; i < lenx; i++){
        for(int j = 0; j < leny; j++){
            // necessario somar 1 porque os indices na matriz comecam em 1 
            value = occupationGrid(xactive[i]+1,yactive[j]+1);
            //printf("occupationGrid(%d, %d): %.0f", xactive[i]+1, yactive[j]+1, value);
            if(value == 1){
                //auxRows[auxCounter] = xactive[i];
                //auxCols[auxCounter] = yactive[j];
                //occupiedCounter++;

                xCell = xactive[i]*CELL_SIZE + CELL_SIZE/2;
                yCell = yactive[i]*CELL_SIZE + CELL_SIZE/2;
                Frx += Fcr*(xCell - poseX)/pow(sqrt(pow(xCell - poseX, 2) + pow(yCell - poseY, 2)), 3);
                Fry += Fcr*(yCell - poseY)/pow(sqrt(pow(xCell - poseX, 2) + pow(yCell - poseY, 2)), 3);
            }
            else{
               continue; 
                //auxRows[auxCounter] = -1;
                //auxCols[auxCounter] = -1;
            }
            //auxCounter++;
        }
    }
    //pc.printf("Fr = [%.1f, %.1f]    ", Frx, Fry);
    float Fax = Fca*(objCellCenter[0] - poseX)/(sqrt(pow(objCellCenter[0] - poseX, 2) + pow(objCellCenter[1] - poseY, 2)));
    float Fay = Fca*(objCellCenter[1] - poseY)/(sqrt(pow(objCellCenter[0] - poseX, 2) + pow(objCellCenter[1] - poseY, 2)));
    //pc.printf("Fa = [%.1f, %.1f]    ", Fax, Fay);
    
    /*if(occupiedCounter == 0){
        float F[2] = {Fax, Fay};
        printf("f vff; ");
        return atan2(F[1], F[0]);
    }*/

    /*int *xOccupied, *yOccupied;
    xOccupied = (int *)calloc(occupiedCounter, sizeof(int));
    yOccupied = (int *)calloc(occupiedCounter, sizeof(int));

    occupiedCounter = 0;

    for(int i = 0; i < lenx*leny; i++){
        if(auxRows[i] != -1){
            xOccupied[occupiedCounter] = auxRows[i];
            yOccupied[occupiedCounter] = auxCols[i];
            occupiedCounter++;
        }
    }

    for (int i = 0; i < occupiedCounter; i++) {
        xCell = xOccupied[i]*CELL_SIZE + CELL_SIZE/2;
        yCell = yOccupied[i]*CELL_SIZE + CELL_SIZE/2;

        Frx += Fcr*(xCell - poseX)/pow(sqrt(pow(xCell - poseX, 2) + pow(yCell - poseY, 2)), 3);
        Fry += Fcr*(yCell - poseY)/pow(sqrt(pow(xCell - poseX, 2) + pow(yCell - poseY, 2)), 3);
    }

    free(auxCols);
    free(auxRows);
    free(xOccupied);
    free(yOccupied);*/

    float F[2] = {Fax - Frx, Fay - Fry};
    //printf("f vff; ");
    return atan2(F[1], F[0]);
}

void followPath(float &objAheadX, float &objAheadY, float &intError, float *objCellCenter, Matrix occupationGrid, Serial pc){
    //printf("i followPath; ");
    float kv = 0.3;
    float ki = 0.2;
    float kw = 10;
    int vObj = 7;  // temos de fazer experiências para afinar isto
    float vffAngle = poseTheta;
    int dObj = 3;

    float xmax = floor(static_cast<float>(poseX/CELL_SIZE)) + floor(static_cast<float>(WINDOW_SIZE/2));
    float xmin = floor(static_cast<float>(poseX/CELL_SIZE)) - floor(static_cast<float>(WINDOW_SIZE/2));
    float ymax = floor(static_cast<float>(poseY/CELL_SIZE)) + floor(static_cast<float>(WINDOW_SIZE/2));
    float ymin = floor(static_cast<float>(poseY/CELL_SIZE)) - floor(static_cast<float>(WINDOW_SIZE/2));

    if(xmin < 0)
        xmin = 0;
    if(xmax >= GRID_SIZE)   
        xmax = GRID_SIZE - 1;
    if(ymin < 0)
        ymin = 0;
    if(ymax >= GRID_SIZE)   
        ymax = GRID_SIZE - 1;
    
    int lenx = xmax - xmin + 1;
    int leny = ymax - ymin + 1;

    // pre alocacao de espaco
    int *xactive, *yactive;
    xactive = (int *)calloc(lenx, sizeof(int));
    yactive = (int *)calloc(leny, sizeof(int));

    // linspace do matlab 
    for(int j = 0; j < lenx; j++)
        xactive[j] = xmin + j;

    for(int i = 0; i < leny; i++)
        yactive[i] = ymin + i;

    // repmat do matlab
    //int *xactive = repeat_matrix2(x, lenx, leny);
    //int *yactive = repeat_matrix(y, leny, lenx);

    //int activeSize = lenx*leny;

    vffAngle = vff(occupationGrid, xactive, yactive, lenx, leny, objCellCenter, pc);

    objAheadX = objAheadX + TIMESTEP * vObj * cos(vffAngle);
    objAheadY = objAheadY + TIMESTEP * vObj * sin(vffAngle);
    
    float distError = sqrt(pow(objAheadY - poseY, 2) + pow(objAheadX - poseX, 2)) - dObj;

    intError = intError + distError * TIMESTEP;

    float v = kv * distError + ki * intError;
    float phiObj = atan2(objAheadY - poseY, objAheadX - poseX);
    float w = kw*atan2(sin(phiObj - poseTheta), cos(phiObj - poseTheta));
    float *wheelSpeeds;
    wheelSpeeds = (float *)calloc(2, sizeof(float));

    robotVel2wheelVel(v, w, wheelSpeeds);

    wheelSpeeds[0] = mapSpeeds(wheelSpeeds[0]);
    wheelSpeeds[1] = mapSpeeds(wheelSpeeds[1]);

    setSpeeds((int)wheelSpeeds[0], (int)wheelSpeeds[1]);

    free(wheelSpeeds);
    free(xactive);
    free(yactive);
    
    //printf("f followPath; ");
}