Revision 0:bf6e23027ccb, committed 2021-05-13

Comitter:

xaficz

Date:

Thu May 13 15:16:49 2021 +0000

Commit message:

vfh__

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.cpp	Thu May 13 15:16:49 2021 +0000
@@ -0,0 +1,63 @@
+#include "Robot.h"
+#include "mbed.h"
+
+I2C i2c(I2C_SDA, I2C_SCL );
+const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
+
+int16_t countsLeft = 0;
+int16_t countsRight = 0;
+
+void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
+{
+    char buffer[5];
+    
+    buffer[0] = 0xA1;
+    memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
+    memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));
+
+    i2c.write(addr8bit, buffer, 5); // 5 bytes
+}
+
+void setLeftSpeed(int16_t speed)
+{
+    char buffer[3];
+    
+    buffer[0] = 0xA2;
+    memcpy(&buffer[1], &speed, sizeof(speed));
+
+    i2c.write(addr8bit, buffer, 3); // 3 bytes
+}
+
+void setRightSpeed(int16_t speed)
+{
+    char buffer[3];
+    
+    buffer[0] = 0xA3;
+    memcpy(&buffer[1], &speed, sizeof(speed));
+
+    i2c.write(addr8bit, buffer, 3); // 3 bytes
+}
+
+void getCounts()
+{
+    char write_buffer[2];
+    char read_buffer[4];
+    
+    write_buffer[0] = 0xA0;
+    i2c.write(addr8bit, write_buffer, 1); wait_us(100);
+    i2c.read( addr8bit, read_buffer, 4);
+    countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
+    countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
+}
+
+void getCountsAndReset()
+{
+    char write_buffer[2];
+    char read_buffer[4];
+    
+    write_buffer[0] = 0xA4;
+    i2c.write(addr8bit, write_buffer, 1); wait_us(100);
+    i2c.read( addr8bit, read_buffer, 4);
+    countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
+    countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.h	Thu May 13 15:16:49 2021 +0000
@@ -0,0 +1,49 @@
+/** @file */
+#ifndef ROBOT_H_
+#define ROBOT_H_
+
+#include "mbed.h"
+
+extern int16_t countsLeft;
+extern int16_t countsRight;
+
+/** \brief Sets the speed for both motors.
+ *
+ * \param leftSpeed A number from -300 to 300 representing the speed and
+ * direction of the left motor. Values of -300 or less result in full speed
+ * reverse, and values of 300 or more result in full speed forward.
+ * \param rightSpeed A number from -300 to 300 representing the speed and
+ * direction of the right motor. Values of -300 or less result in full speed
+ * reverse, and values of 300 or more result in full speed forward. */
+void setSpeeds(int16_t leftSpeed, int16_t rightSpeed);
+
+/** \brief Sets the speed for the left motor.
+ *
+ * \param speed A number from -300 to 300 representing the speed and
+ * direction of the left motor.  Values of -300 or less result in full speed
+ * reverse, and values of 300 or more result in full speed forward. */
+void setLeftSpeed(int16_t speed);
+
+/** \brief Sets the speed for the right motor.
+ *
+ * \param speed A number from -300 to 300 representing the speed and
+ * direction of the right motor. Values of -300 or less result in full speed
+ * reverse, and values of 300 or more result in full speed forward. */
+void setRightSpeed(int16_t speed);
+
+/*! Returns the number of counts that have been detected from the both
+ * encoders.  These counts start at 0.  Positive counts correspond to forward
+ * movement of the wheel of the Romi, while negative counts correspond
+ * to backwards movement.
+ *
+ * The count is returned as a signed 16-bit integer.  When the count goes
+ * over 32767, it will overflow down to -32768.  When the count goes below
+ * -32768, it will overflow up to 32767. */
+void getCounts();
+
+/*! This function is just like getCounts() except it also clears the
+ *  counts before returning.  If you call this frequently enough, you will
+ *  not have to worry about the count overflowing. */
+void getCountsAndReset();
+
+#endif /* ROBOT_H_ */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/funcs.cpp	Thu May 13 15:16:49 2021 +0000
@@ -0,0 +1,98 @@
+#include "mbed.h"
+#include "math.h"
+#include <stdio.h>
+#include "funcs.h"
+
+
+Serial pc1(SERIAL_TX, SERIAL_RX);
+
+//Funcao para criar um mapa simples com um quadrado no meio
+void read_map1(int Map_Matrix[80][80]){
+    for(int i = 0; i < 80; i++){
+        for(int j = 0; j < 80; j++){
+            if(i == 0 || j==0 || i==79 || j==79){
+                Map_Matrix[i][j] = 1;  
+            }     
+            else{
+                Map_Matrix[i][j] = 0;
+                if(i >= 10 && j>=10 && i<=69 && j<=69){
+                    Map_Matrix[i][j] = 1;
+                }
+            }              
+        }
+    } 
+}
+
+//Funcao para criar um mapa mias pequeno com um quadrado no meio
+void read_map2(int Map_Matrix[80][80]){
+    for(int i = 0; i < 80; i++){
+        for(int j = 0; j < 80; j++){
+            Map_Matrix[i][j] = 0;  
+            
+            if((i >= 30 || j >= 30) || (i >= 10 && j >= 10 && i <= 15 && j <= 15) || (i >= 17 && j >= 17 && i <= 23  && j <= 23)){
+                Map_Matrix[i][j] = 1;
+            } 
+        }
+    } 
+}
+
+//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]){
+    double map_position_x1 = floor(actual_position_x/50);
+    double map_position_y1 = floor(actual_position_y/50);
+    
+    int map_position_x = (int) map_position_x1;
+    int map_position_y = (int) map_position_y1;
+    
+    int i = 0;
+    int j = 0;
+    for(int x = map_position_x-7; x < map_position_x+8; x++){
+        for(int y = map_position_y-7; 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;
+    }
+    
+    for(int x = 0; x < 15; x++){ 
+        for(int y = 0; y < 15; y++){
+            pc1.printf("%d,", Surroundings_Matrix[x][y]);  
+        }
+        pc1.printf("\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]){
+    double Force_vector_x;
+    double Force_vector_y;
+    double distance;
+    double orientation_angle;
+    
+    distance = sqrt((double)(end_position_x-actual_position_x)*(end_position_x-actual_position_x)+(end_position_y-actual_position_y)*(end_position_y-actual_position_y));
+    
+    //Vetor forca atrativa
+    Force_vector_x = 2*(end_position_x - actual_position_x)/(distance);
+    Force_vector_y = 2*(end_position_y - actual_position_y)/(distance);
+    pc1.printf("x = %f, y = %f\n", Force_vector_x, Force_vector_y);
+    
+    //Subtracao das forcas repulsivas
+    for(int x = 0; x < 15; x++){
+        for(int y = 0; y < 15; y++){
+            if(!(x == 7 && y == 7)){
+                distance = sqrt((double)(x-7)*(x-7) + (y-7)*(y-7));
+                Force_vector_x = Force_vector_x - (Surroundings_Matrix[x][y]/(distance*distance))*((x-7)/distance);
+                Force_vector_y = Force_vector_y - (Surroundings_Matrix[x][y]/(distance*distance))*((y-7)/distance);
+             }
+        }
+    }
+
+    orientation_angle = atan2(Force_vector_y,Force_vector_x);
+    return(orientation_angle);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/funcs.h	Thu May 13 15:16:49 2021 +0000
@@ -0,0 +1,7 @@
+void read_map1(int Map_Matrix[80][80]);
+
+void read_map2(int Map_Matrix[80][80]);
+
+void get_Surroundings_Matrix(int Surroundings_Matrix[15][15], double actual_position_x, double actual_position_y, int Map_Matrix[80][80]);
+
+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]);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 13 15:16:49 2021 +0000
@@ -0,0 +1,110 @@
+#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 Surroundings_Matrix[15][15];    //Matriz das redondezas
+    
+    //Ganhos
+    double k = 8;                       //Ganho angular
+    
+    //Variaveis de posicao
+    double x = 300;                     //x inicial
+    double y = 300;                     //y inicial
+    double phi = 0.01;                  //phi inicial
+    double end_position_x = 1400;       //x final
+    double end_position_y = 1400;       //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 da roda esquerda    
+    double w_right;                     //velocidade da roda direita 
+    double v = 2400;                    //velocidade linear do robo
+    /*
+    double erro = 0;
+    double e = 0;
+    double d = 20;          //5 contimetros para ir andando para a frente
+    float kv = 3;
+    float ki = 0.1;    // nao conseguimos obter ganhos adquados para o controlo pretendido
+                        // pelo que considermos a velocidade constante ao longo do nosso programa
+                        
+    */
+    
+    //Funcao de obtencao do Mapa    
+    read_map2(Map_Matrix);              
+    
+    while(sqrt((double)pow(x-end_position_x,2) + pow(y-end_position_y,2)) > 100){
+        //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);
+        
+        //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
+        next_phi = get_orientation_angle(x, y, end_position_x, end_position_y, Surroundings_Matrix);
+        pc.printf("nphi = %f\n", next_phi);
+        
+        
+        pc.printf("%f\n",sqrt((double)((x-end_position_x)*(x-end_position_x) + (y-end_position_y)*(y-end_position_y))));
+        
+       /* 
+        e = sqrt(pow(end_position_x - x, 2) + pow(end_position_y - y, 2)) - d;
+        erro = erro + e;
+        v = kv*e + ki*erro;
+        
+        pc.printf("%f\n",erro);
+        pc.printf("%f\n",e);
+        pc.printf("%f\n",v);
+        */
+        
+        //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);
+}        
+        

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu May 13 15:16:49 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file