João Sousa / Mbed OS romi-vff

sra-romi

Dependencies:   BufferedSerial Matrix

control.cpp@4:1defb279922a, 2021-05-11 (annotated)

Committer:
joaopsousa99
Date:
Tue May 11 18:10:22 2021 +0000
Revision:
4:1defb279922a
as.djvblaskdvj

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joaopsousa99 4:1defb279922a 1 #include <cstdint>
joaopsousa99 4:1defb279922a 2 #include <cstdio>
joaopsousa99 4:1defb279922a 3 #include <cstdlib>
joaopsousa99 4:1defb279922a 4 #include <math.h>
joaopsousa99 4:1defb279922a 5 #include "Communication.h"
joaopsousa99 4:1defb279922a 6 #include "control.h"
joaopsousa99 4:1defb279922a 7 #include "mbed.h"
joaopsousa99 4:1defb279922a 8 #include "Robot.h"
joaopsousa99 4:1defb279922a 9 #include "Matrix.h"
joaopsousa99 4:1defb279922a 10
joaopsousa99 4:1defb279922a 11 const float TIMESTEP = 0.05;
joaopsousa99 4:1defb279922a 12 const float CELL_SIZE = 5;
joaopsousa99 4:1defb279922a 13 const int MAP_SIZE = 100;
joaopsousa99 4:1defb279922a 14 const int GRID_SIZE = int(MAP_SIZE/CELL_SIZE);
joaopsousa99 4:1defb279922a 15 const int WINDOW_SIZE = int(45/CELL_SIZE);
joaopsousa99 4:1defb279922a 16 const float PI = 3.1415926;
joaopsousa99 4:1defb279922a 17
joaopsousa99 4:1defb279922a 18 Matrix create_occupation_grid(){
joaopsousa99 4:1defb279922a 19 Matrix occupationGrid = Matrix(GRID_SIZE,GRID_SIZE);
joaopsousa99 4:1defb279922a 20 for(int i = 1; i <= 20; i++)
joaopsousa99 4:1defb279922a 21 for(int j = 1;j <= 20; j++)
joaopsousa99 4:1defb279922a 22 if (i > 8 && i < 12 && j > 8 && j < 12)
joaopsousa99 4:1defb279922a 23 occupationGrid.add(i,j,1);
joaopsousa99 4:1defb279922a 24
joaopsousa99 4:1defb279922a 25 return occupationGrid;
joaopsousa99 4:1defb279922a 26 }
joaopsousa99 4:1defb279922a 27 /*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},
joaopsousa99 4:1defb279922a 28 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 29 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 30 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 31 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 32 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 33 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 34 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 35 {0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 36 {0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 37 {0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 38 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 39 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 40 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 41 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 42 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 43 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 44 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 45 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 46 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}};*/
joaopsousa99 4:1defb279922a 47 /*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},
joaopsousa99 4:1defb279922a 48 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 49 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 50 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 51 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 52 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 53 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 54 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 55 {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 56 {0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 57 {0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 58 {0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 59 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 60 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 61 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 62 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 63 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 64 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 65 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
joaopsousa99 4:1defb279922a 66 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}};*/
joaopsousa99 4:1defb279922a 67
joaopsousa99 4:1defb279922a 68 void moveToPoint(float xObj, float yObj){
joaopsousa99 4:1defb279922a 69 float kv = 0.2;
joaopsousa99 4:1defb279922a 70 float kw = 10;
joaopsousa99 4:1defb279922a 71
joaopsousa99 4:1defb279922a 72 float d = sqrt((xObj - poseX)*(xObj - poseX) + (yObj - poseY)*(yObj - poseY));
joaopsousa99 4:1defb279922a 73 float angleError = atan2(yObj - poseY, xObj - poseX);
joaopsousa99 4:1defb279922a 74 //printf("ANGLE ERROR: %.1f ", angleError*180/PI);
joaopsousa99 4:1defb279922a 75 angleError = atan2(sin(angleError - poseTheta), cos(angleError - poseTheta)); // equivalente a wrapToPi()
joaopsousa99 4:1defb279922a 76
joaopsousa99 4:1defb279922a 77 float wheelSpeeds[2];
joaopsousa99 4:1defb279922a 78
joaopsousa99 4:1defb279922a 79 robotVel2wheelVel(kv*d, kw*angleError, wheelSpeeds);
joaopsousa99 4:1defb279922a 80
joaopsousa99 4:1defb279922a 81 wheelSpeeds[0] = mapSpeeds(wheelSpeeds[0]);
joaopsousa99 4:1defb279922a 82 wheelSpeeds[1] = mapSpeeds(wheelSpeeds[1]);
joaopsousa99 4:1defb279922a 83
joaopsousa99 4:1defb279922a 84 //printf("wheel speeds: [%.0f, %.0f]\t", wheelSpeeds[0], wheelSpeeds[1]);
joaopsousa99 4:1defb279922a 85
joaopsousa99 4:1defb279922a 86 setSpeeds(wheelSpeeds[0], wheelSpeeds[1]);
joaopsousa99 4:1defb279922a 87 }
joaopsousa99 4:1defb279922a 88
joaopsousa99 4:1defb279922a 89 float vff(Matrix occupationGrid, int *xactive, int *yactive, int lenx, int leny, float *objCellCenter, Serial pc){
joaopsousa99 4:1defb279922a 90 //printf("i vff; ");
joaopsousa99 4:1defb279922a 91 float Fcr = 35;
joaopsousa99 4:1defb279922a 92 float Fca = 1.5;
joaopsousa99 4:1defb279922a 93
joaopsousa99 4:1defb279922a 94 // não se sabe quantas células estão ocupadas então aloca-se espaço para todas para mais
joaopsousa99 4:1defb279922a 95 // à frente guardar apenas as ocupadas e apagar estes arrays
joaopsousa99 4:1defb279922a 96 //int *auxRows, *auxCols;
joaopsousa99 4:1defb279922a 97
joaopsousa99 4:1defb279922a 98 //auxRows = (int *)calloc(lenx*leny, sizeof(int));
joaopsousa99 4:1defb279922a 99 //auxCols = (int *)calloc(lenx*leny, sizeof(int));
joaopsousa99 4:1defb279922a 100
joaopsousa99 4:1defb279922a 101 //int occupiedCounter = 0;
joaopsousa99 4:1defb279922a 102 //int auxCounter = 0;
joaopsousa99 4:1defb279922a 103
joaopsousa99 4:1defb279922a 104 // pesquisa a grelha de ocupação e guarda os índices da janela ativa que correspondem a
joaopsousa99 4:1defb279922a 105 // células ocupadas
joaopsousa99 4:1defb279922a 106 float Frx = 0;
joaopsousa99 4:1defb279922a 107 float Fry = 0;
joaopsousa99 4:1defb279922a 108
joaopsousa99 4:1defb279922a 109 float xCell, yCell;
joaopsousa99 4:1defb279922a 110 float value;
joaopsousa99 4:1defb279922a 111
joaopsousa99 4:1defb279922a 112 for(int i = 0; i < lenx; i++){
joaopsousa99 4:1defb279922a 113 for(int j = 0; j < leny; j++){
joaopsousa99 4:1defb279922a 114 // necessario somar 1 porque os indices na matriz comecam em 1
joaopsousa99 4:1defb279922a 115 value = occupationGrid(xactive[i]+1,yactive[j]+1);
joaopsousa99 4:1defb279922a 116 //printf("occupationGrid(%d, %d): %.0f", xactive[i]+1, yactive[j]+1, value);
joaopsousa99 4:1defb279922a 117 if(value == 1){
joaopsousa99 4:1defb279922a 118 //auxRows[auxCounter] = xactive[i];
joaopsousa99 4:1defb279922a 119 //auxCols[auxCounter] = yactive[j];
joaopsousa99 4:1defb279922a 120 //occupiedCounter++;
joaopsousa99 4:1defb279922a 121
joaopsousa99 4:1defb279922a 122 xCell = xactive[i]*CELL_SIZE + CELL_SIZE/2;
joaopsousa99 4:1defb279922a 123 yCell = yactive[i]*CELL_SIZE + CELL_SIZE/2;
joaopsousa99 4:1defb279922a 124 Frx += Fcr*(xCell - poseX)/pow(sqrt(pow(xCell - poseX, 2) + pow(yCell - poseY, 2)), 3);
joaopsousa99 4:1defb279922a 125 Fry += Fcr*(yCell - poseY)/pow(sqrt(pow(xCell - poseX, 2) + pow(yCell - poseY, 2)), 3);
joaopsousa99 4:1defb279922a 126 }
joaopsousa99 4:1defb279922a 127 else{
joaopsousa99 4:1defb279922a 128 continue;
joaopsousa99 4:1defb279922a 129 //auxRows[auxCounter] = -1;
joaopsousa99 4:1defb279922a 130 //auxCols[auxCounter] = -1;
joaopsousa99 4:1defb279922a 131 }
joaopsousa99 4:1defb279922a 132 //auxCounter++;
joaopsousa99 4:1defb279922a 133 }
joaopsousa99 4:1defb279922a 134 }
joaopsousa99 4:1defb279922a 135 //pc.printf("Fr = [%.1f, %.1f] ", Frx, Fry);
joaopsousa99 4:1defb279922a 136 float Fax = Fca*(objCellCenter[0] - poseX)/(sqrt(pow(objCellCenter[0] - poseX, 2) + pow(objCellCenter[1] - poseY, 2)));
joaopsousa99 4:1defb279922a 137 float Fay = Fca*(objCellCenter[1] - poseY)/(sqrt(pow(objCellCenter[0] - poseX, 2) + pow(objCellCenter[1] - poseY, 2)));
joaopsousa99 4:1defb279922a 138 //pc.printf("Fa = [%.1f, %.1f] ", Fax, Fay);
joaopsousa99 4:1defb279922a 139
joaopsousa99 4:1defb279922a 140 /*if(occupiedCounter == 0){
joaopsousa99 4:1defb279922a 141 float F[2] = {Fax, Fay};
joaopsousa99 4:1defb279922a 142 printf("f vff; ");
joaopsousa99 4:1defb279922a 143 return atan2(F[1], F[0]);
joaopsousa99 4:1defb279922a 144 }*/
joaopsousa99 4:1defb279922a 145
joaopsousa99 4:1defb279922a 146 /*int *xOccupied, *yOccupied;
joaopsousa99 4:1defb279922a 147 xOccupied = (int *)calloc(occupiedCounter, sizeof(int));
joaopsousa99 4:1defb279922a 148 yOccupied = (int *)calloc(occupiedCounter, sizeof(int));
joaopsousa99 4:1defb279922a 149
joaopsousa99 4:1defb279922a 150 occupiedCounter = 0;
joaopsousa99 4:1defb279922a 151
joaopsousa99 4:1defb279922a 152 for(int i = 0; i < lenx*leny; i++){
joaopsousa99 4:1defb279922a 153 if(auxRows[i] != -1){
joaopsousa99 4:1defb279922a 154 xOccupied[occupiedCounter] = auxRows[i];
joaopsousa99 4:1defb279922a 155 yOccupied[occupiedCounter] = auxCols[i];
joaopsousa99 4:1defb279922a 156 occupiedCounter++;
joaopsousa99 4:1defb279922a 157 }
joaopsousa99 4:1defb279922a 158 }
joaopsousa99 4:1defb279922a 159
joaopsousa99 4:1defb279922a 160 for (int i = 0; i < occupiedCounter; i++) {
joaopsousa99 4:1defb279922a 161 xCell = xOccupied[i]*CELL_SIZE + CELL_SIZE/2;
joaopsousa99 4:1defb279922a 162 yCell = yOccupied[i]*CELL_SIZE + CELL_SIZE/2;
joaopsousa99 4:1defb279922a 163
joaopsousa99 4:1defb279922a 164 Frx += Fcr*(xCell - poseX)/pow(sqrt(pow(xCell - poseX, 2) + pow(yCell - poseY, 2)), 3);
joaopsousa99 4:1defb279922a 165 Fry += Fcr*(yCell - poseY)/pow(sqrt(pow(xCell - poseX, 2) + pow(yCell - poseY, 2)), 3);
joaopsousa99 4:1defb279922a 166 }
joaopsousa99 4:1defb279922a 167
joaopsousa99 4:1defb279922a 168 free(auxCols);
joaopsousa99 4:1defb279922a 169 free(auxRows);
joaopsousa99 4:1defb279922a 170 free(xOccupied);
joaopsousa99 4:1defb279922a 171 free(yOccupied);*/
joaopsousa99 4:1defb279922a 172
joaopsousa99 4:1defb279922a 173 float F[2] = {Fax - Frx, Fay - Fry};
joaopsousa99 4:1defb279922a 174 //printf("f vff; ");
joaopsousa99 4:1defb279922a 175 return atan2(F[1], F[0]);
joaopsousa99 4:1defb279922a 176 }
joaopsousa99 4:1defb279922a 177
joaopsousa99 4:1defb279922a 178 void followPath(float &objAheadX, float &objAheadY, float &intError, float *objCellCenter, Matrix occupationGrid, Serial pc){
joaopsousa99 4:1defb279922a 179 //printf("i followPath; ");
joaopsousa99 4:1defb279922a 180 float kv = 0.3;
joaopsousa99 4:1defb279922a 181 float ki = 0.2;
joaopsousa99 4:1defb279922a 182 float kw = 10;
joaopsousa99 4:1defb279922a 183 int vObj = 7; // temos de fazer experiências para afinar isto
joaopsousa99 4:1defb279922a 184 float vffAngle = poseTheta;
joaopsousa99 4:1defb279922a 185 int dObj = 3;
joaopsousa99 4:1defb279922a 186
joaopsousa99 4:1defb279922a 187 float xmax = floor(static_cast<float>(poseX/CELL_SIZE)) + floor(static_cast<float>(WINDOW_SIZE/2));
joaopsousa99 4:1defb279922a 188 float xmin = floor(static_cast<float>(poseX/CELL_SIZE)) - floor(static_cast<float>(WINDOW_SIZE/2));
joaopsousa99 4:1defb279922a 189 float ymax = floor(static_cast<float>(poseY/CELL_SIZE)) + floor(static_cast<float>(WINDOW_SIZE/2));
joaopsousa99 4:1defb279922a 190 float ymin = floor(static_cast<float>(poseY/CELL_SIZE)) - floor(static_cast<float>(WINDOW_SIZE/2));
joaopsousa99 4:1defb279922a 191
joaopsousa99 4:1defb279922a 192 if(xmin < 0)
joaopsousa99 4:1defb279922a 193 xmin = 0;
joaopsousa99 4:1defb279922a 194 if(xmax >= GRID_SIZE)
joaopsousa99 4:1defb279922a 195 xmax = GRID_SIZE - 1;
joaopsousa99 4:1defb279922a 196 if(ymin < 0)
joaopsousa99 4:1defb279922a 197 ymin = 0;
joaopsousa99 4:1defb279922a 198 if(ymax >= GRID_SIZE)
joaopsousa99 4:1defb279922a 199 ymax = GRID_SIZE - 1;
joaopsousa99 4:1defb279922a 200
joaopsousa99 4:1defb279922a 201 int lenx = xmax - xmin + 1;
joaopsousa99 4:1defb279922a 202 int leny = ymax - ymin + 1;
joaopsousa99 4:1defb279922a 203
joaopsousa99 4:1defb279922a 204 // pre alocacao de espaco
joaopsousa99 4:1defb279922a 205 int *xactive, *yactive;
joaopsousa99 4:1defb279922a 206 xactive = (int *)calloc(lenx, sizeof(int));
joaopsousa99 4:1defb279922a 207 yactive = (int *)calloc(leny, sizeof(int));
joaopsousa99 4:1defb279922a 208
joaopsousa99 4:1defb279922a 209 // linspace do matlab
joaopsousa99 4:1defb279922a 210 for(int j = 0; j < lenx; j++)
joaopsousa99 4:1defb279922a 211 xactive[j] = xmin + j;
joaopsousa99 4:1defb279922a 212
joaopsousa99 4:1defb279922a 213 for(int i = 0; i < leny; i++)
joaopsousa99 4:1defb279922a 214 yactive[i] = ymin + i;
joaopsousa99 4:1defb279922a 215
joaopsousa99 4:1defb279922a 216 // repmat do matlab
joaopsousa99 4:1defb279922a 217 //int *xactive = repeat_matrix2(x, lenx, leny);
joaopsousa99 4:1defb279922a 218 //int *yactive = repeat_matrix(y, leny, lenx);
joaopsousa99 4:1defb279922a 219
joaopsousa99 4:1defb279922a 220 //int activeSize = lenx*leny;
joaopsousa99 4:1defb279922a 221
joaopsousa99 4:1defb279922a 222 vffAngle = vff(occupationGrid, xactive, yactive, lenx, leny, objCellCenter, pc);
joaopsousa99 4:1defb279922a 223
joaopsousa99 4:1defb279922a 224 objAheadX = objAheadX + TIMESTEP * vObj * cos(vffAngle);
joaopsousa99 4:1defb279922a 225 objAheadY = objAheadY + TIMESTEP * vObj * sin(vffAngle);
joaopsousa99 4:1defb279922a 226
joaopsousa99 4:1defb279922a 227 float distError = sqrt(pow(objAheadY - poseY, 2) + pow(objAheadX - poseX, 2)) - dObj;
joaopsousa99 4:1defb279922a 228
joaopsousa99 4:1defb279922a 229 intError = intError + distError * TIMESTEP;
joaopsousa99 4:1defb279922a 230
joaopsousa99 4:1defb279922a 231 float v = kv * distError + ki * intError;
joaopsousa99 4:1defb279922a 232 float phiObj = atan2(objAheadY - poseY, objAheadX - poseX);
joaopsousa99 4:1defb279922a 233 float w = kw*atan2(sin(phiObj - poseTheta), cos(phiObj - poseTheta));
joaopsousa99 4:1defb279922a 234 float *wheelSpeeds;
joaopsousa99 4:1defb279922a 235 wheelSpeeds = (float *)calloc(2, sizeof(float));
joaopsousa99 4:1defb279922a 236
joaopsousa99 4:1defb279922a 237 robotVel2wheelVel(v, w, wheelSpeeds);
joaopsousa99 4:1defb279922a 238
joaopsousa99 4:1defb279922a 239 wheelSpeeds[0] = mapSpeeds(wheelSpeeds[0]);
joaopsousa99 4:1defb279922a 240 wheelSpeeds[1] = mapSpeeds(wheelSpeeds[1]);
joaopsousa99 4:1defb279922a 241
joaopsousa99 4:1defb279922a 242 setSpeeds((int)wheelSpeeds[0], (int)wheelSpeeds[1]);
joaopsousa99 4:1defb279922a 243
joaopsousa99 4:1defb279922a 244 free(wheelSpeeds);
joaopsousa99 4:1defb279922a 245 free(xactive);
joaopsousa99 4:1defb279922a 246 free(yactive);
joaopsousa99 4:1defb279922a 247
joaopsousa99 4:1defb279922a 248 //printf("f followPath; ");
joaopsousa99 4:1defb279922a 249 }