João Sousa
sra-romi
Dependencies: BufferedSerial Matrix
Diff: control.cpp
- Revision:
- 4:1defb279922a
diff -r 0a718d139ed1 -r 1defb279922a control.cpp
--- /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