Diff: main.cpp

Revision:

0:f623431aed01

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 13 15:19:19 2021 +0000
@@ -0,0 +1,100 @@
+#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);
+}
+