File content as of revision 4:213afe3d5c4b:

#include "mbed.h"
#include "math.h"
#include <stdio.h>
#include "funcs.h"

static const double pi = 3.14159265358;

Serial pc1(SERIAL_TX, SERIAL_RX);


//Funcao para obter as matrizes de Seccoes e Amplitudes
void get_Sector_Matrix(int Sector_Matrix_Sections[15][15], double Sector_Matrix_Abs[15][15]){
    double division_angle = 2*pi/24;
    double Aux_Angle_Matrix[15][15];
    double a = sqrt((double)2)*7;
    double b = 1;
    int i;
    
    //Atribui angulos aos varios pontos da matriz
    for(int x = 0; x < 15; x++){
        for(int y = 0; y < 15; y++){
            Aux_Angle_Matrix[x][y] = atan2((double)(y-7),(double)(x-7));
        }
    }
    
    //Colocar a matriz de seccoes toda a 0
    for(int x = 0; x < 15; x++){
        for(int y = 0; y < 15; y++){
            Sector_Matrix_Sections[x][y] = 0;
        }
    }    
    
    //Atribuir o indice da seccao em funcao dos angulos obtidos anteriormente
    for(int x = 0; x < 15; x++){
        for(int y = 0; y < 15; y++){
            i = 0;
            for(double ca = division_angle-pi; ca < pi; ca = ca+division_angle){ 
                if(Sector_Matrix_Sections[x][y] == 0 && (Aux_Angle_Matrix[x][y] <= ca || Aux_Angle_Matrix[x][y] == pi) && Aux_Angle_Matrix[x][y] >= ca-division_angle){
                    Sector_Matrix_Sections[x][y] = i;      
                }
                i = i+1;
            }
        }
    }
    
    //Atribuir a amplitude a cada elemento da matriz
    for(int x = 0; x < 15; x++){
        for(int y = 0; y < 15; y++){
            Sector_Matrix_Abs[x][y] = a-b*sqrt((double)((7-y)*(7-y) + (7-x)*(7-x)));
        }
    }
}


//Funcao para obter a matriz das redondezas do robo 
void get_Surroundings_Matrix(int Surroundings_Matrix[15][15], double actual_position_x, double actual_position_y, int Map_Matrix[80][80]){
    int map_position_x = floor(actual_position_x/50);
    int map_position_y = floor(actual_position_y/50);
    
    int i = 0;
    int j = 0;
    for(int x = map_position_x-8; x < map_position_x+8; x++){
        for(int y = map_position_y-8; y < map_position_y+8; y++){
            if(x < 0 || x > 79 || y < 0 || y > 79){
                Surroundings_Matrix[i][j] = 1;
            }    
            else{
                Surroundings_Matrix[i][j] = Map_Matrix[x][y];
            }
            j = j + 1;
        }
        i = i + 1;
        j = 0;
    }
}

//Funcao auxiliar de get_orientation_angle para encontrar os indices do histograma
void find_index(int given_index, int l, int out_index[9]){
    int n = 0;
    for(int i = given_index-l; i <= given_index+l; i++){
        if(i < 0){
            out_index[n] = 22 + i;
            n++;
        }
        else{   
            if(i > 22){
                out_index[n] = i - 23;
                n++;
            }
            else{
                out_index[n] = i;
                n++;
            }    
        }
    }
}

//Funcao para obter o angulo de orientacao seguinte 
double get_orientation_angle(double actual_position_x, double actual_position_y, double end_position_x, double end_position_y, int Surroundings_Matrix[15][15], int Sector_Matrix_Sections[15][15], double Sector_Matrix_Abs[15][15], double threshold){
    double Sector_Sum_Vector[23] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
    double Sector_Sum_Vector_Filtered[23];
    double auxiliar_angle[23];
    double angle_difference_end[23];
    double section_angle = 2*pi/23;
    double end_angle;
    int index = 0;
    int l = 4;
    int out_index[(2*l)+1];
    int ind;
    
    double Surroundings_Matrix_Aux[15][15];
    
    //Atribuimos os valores de amplitudes aos pontos da matriz das redondezas
    for(int x = 0; x < 15; x++){
        for(int y = 0; y < 15; y++){
            Surroundings_Matrix_Aux[x][y] = Surroundings_Matrix[x][y]*Sector_Matrix_Abs[x][y];
        }
    }    

    //Criamos o vetor do histograma das seccoes
    for(int x = 0; x < 15; x++){  
        for(int y = 0; y < 15; y++){   
            index = Sector_Matrix_Sections[x][y];
            Sector_Sum_Vector[index] = Sector_Sum_Vector[index] + Surroundings_Matrix_Aux[x][y];
        }
    } 
    
    //Filtramos o histograma
    for(int i = 0; i < 23; i++){  
        find_index(i,l,out_index);
        
        for(int c = 0; c < l; c++){
            ind = out_index[c];
            Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i] + (c+1)*Sector_Sum_Vector[ind];
        }
        
        Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i] + (l+1)*Sector_Sum_Vector[l];

        for(int c = 0; c < l; c++){
            ind = out_index[c+l+1];
            Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i] + (l-c)*Sector_Sum_Vector[ind];
        }
            
        Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i]/(2*l+1);
    }

    
    for(int i = 0; i < 23; i++){
        if(Sector_Sum_Vector_Filtered[i] < threshold){
            auxiliar_angle[i] = i*section_angle - pi;
        }
        else{
            auxiliar_angle[i] = 999;
        }    
    }
    
    end_angle = atan2((double)(end_position_y-actual_position_y),(double)(end_position_x-actual_position_x));
    
    //Escolhemos a secccao disponivel cujo angulo e mais proximo do angulo final
    for(int i = 0; i < 23; i++){
        angle_difference_end[i] = sqrt((double)(auxiliar_angle[i] - end_angle)*(auxiliar_angle[i] - end_angle));  
    }    
    
    ind = 0;
    for(int i = 0; i < 23; i++){
        if(angle_difference_end[i] < angle_difference_end[ind]){
            ind = i;
        }      
    } 
    
    if(auxiliar_angle[ind] == 999) auxiliar_angle[ind] = 0;  // caso de erro andar em frente
          
    return(auxiliar_angle[ind]);
}

void update_map(double Log_Map_Matrix[80][80], int Aux_Matrix[80][80], int Map_Matrix[80][80], double distance, double theta, double actual_position_x, double actual_position_y){
    double inc = 1;
    int x, y;
    actual_position_x = floor(actual_position_x/50); // saber a celula
    actual_position_y = floor(actual_position_y/50);
    double ang = theta;
        for(double r = inc; r < (distance-inc); r += inc){
            x = actual_position_x+floor(r*cos(ang));
            y = actual_position_y+floor(r*sin(ang));
            if(x >= 0 && y >= 0 && x < 80 && y < 80){
                if(Aux_Matrix[x][y] != 1){
                    Log_Map_Matrix[x][y] = Log_Map_Matrix[x][y] - 0.65;    
                    Aux_Matrix[x][y] = 1;
                    if((1-(1/(1+exp(Log_Map_Matrix[x][y])))) > 0.5){
                        Map_Matrix[x][y] = 1;
                    }
                    else{
                        Map_Matrix[y][x] = 0;
                    }
                }
            }    
        }
        
        x = actual_position_x+floor(distance*cos(ang));
        y = actual_position_y+floor(distance*sin(ang));
        if(x >= 0 && y >= 0 && x < 80 && y < 80){
            if(Aux_Matrix[x][y] != 1){
                Log_Map_Matrix[x][y] = Log_Map_Matrix[x][y] + 0.65;
                Aux_Matrix[x][y] = 1;
                if((1-(1/(1+exp(Log_Map_Matrix[x][y])))) > 0.5){
                    Map_Matrix[x][y] = 1;
                }
                else{
                    Map_Matrix[y][x] = 0;
                }
            }
        }    
    
    //for(double ang=(theta-alpha); ang <= (theta+alpha); ang += alpha){
        for(double r = 0; r < (distance-inc); r += inc){
            x = actual_position_x+floor(r*cos(ang));
            y = actual_position_y+floor(r*sin(ang));
            if(x >= 0 && y >= 0 && x < 80 && y < 80){    
                Aux_Matrix[x][y] = 0;
            }    
        }
        
        x = actual_position_x+floor(distance*cos(theta));
        y = actual_position_y+floor(distance*sin(theta));
        if(x >= 0 && y >= 0 && x < 80 && y < 80){
            Aux_Matrix[x][y] = 0;
        }   
    }