Revision 0:f623431aed01, committed 2021-05-13
- Comitter:
- xaficz
- Date:
- Thu May 13 15:19:19 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:19:19 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:19:19 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:19:19 2021 +0000
@@ -0,0 +1,225 @@
+#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 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++){
+ Map_Matrix[i][j] = 0;
+
+ if((i >= 30 || j >= 30) || (i >= 5 && j >=5 && i <= 10 && j <= 10) || (i >= 5 && j >= 20 && i <= 10 && j <= 25)){
+ Map_Matrix[i][j] = 1;
+ }
+ }
+ }
+}
+
+//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)));
+ }
+ }
+ /*
+ for(int x = 0; x < 15; x++){
+ for(int y = 0; y < 15; y++){
+ pc1.printf("%d,",Sector_Matrix_Sections[x][y]);
+ }
+ pc1.printf("\n");
+ }
+ */
+}
+
+//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;
+ }
+ pc1.printf("matriz surroundings:\n");
+ 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 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 out_index[23];
+ int ind;
+ int l = 4;
+
+ 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];
+ }
+ }
+ /*
+ pc1.printf("matriz abs:\n");
+ for(int x = 0; x < 15; x++){
+ for(int y = 0; y < 15; y++){
+ pc1.printf("%f,", Surroundings_Matrix_Aux[x][y]);
+ }
+ pc1.printf("\n");
+ }
+ */
+
+ //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];
+ }
+ }
+
+ /*
+ pc1.printf("tabela s filtro:\n");
+ for(int n = 0; n < 23; n++){
+ pc1.printf("%f\n", Sector_Sum_Vector[n]);
+ }
+ */
+
+ //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);
+ }
+
+ /*
+ pc1.printf("\ntabela c f:\n");
+ for(int n = 0; n < 23; n++){
+ pc1.printf("%f\n", Sector_Sum_Vector_Filtered[n]);
+ }
+ */
+
+ for(int i = 0; i < 23; i++){
+ if(Sector_Sum_Vector_Filtered[i] < threshold){
+ auxiliar_angle[i] = i*section_angle - pi; // subtraimos o pi
+ } // para que os angulos rodem 180 graus
+ else{
+ auxiliar_angle[i] = 0; // trajectoria negada
+ }
+ }
+
+ 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;
+ }
+ }
+
+ return(auxiliar_angle[ind]);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/funcs.h Thu May 13 15:19:19 2021 +0000
@@ -0,0 +1,9 @@
+void read_map1(int Map_Matrix[80][80]);
+
+void get_Sector_Matrix(int Sector_Matrix_Sections[15][15], double Sector_Matrix_Abs[15][15]);
+
+void get_Surroundings_Matrix(int Surroundings_Matrix[15][15], double actual_position_x, double actual_position_y, int Map_Matrix[80][80]);
+
+void find_index(int given_index, int l, int out_index[23]);
+
+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);
--- /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);
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld Thu May 13 15:19:19 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file