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?
User | Revision | Line number | New 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 | } |