Revision 4:1defb279922a, committed 2021-05-11

Comitter:

joaopsousa99

Date:

Tue May 11 18:10:22 2021 +0000

Parent:

3:0a718d139ed1

Commit message:

as.djvblaskdvj

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix.lib	Tue May 11 18:10:22 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Yo_Robot/code/Matrix/#a4014ab0a8cf

--- a/Robot.cpp	Wed Apr 24 16:00:07 2019 +0000
+++ b/Robot.cpp	Tue May 11 18:10:22 2021 +0000
@@ -1,12 +1,27 @@
+#include <math.h>
 #include "Robot.h"
 #include "mbed.h"
+#include "control.h"
+#include "I2C.h"
 
-I2C i2c(I2C_SDA, I2C_SCL );
+I2C i2c(PB_9, PB_8);
 const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
 
+const int16_t COUNTS_PER_ROTATION = 1440;   // ticks dos encoders por volta do motor
+const int16_t WHEEL_DIAMETER = 7;           // diâmetro das rodas, em centímetros
+const int16_t WHEEL_DISTANCE = 14;
+const float MAX_LIN_VEL = 20;
+const float MIN_LIN_VEL = -20;
+const float MIN_SET_SPEEDS = -100;
+const float MAX_SET_SPEEDS = 100;
+
 int16_t countsLeft = 0;
 int16_t countsRight = 0;
 
+float poseX = 0;
+float poseY = 50;
+float poseTheta = 0;
+
 void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
 {
     char buffer[5];
@@ -60,4 +75,43 @@
     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]));
+}
+
+float mapSpeeds(float value){
+    value = (value - MIN_LIN_VEL) * (MAX_SET_SPEEDS - MIN_SET_SPEEDS)/(MAX_LIN_VEL - MIN_LIN_VEL) + MIN_SET_SPEEDS;
+
+    if(value < 35 && value > 0)
+        value = 35;
+    if(value > -35 && value < 0)
+        value = -35;
+    if(value > 150)
+        value = 150;
+    if(value < -150)
+        value = -150;
+
+    return value;
+}
+
+void updatePose(){
+    //printf("início do updatePose; ");
+    getCountsAndReset();
+
+    float Dl = (2 * 3.14 * countsLeft) * (WHEEL_DIAMETER / 2) / COUNTS_PER_ROTATION;
+
+    float Dr = (2 * 3.14 * countsRight) * (WHEEL_DIAMETER / 2) / COUNTS_PER_ROTATION;
+
+    float D = (Dl + Dr) / 2;
+
+    float deltaTheta = (Dr - Dl) / WHEEL_DISTANCE;
+
+    poseX = poseX + D * cos(poseTheta + deltaTheta / 2);
+    poseY = poseY + D * sin(poseTheta + deltaTheta / 2);
+    poseTheta = poseTheta + deltaTheta;
+    poseTheta = atan2(sin(poseTheta), cos(poseTheta));
+    //printf("fim do updatePose; ");
+}
+
+void robotVel2wheelVel(float v, float w, float *wheelSpeeds){
+    wheelSpeeds[0] = (v - (WHEEL_DISTANCE / 2) * w) / (WHEEL_DIAMETER / 2);
+    wheelSpeeds[1] = (v + (WHEEL_DISTANCE / 2) * w) / (WHEEL_DIAMETER / 2);
 }
\ No newline at end of file

--- a/Robot.h	Wed Apr 24 16:00:07 2019 +0000
+++ b/Robot.h	Tue May 11 18:10:22 2021 +0000
@@ -3,10 +3,23 @@
 #define ROBOT_H_
 
 #include "mbed.h"
+#include <cstdint>
+
+extern const int16_t COUNTS_PER_ROTATION;
+extern const int16_t WHEEL_DIAMETER;
+extern const int16_t WHEEL_DISTANCE;
+extern const float MAX_LIN_VEL;
+extern const float MIN_LIN_VEL;
+extern const float MIN_SET_SPEEDS;
+extern const float MAX_SET_SPEEDS;
 
 extern int16_t countsLeft;
 extern int16_t countsRight;
 
+extern float poseX;
+extern float poseY;
+extern float poseTheta;
+
 /** \brief Sets the speed for both motors.
  *
  * \param leftSpeed A number from -300 to 300 representing the speed and
@@ -46,4 +59,10 @@
  *  not have to worry about the count overflowing. */
 void getCountsAndReset();
 
+void updatePose();
+
+void robotVel2wheelVel(float v, float w, float *wheelSpeeds);
+
+float mapSpeeds(float value);
+
 #endif /* ROBOT_H_ */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control.cpp	Tue May 11 18:10:22 2021 +0000
@@ -0,0 +1,249 @@
+#include <cstdint>
+#include <cstdio>
+#include <cstdlib>
+#include <math.h>
+#include "Communication.h"
+#include "control.h"
+#include "mbed.h"
+#include "Robot.h"
+#include "Matrix.h"
+
+const float TIMESTEP = 0.05;
+const float CELL_SIZE = 5;
+const int MAP_SIZE = 100;
+const int GRID_SIZE = int(MAP_SIZE/CELL_SIZE);
+const int WINDOW_SIZE = int(45/CELL_SIZE);
+const float PI = 3.1415926;
+
+Matrix create_occupation_grid(){
+    Matrix occupationGrid = Matrix(GRID_SIZE,GRID_SIZE);
+    for(int i = 1; i <= 20; i++)
+        for(int j = 1;j <= 20; j++)
+            if (i > 8 && i < 12 && j > 8 && j < 12)
+                occupationGrid.add(i,j,1);
+
+    return occupationGrid;
+}
+/*int occupationGrid[GRID_SIZE][GRID_SIZE] = {{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}};*/
+/*int occupationGrid[GRID_SIZE][GRID_SIZE] = {{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
+                                            {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}};*/
+
+void moveToPoint(float xObj, float yObj){
+    float kv = 0.2;
+    float kw = 10;
+    
+    float d = sqrt((xObj - poseX)*(xObj - poseX) + (yObj - poseY)*(yObj - poseY));
+    float angleError = atan2(yObj - poseY, xObj - poseX);
+    //printf("ANGLE ERROR: %.1f    ", angleError*180/PI);
+    angleError = atan2(sin(angleError - poseTheta), cos(angleError - poseTheta));   // equivalente a wrapToPi()
+
+    float wheelSpeeds[2];
+
+    robotVel2wheelVel(kv*d, kw*angleError, wheelSpeeds);
+
+    wheelSpeeds[0] = mapSpeeds(wheelSpeeds[0]);
+    wheelSpeeds[1] = mapSpeeds(wheelSpeeds[1]);
+
+    //printf("wheel speeds: [%.0f, %.0f]\t", wheelSpeeds[0], wheelSpeeds[1]);
+
+    setSpeeds(wheelSpeeds[0], wheelSpeeds[1]);
+}
+
+float vff(Matrix occupationGrid, int *xactive, int *yactive, int lenx, int leny, float *objCellCenter, Serial pc){
+    //printf("i vff; ");
+    float Fcr = 35;
+    float Fca = 1.5;
+
+    // não se sabe quantas células estão ocupadas então aloca-se espaço para todas para mais
+    // à frente guardar apenas as ocupadas e apagar estes arrays
+    //int *auxRows, *auxCols;
+
+    //auxRows = (int *)calloc(lenx*leny, sizeof(int));
+    //auxCols = (int *)calloc(lenx*leny, sizeof(int));
+
+    //int occupiedCounter = 0;
+    //int auxCounter = 0;
+
+    // pesquisa a grelha de ocupação e guarda os índices da janela ativa que correspondem a 
+    // células ocupadas
+    float Frx = 0;
+    float Fry = 0;
+
+    float xCell, yCell;
+    float value;
+
+    for(int i = 0; i < lenx; i++){
+        for(int j = 0; j < leny; j++){
+            // necessario somar 1 porque os indices na matriz comecam em 1 
+            value = occupationGrid(xactive[i]+1,yactive[j]+1);
+            //printf("occupationGrid(%d, %d): %.0f", xactive[i]+1, yactive[j]+1, value);
+            if(value == 1){
+                //auxRows[auxCounter] = xactive[i];
+                //auxCols[auxCounter] = yactive[j];
+                //occupiedCounter++;
+
+                xCell = xactive[i]*CELL_SIZE + CELL_SIZE/2;
+                yCell = yactive[i]*CELL_SIZE + CELL_SIZE/2;
+                Frx += Fcr*(xCell - poseX)/pow(sqrt(pow(xCell - poseX, 2) + pow(yCell - poseY, 2)), 3);
+                Fry += Fcr*(yCell - poseY)/pow(sqrt(pow(xCell - poseX, 2) + pow(yCell - poseY, 2)), 3);
+            }
+            else{
+               continue; 
+                //auxRows[auxCounter] = -1;
+                //auxCols[auxCounter] = -1;
+            }
+            //auxCounter++;
+        }
+    }
+    //pc.printf("Fr = [%.1f, %.1f]    ", Frx, Fry);
+    float Fax = Fca*(objCellCenter[0] - poseX)/(sqrt(pow(objCellCenter[0] - poseX, 2) + pow(objCellCenter[1] - poseY, 2)));
+    float Fay = Fca*(objCellCenter[1] - poseY)/(sqrt(pow(objCellCenter[0] - poseX, 2) + pow(objCellCenter[1] - poseY, 2)));
+    //pc.printf("Fa = [%.1f, %.1f]    ", Fax, Fay);
+    
+    /*if(occupiedCounter == 0){
+        float F[2] = {Fax, Fay};
+        printf("f vff; ");
+        return atan2(F[1], F[0]);
+    }*/
+
+    /*int *xOccupied, *yOccupied;
+    xOccupied = (int *)calloc(occupiedCounter, sizeof(int));
+    yOccupied = (int *)calloc(occupiedCounter, sizeof(int));
+
+    occupiedCounter = 0;
+
+    for(int i = 0; i < lenx*leny; i++){
+        if(auxRows[i] != -1){
+            xOccupied[occupiedCounter] = auxRows[i];
+            yOccupied[occupiedCounter] = auxCols[i];
+            occupiedCounter++;
+        }
+    }
+
+    for (int i = 0; i < occupiedCounter; i++) {
+        xCell = xOccupied[i]*CELL_SIZE + CELL_SIZE/2;
+        yCell = yOccupied[i]*CELL_SIZE + CELL_SIZE/2;
+
+        Frx += Fcr*(xCell - poseX)/pow(sqrt(pow(xCell - poseX, 2) + pow(yCell - poseY, 2)), 3);
+        Fry += Fcr*(yCell - poseY)/pow(sqrt(pow(xCell - poseX, 2) + pow(yCell - poseY, 2)), 3);
+    }
+
+    free(auxCols);
+    free(auxRows);
+    free(xOccupied);
+    free(yOccupied);*/
+
+    float F[2] = {Fax - Frx, Fay - Fry};
+    //printf("f vff; ");
+    return atan2(F[1], F[0]);
+}
+
+void followPath(float &objAheadX, float &objAheadY, float &intError, float *objCellCenter, Matrix occupationGrid, Serial pc){
+    //printf("i followPath; ");
+    float kv = 0.3;
+    float ki = 0.2;
+    float kw = 10;
+    int vObj = 7;  // temos de fazer experiências para afinar isto
+    float vffAngle = poseTheta;
+    int dObj = 3;
+
+    float xmax = floor(static_cast<float>(poseX/CELL_SIZE)) + floor(static_cast<float>(WINDOW_SIZE/2));
+    float xmin = floor(static_cast<float>(poseX/CELL_SIZE)) - floor(static_cast<float>(WINDOW_SIZE/2));
+    float ymax = floor(static_cast<float>(poseY/CELL_SIZE)) + floor(static_cast<float>(WINDOW_SIZE/2));
+    float ymin = floor(static_cast<float>(poseY/CELL_SIZE)) - floor(static_cast<float>(WINDOW_SIZE/2));
+
+    if(xmin < 0)
+        xmin = 0;
+    if(xmax >= GRID_SIZE)   
+        xmax = GRID_SIZE - 1;
+    if(ymin < 0)
+        ymin = 0;
+    if(ymax >= GRID_SIZE)   
+        ymax = GRID_SIZE - 1;
+    
+    int lenx = xmax - xmin + 1;
+    int leny = ymax - ymin + 1;
+
+    // pre alocacao de espaco
+    int *xactive, *yactive;
+    xactive = (int *)calloc(lenx, sizeof(int));
+    yactive = (int *)calloc(leny, sizeof(int));
+
+    // linspace do matlab 
+    for(int j = 0; j < lenx; j++)
+        xactive[j] = xmin + j;
+
+    for(int i = 0; i < leny; i++)
+        yactive[i] = ymin + i;
+
+    // repmat do matlab
+    //int *xactive = repeat_matrix2(x, lenx, leny);
+    //int *yactive = repeat_matrix(y, leny, lenx);
+
+    //int activeSize = lenx*leny;
+
+    vffAngle = vff(occupationGrid, xactive, yactive, lenx, leny, objCellCenter, pc);
+
+    objAheadX = objAheadX + TIMESTEP * vObj * cos(vffAngle);
+    objAheadY = objAheadY + TIMESTEP * vObj * sin(vffAngle);
+    
+    float distError = sqrt(pow(objAheadY - poseY, 2) + pow(objAheadX - poseX, 2)) - dObj;
+
+    intError = intError + distError * TIMESTEP;
+
+    float v = kv * distError + ki * intError;
+    float phiObj = atan2(objAheadY - poseY, objAheadX - poseX);
+    float w = kw*atan2(sin(phiObj - poseTheta), cos(phiObj - poseTheta));
+    float *wheelSpeeds;
+    wheelSpeeds = (float *)calloc(2, sizeof(float));
+
+    robotVel2wheelVel(v, w, wheelSpeeds);
+
+    wheelSpeeds[0] = mapSpeeds(wheelSpeeds[0]);
+    wheelSpeeds[1] = mapSpeeds(wheelSpeeds[1]);
+
+    setSpeeds((int)wheelSpeeds[0], (int)wheelSpeeds[1]);
+
+    free(wheelSpeeds);
+    free(xactive);
+    free(yactive);
+    
+    //printf("f followPath; ");
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control.h	Tue May 11 18:10:22 2021 +0000
@@ -0,0 +1,17 @@
+#ifndef CONTROL_H_
+#define CONTROL_H_
+
+#include "mbed.h"
+#include "Matrix.h"
+
+extern const float TIMESTEP;
+extern const float PI;
+extern const float CELL_SIZE;
+extern const int GRID_SIZE;
+
+void moveToPoint(float xObj, float yObj);
+void followPath(float &objAheadX, float &objAheadY, float &intError, float *objCellCenter, Matrix occupationGrid, Serial pc);
+float vff(Matrix occupationGrid, int *xactive, int *yactive, int activeSize, float *objCellCenter, Serial pc);
+Matrix create_occupation_grid();
+
+#endif
\ No newline at end of file

--- a/main.cpp	Wed Apr 24 16:00:07 2019 +0000
+++ b/main.cpp	Tue May 11 18:10:22 2021 +0000
@@ -1,38 +1,34 @@
-// Coded by Luís Afonso 11-04-2019
 #include "mbed.h"
-#include "BufferedSerial.h"
-#include "rplidar.h"
 #include "Robot.h"
 #include "Communication.h"
-
-Serial pc(SERIAL_TX, SERIAL_RX);
-RPLidar lidar;
-BufferedSerial se_lidar(PA_9, PA_10);
-PwmOut rplidar_motor(D3);
+#include "control.h"
+//#include <Serial.h>
 
-int main()
-{
-    float odomX, odomY, odomTheta;
-    struct RPLidarMeasurement data;
-    
-    pc.baud(115200);
-    init_communication(&pc);
+Serial pc(USBTX, USBRX);
+//PwmOut rplidar_motor(D3);
 
-    // Lidar initialization
-    rplidar_motor.period(0.001f);
-    lidar.begin(se_lidar);
-    lidar.setAngle(0,360);
+int main() {
+    //pc.baud(115200);
+    //init_communication(&pc);
+    Matrix occupationGrid;
+    occupationGrid = create_occupation_grid();
+    float xObj = 99.9;
+    float yObj = poseY;
+    float error = 0;
+    float intError = 0;
+    float objAheadX = poseX;
+    float objAheadY = poseY;
+    float objCellCenter[2] = {floor(xObj / CELL_SIZE) * CELL_SIZE + (CELL_SIZE / 2),
+                              floor(yObj / CELL_SIZE) * CELL_SIZE + (CELL_SIZE / 2)};
 
-    pc.printf("Program started.\n");
-        
-    lidar.startThreadScan();
-    
-    while(1) {
-        // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
-        if(lidar.pollSensorData(&data) == 0)
-        {
-            pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
-        }
-       wait(0.01); 
-    }
+    do{
+        updatePose();
+        followPath(objAheadX, objAheadY, intError, objCellCenter, occupationGrid, pc);
+        error = sqrt((xObj - poseX)*(xObj - poseX) + (yObj - poseY)*(yObj - poseY));
+        //printf("error = %f   ", error);
+        printf("pose = [%.1f, %.1f]\n", poseX, poseY);
+        wait_us(TIMESTEP*1000000);
+    } while(error > CELL_SIZE);
+
+    setSpeeds(0, 0);
 }
\ No newline at end of file