File content as of revision 0:f623431aed01:

#include "mbed.h"
#include "Robot.h"
#include "funcs.h"

static const double pi = 3.14159265358;
DigitalIn Button(USER_BUTTON);
Serial pc(SERIAL_TX, SERIAL_RX);


int main() {
    //Espera que o botao seja premido
    while (Button == 1) {}
    wait(1);
    
    //Matrizes de mapeamento
    int Map_Matrix[80][80];             //Matriz do mapa
    int Sector_Matrix_Sections[15][15]; //Matriz com os indices das seccoes
    double Sector_Matrix_Abs[15][15];   //Matriz com as amplitudes 
    int Surroundings_Matrix[15][15];    //Matriz das redondezas do bot
    
    //Ganhos
    double k = 3.3;                       //ganho angular
    
    //Variaveis de posicao
    double x = 150;                     //x inicial
    double y = 150;                     //y inicial
    double phi = 0.01;                  //phi inicial
    double end_position_x = 1200;       //x final
    double end_position_y = 1200;       //y final
    double next_phi;                    //phi seguinte
    
    //Variaveis de estimacao de posicao
    double var_l;                       //contagens do encoder esquerdas
    double var_r;                       //contagens do encoder direitas
    double var_d;                       //variacao linear entre iteracoes
    double var_phi;                     //variacao angular entre iteracoes
    double L = 150;                     //distancia entre rodas
    double r = 35;                      //raio da roda
    
    //Variaveis de comando as rodas
    double w;                           //velociade angular do robo
    double w_left;                      //velocidade roda esquerda
    double w_right;                     //velocidade roda direita
    double v = 2400;                    //velocidade linear do robo
      
    //Funcao de obtencao do Mapa  
    read_map1(Map_Matrix);             
 
    //Funcao para obter a matriz com os indices das seccoes assim como as amplitudes
    get_Sector_Matrix(Sector_Matrix_Sections, Sector_Matrix_Abs);
    
    //Distancia ao ponto objetivo
    double dist = sqrt((double)((x-end_position_x)*(x-end_position_x) + (y-end_position_y)*(y-end_position_y)));
    
    while(dist > 150){
        //Obter as contagens dos encoders
        getCountsAndReset();
    
        //Estimacao de pose
        var_l = (2*pi*r*countsLeft/1440);
        var_r = (2*pi*r*countsRight/1440);
        var_d = (var_l+var_r)/2;
        var_phi= (var_r-var_l)/L;

        if(var_phi == 0){
            x = x + var_d*cos(phi);
            y = y + var_d*sin(phi);
        } 
        else{
            x = x + (var_d)*((sin(phi/2)/(phi/2))*cos(phi + var_phi/2));
            y = y + (var_d)*((sin(phi/2)/(phi/2))*sin(phi + var_phi/2));
            phi = phi + var_phi;
        }
        
        pc.printf("x_atual = %f, y_atual = %f\n",x,y);
        //Distancia ao ponto objetivo
        dist = sqrt((double)(x-end_position_x)*(x-end_position_x) + (y-end_position_y)*(y-end_position_y));
        pc.printf("\ndist = %f\n", dist);
        
        //Funcao para obter o mapa das redondezas do robo
        get_Surroundings_Matrix(Surroundings_Matrix, x, y, Map_Matrix);
        
        //Funcao para obter a direcao que o robo deve tomar em funcao da sua posicao atual e das suas redondezas    
        // defenimos como o limiar 31 !!
        next_phi = get_orientation_angle(x, y, end_position_x, end_position_y, Surroundings_Matrix, Sector_Matrix_Sections, Sector_Matrix_Abs, 31);
        pc.printf("\n\n next phi = %f\n\n", next_phi);
        
        //Controlo proporcional da direcao
        w = k*atan2(sin(next_phi-phi),cos(next_phi-phi));
        
        //Enviar os comandos de velocidades para as rodas
        w_left=(v-(L/2)*w)/r;
        w_right=(v+(L/2)*w)/r;
        setSpeeds(w_left,w_right);    
    }    
    
    setSpeeds(0,0);
    return(0);
}