SRM / Mbed OS LabWork3Lidar

dasd

Dependencies:   BufferedSerial

main.cpp@7:5e59f8a011fd, 2019-06-07 (annotated)

Committer:
shut
Date:
Fri Jun 07 15:39:06 2019 +0000
Revision:
7:5e59f8a011fd
Parent:
6:1c84602323c8 
7-6

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LuisRA 0:2b691d200d6f 1 #include "mbed.h"
LuisRA 0:2b691d200d6f 2 #include "BufferedSerial.h"
LuisRA 0:2b691d200d6f 3 #include "rplidar.h"
fabiofaria 1:dc87724abce8 4 #include "Robot.h"
fabiofaria 1:dc87724abce8 5 #include "Communication.h"
jsobiecki 4:6ebe8982de0e 6 #include "math.h"
jsobiecki 4:6ebe8982de0e 7 #include "ActiveCell.h"
jsobiecki 4:6ebe8982de0e 8 #include "HistogramCell.h"
jsobiecki 4:6ebe8982de0e 9 #define M_PI 3.14159265358979323846f
shut 6:1c84602323c8 10 //Luis Cruz N2011164454
shut 6:1c84602323c8 11 //Jacek Sobecki N2018319609
shut 6:1c84602323c8 12 //VFH/VFF (LIDAR) - LW3
LuisRA 0:2b691d200d6f 13 RPLidar lidar;
LuisRA 0:2b691d200d6f 14 BufferedSerial se_lidar(PA_9, PA_10);
fabiofaria 3:0a718d139ed1 15 PwmOut rplidar_motor(D3);
shut 6:1c84602323c8 16 struct RPLidarMeasurement data;
LuisRA 0:2b691d200d6f 17
jsobiecki 4:6ebe8982de0e 18 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
jsobiecki 4:6ebe8982de0e 19 DigitalIn button(PC_13);
jsobiecki 4:6ebe8982de0e 20 void poseEst(float p[], float radius, float enc_res, float b);
jsobiecki 4:6ebe8982de0e 21 void SpeedLim(float w[]);
jsobiecki 4:6ebe8982de0e 22 void initializeArrays();
jsobiecki 4:6ebe8982de0e 23 void calcSectors(float theta);
jsobiecki 4:6ebe8982de0e 24 void sumForces();
jsobiecki 4:6ebe8982de0e 25 void updateActive(float xR, float yR,float theta);
shut 6:1c84602323c8 26 void mapping();
shut 6:1c84602323c8 27 void prob_calc();
shut 6:1c84602323c8 28 int Bresenham(int x1, int y1, int x2, int y2, int cX[], int cY[]);
shut 6:1c84602323c8 29 void log_cell(int cX[], int cY[], int npts);
shut 6:1c84602323c8 30 //Histogram size -> hSize | Active region size -> aSize
jsobiecki 4:6ebe8982de0e 31 const int hSize = 80, aSize = 11;
jsobiecki 4:6ebe8982de0e 32 ActiveCell activeReg[aSize][aSize];
jsobiecki 4:6ebe8982de0e 33 HistogramCell histogram[hSize][hSize];
jsobiecki 4:6ebe8982de0e 34 //Repulsive force sums
jsobiecki 4:6ebe8982de0e 35 float p[3], p_obj[3], p_final[3], fX, fY;
shut 7:5e59f8a011fd 36 int aux99 = 0;
shut 6:1c84602323c8 37 //const float Fca=6;/*5*/
jsobiecki 4:6ebe8982de0e 38
jsobiecki 4:6ebe8982de0e 39 //VFH
jsobiecki 4:6ebe8982de0e 40 const int L=2;
jsobiecki 4:6ebe8982de0e 41 float secVal[36];
jsobiecki 4:6ebe8982de0e 42 float smooth[36];
jsobiecki 4:6ebe8982de0e 43
shut 6:1c84602323c8 44 int main(){
fabiofaria 1:dc87724abce8 45 pc.baud(115200);
fabiofaria 1:dc87724abce8 46 init_communication(&pc);
shut 6:1c84602323c8 47 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
shut 6:1c84602323c8 48 button.mode(PullUp);
shut 6:1c84602323c8 49 getCountsAndReset();
shut 6:1c84602323c8 50 setSpeeds(0, 0);
shut 6:1c84602323c8 51 while(button==1);
fabiofaria 1:dc87724abce8 52 // Lidar initialization
LuisRA 0:2b691d200d6f 53 rplidar_motor.period(0.001f);
LuisRA 0:2b691d200d6f 54 lidar.begin(se_lidar);
LuisRA 0:2b691d200d6f 55 lidar.setAngle(0,360);
shut 7:5e59f8a011fd 56 rplidar_motor.write(0.8f);
LuisRA 0:2b691d200d6f 57 pc.printf("Program started.\n");
LuisRA 0:2b691d200d6f 58 lidar.startThreadScan();
jsobiecki 4:6ebe8982de0e 59 //w[0] = Omega | w[1] = Left | w[2] = Right
jsobiecki 4:6ebe8982de0e 60 //p[0] = X | p[1] = Y | p[2] = Theta
jsobiecki 4:6ebe8982de0e 61 //p_obj[0] = X | p_obj[1] = Y | p_obj[2] = Theta
jsobiecki 4:6ebe8982de0e 62 //b = Distance between wheels, enc_res = Encoder Resolution, v = Calculated speed
jsobiecki 4:6ebe8982de0e 63 //k_v = Speed gain, k_s = Curvature gain, wratio = Angular speed ratio control command
jsobiecki 4:6ebe8982de0e 64 //Cells dim: 5x5cm |
jsobiecki 4:6ebe8982de0e 65 float w[3], v, theta, theta_error, err, integral = 0.0, k_i = 0.01/*0.02*/;
jsobiecki 4:6ebe8982de0e 66 const float radius = 3.5, b = 13.3, enc_res = 1440, k_v = 8/*7*/,
jsobiecki 4:6ebe8982de0e 67 k_s = 12/*10*/, sample_time = 0.05, d_stalker = 5, k_f = 12.5; /*12.5*/ //VFF
jsobiecki 4:6ebe8982de0e 68 float theta_final;
jsobiecki 4:6ebe8982de0e 69 // ===============================================================================
jsobiecki 4:6ebe8982de0e 70 // =================================== COORDS ====================================
jsobiecki 4:6ebe8982de0e 71 // ===============================================================================
jsobiecki 4:6ebe8982de0e 72 //Target coordinates
shut 7:5e59f8a011fd 73 p_final[0] = 250, p_final[1] = 100, p_final[2] = 0;
jsobiecki 4:6ebe8982de0e 74 //p_obj[0] = 20, p_obj[1] = 20, p_obj[2] = 0;
jsobiecki 4:6ebe8982de0e 75 //Initial coordinates:
shut 7:5e59f8a011fd 76 p[0] = 100, p[1] = 100, p[2] = 0;
jsobiecki 4:6ebe8982de0e 77 // ===============================================================================
jsobiecki 4:6ebe8982de0e 78 // =================================== EXECUTION =================================
jsobiecki 4:6ebe8982de0e 79 // ===============================================================================
shut 6:1c84602323c8 80 initializeArrays();
jsobiecki 4:6ebe8982de0e 81 while(1){
LuisRA 0:2b691d200d6f 82 // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
shut 7:5e59f8a011fd 83 if(lidar.pollSensorData(&data) == 0) //pc.printf("dist:%f angle:%f\n", data.distance, data.angle); // Prints one lidar measurement.
jsobiecki 4:6ebe8982de0e 84 getCountsAndReset();
shut 7:5e59f8a011fd 85 //pc.printf("Speeds: Left=%lf Right=%lf\n", w[1], w[2]); // DEBUG
shut 6:1c84602323c8 86 //pc.printf("OBJECTIVE X: %lf OBJECTIVE Y: %lf\n", p_obj[0], p_obj[1]); DEBUG
shut 6:1c84602323c8 87 pc.printf("Position: X=%lf Y=%lf Theta=%lf\n\n", p[0], p[1], p[2]);// DEBUG
shut 6:1c84602323c8 88 //pc.printf("Force (X): X=%lf Force(Y)=%lf\n", fX, fY); DEBUG
jsobiecki 4:6ebe8982de0e 89 //Path calculation
shut 6:1c84602323c8 90 mapping();
shut 6:1c84602323c8 91 poseEst(p, radius, enc_res, b);
jsobiecki 4:6ebe8982de0e 92 theta_final = atan2(p_final[1]-p[1],p_final[0]-p[0]);
jsobiecki 4:6ebe8982de0e 93 theta_final = atan2(sin(theta_final),cos(theta_final));
shut 6:1c84602323c8 94 updateActive(p[0], p[1], theta_final);
shut 6:1c84602323c8 95 p_obj[0] = p[0]+k_f*fX; //add parameter to relate chosen direction (VFH) to the point nearby of the robot
jsobiecki 4:6ebe8982de0e 96 p_obj[1] = p[1]+k_f*fY;
jsobiecki 4:6ebe8982de0e 97 //Control Law
shut 6:1c84602323c8 98 err = sqrt(pow((p_obj[0]-p[0]),2)+pow((p_obj[1]-p[1]),2)) - d_stalker; //distance to the "carrot" point
jsobiecki 4:6ebe8982de0e 99 theta = atan2(p_obj[1]-p[1],p_obj[0]-p[0]);
shut 6:1c84602323c8 100 //pc.printf("theta MAIN: = %lf\n\n", theta); DEBUG
jsobiecki 4:6ebe8982de0e 101 theta = atan2(sin(theta),cos(theta));
jsobiecki 4:6ebe8982de0e 102 p[2] = atan2(sin(p[2]),cos(p[2]));
jsobiecki 4:6ebe8982de0e 103 theta_error = theta-p[2];
jsobiecki 4:6ebe8982de0e 104 theta_error = atan2(sin(theta_error),cos(theta_error));
shut 6:1c84602323c8 105 //pc.printf("theta_error = %lf | p[2]= %lf\n\n", theta_error, p[2]); DEBUG
jsobiecki 4:6ebe8982de0e 106 w[0] = k_s*(theta_error); //direction gain
jsobiecki 4:6ebe8982de0e 107 integral += err;
jsobiecki 4:6ebe8982de0e 108 v = k_v*err+k_i*integral; //Speed calculation
jsobiecki 4:6ebe8982de0e 109 w[1] = (v-(b/2)*w[0])/radius;
jsobiecki 4:6ebe8982de0e 110 w[2] = (v+(b/2)*w[0])/radius;
jsobiecki 4:6ebe8982de0e 111 SpeedLim(w);
shut 6:1c84602323c8 112 //if((fabs(p[0]-p_final[0])+fabs(p[1]-p_final[1])) < 70) k_i = -0.005; //Not functional, meant to decrease speed as aproaching final point
jsobiecki 4:6ebe8982de0e 113 if((fabs(p[0]-p_final[0])+fabs(p[1]-p_final[1])) < 5){
jsobiecki 4:6ebe8982de0e 114 setSpeeds(0,0);
jsobiecki 4:6ebe8982de0e 115 }
shut 7:5e59f8a011fd 116 else if (aux99 > 5){
shut 6:1c84602323c8 117 setSpeeds(w[1], w[2]);// Motor's output
jsobiecki 4:6ebe8982de0e 118 }
jsobiecki 4:6ebe8982de0e 119 wait(sample_time);
shut 7:5e59f8a011fd 120 aux99++;
jsobiecki 4:6ebe8982de0e 121 }
jsobiecki 4:6ebe8982de0e 122 }
jsobiecki 4:6ebe8982de0e 123 // ===============================================================================
jsobiecki 4:6ebe8982de0e 124 // =================================== FUNCTIONS =================================
jsobiecki 4:6ebe8982de0e 125 // ===============================================================================
jsobiecki 4:6ebe8982de0e 126 //Pose Estimation function
jsobiecki 4:6ebe8982de0e 127 void poseEst(float p[], float radius, float enc_res, float b){
jsobiecki 4:6ebe8982de0e 128 float deltaDl, deltaDr, deltaD, deltaT;
jsobiecki 4:6ebe8982de0e 129 deltaDl = ((float)countsLeft)*(2.0f*M_PI*radius/enc_res);
jsobiecki 4:6ebe8982de0e 130 deltaDr = ((float)countsRight)*(2.0f*M_PI*radius/enc_res);
jsobiecki 4:6ebe8982de0e 131 deltaD = (deltaDr + deltaDl)/2.0f;
jsobiecki 4:6ebe8982de0e 132 deltaT = (deltaDr - deltaDl)/b;
jsobiecki 4:6ebe8982de0e 133 if(fabs(deltaT) == 0){
jsobiecki 4:6ebe8982de0e 134 p[0] = p[0] + deltaD*cos(p[2]) + deltaT/2;
jsobiecki 4:6ebe8982de0e 135 p[1] = p[1] + deltaD*sin(p[2]) + deltaT/2;
jsobiecki 4:6ebe8982de0e 136 return;
jsobiecki 4:6ebe8982de0e 137 }
jsobiecki 4:6ebe8982de0e 138 p[0] = p[0] + deltaD*(sin(deltaT/2.0f)/(deltaT/2.0f))*cos(p[2]+deltaT/2.0f);
jsobiecki 4:6ebe8982de0e 139 p[1] = p[1] + deltaD*(sin(deltaT/2.0f)/(deltaT/2.0f))*sin(p[2]+deltaT/2.0f);
jsobiecki 4:6ebe8982de0e 140 p[2] = p[2] + deltaT;
jsobiecki 4:6ebe8982de0e 141 }
shut 7:5e59f8a011fd 142
shut 7:5e59f8a011fd 143
shut 7:5e59f8a011fd 144
jsobiecki 4:6ebe8982de0e 145 //Speed limiter function
jsobiecki 4:6ebe8982de0e 146 void SpeedLim(float w[]){
jsobiecki 4:6ebe8982de0e 147 float wratio;
jsobiecki 4:6ebe8982de0e 148 wratio = fabs(w[2]/w[1]);
jsobiecki 4:6ebe8982de0e 149 if(w[2] > 150 || w[1] > 150){
jsobiecki 4:6ebe8982de0e 150 if(wratio < 1){
jsobiecki 4:6ebe8982de0e 151 w[1] = 150;
jsobiecki 4:6ebe8982de0e 152 w[2] = w[1]*wratio;
jsobiecki 4:6ebe8982de0e 153 }
jsobiecki 4:6ebe8982de0e 154 else if(wratio > 1){
jsobiecki 4:6ebe8982de0e 155 w[2] = 150;
jsobiecki 4:6ebe8982de0e 156 w[1] = w[2]/wratio;
jsobiecki 4:6ebe8982de0e 157 }
jsobiecki 4:6ebe8982de0e 158 else{
jsobiecki 4:6ebe8982de0e 159 w[2] = 150;
jsobiecki 4:6ebe8982de0e 160 w[1] = 150;
jsobiecki 4:6ebe8982de0e 161 }
jsobiecki 4:6ebe8982de0e 162 }
shut 7:5e59f8a011fd 163 if(w[2] < 70 || w[1] < 70){
jsobiecki 4:6ebe8982de0e 164 if(wratio < 1){
shut 7:5e59f8a011fd 165 w[1] = 70;
jsobiecki 4:6ebe8982de0e 166 w[2] = w[1]*wratio;
jsobiecki 4:6ebe8982de0e 167 }
jsobiecki 4:6ebe8982de0e 168 else if(wratio > 1){
shut 7:5e59f8a011fd 169 w[2] = 70;
jsobiecki 4:6ebe8982de0e 170 w[1] = w[2]/wratio;
jsobiecki 4:6ebe8982de0e 171 }
jsobiecki 4:6ebe8982de0e 172 else{
shut 7:5e59f8a011fd 173 w[2] = 70;
shut 7:5e59f8a011fd 174 w[1] = 70;
jsobiecki 4:6ebe8982de0e 175 }
jsobiecki 4:6ebe8982de0e 176 }
jsobiecki 4:6ebe8982de0e 177 }
jsobiecki 4:6ebe8982de0e 178
jsobiecki 4:6ebe8982de0e 179 void initializeArrays() {
shut 6:1c84602323c8 180 /*for (int i = 0; i < hSize; i++) {
jsobiecki 4:6ebe8982de0e 181 for (int j = 0; j < hSize; j++) {
shut 6:1c84602323c8 182 histogram[i][j].cellVal = 0;
jsobiecki 4:6ebe8982de0e 183 //if(((i >= 8 && i <= 12) && (j == 0 || j == 8)) || ((i == 8 || i == 12) && (j >= 0 && j <= 8))) histogram[i][j].cellVal=3;
jsobiecki 4:6ebe8982de0e 184 //if(((i >= 0 && i <= 3) && (j == 8 || j == 12)) || ((i == 0 || i == 3) && (j >= 8 && j <= 12))) histogram[i][j].cellVal=3;
jsobiecki 4:6ebe8982de0e 185 //if(((i >= 14 && i <= 20) && (j == 8 || j == 9)) || ((i == 14 || i == 20) && (j >= 8 && j <= 9))) histogram[i][j].cellVal=3;
jsobiecki 4:6ebe8982de0e 186 }
shut 6:1c84602323c8 187 }*/
jsobiecki 4:6ebe8982de0e 188 for (int i = 0; i < aSize; i++) {
jsobiecki 4:6ebe8982de0e 189 for (int j = 0; j < aSize; j++) {
jsobiecki 4:6ebe8982de0e 190 activeReg[i][j].calDist(i, j);
jsobiecki 4:6ebe8982de0e 191 }
jsobiecki 4:6ebe8982de0e 192 }
jsobiecki 4:6ebe8982de0e 193 }
shut 6:1c84602323c8 194 //xR, yR - robots position in coordinates system - ATM updating histogram (10/5)
jsobiecki 4:6ebe8982de0e 195 void updateActive(float xR, float yR,float theta) {
jsobiecki 4:6ebe8982de0e 196 int idXr = 0;
jsobiecki 4:6ebe8982de0e 197 int idYr = 0;
jsobiecki 4:6ebe8982de0e 198 for (int i = 0; i < hSize; i++) {
jsobiecki 4:6ebe8982de0e 199 for (int j = 0; j < hSize; j++) {
shut 7:5e59f8a011fd 200 if (xR >= (i*5.0f) && xR < (i*5.0f+5.0f) && yR >= (j*5.0f) &&
shut 7:5e59f8a011fd 201 yR < (j*5.0f+5.0f)){
jsobiecki 4:6ebe8982de0e 202 idXr = i;
jsobiecki 4:6ebe8982de0e 203 idYr = j;
jsobiecki 4:6ebe8982de0e 204 break;
jsobiecki 4:6ebe8982de0e 205 }
jsobiecki 4:6ebe8982de0e 206 }
jsobiecki 4:6ebe8982de0e 207 }
shut 6:1c84602323c8 208
jsobiecki 4:6ebe8982de0e 209 int m = idXr - aSize / 2;
jsobiecki 4:6ebe8982de0e 210 for (int k = 0; k < aSize; k++) {
jsobiecki 4:6ebe8982de0e 211 int n = idYr - aSize / 2;
jsobiecki 4:6ebe8982de0e 212 for (int l = 0; l < aSize; l++) {
shut 6:1c84602323c8 213 if(m >= 0 && n >= 0 && m < hSize && n < hSize) {
shut 6:1c84602323c8 214 activeReg[k][l].cellVal = histogram[m][n].cellVal;
jsobiecki 4:6ebe8982de0e 215 }
jsobiecki 4:6ebe8982de0e 216 n++;
jsobiecki 4:6ebe8982de0e 217 }
jsobiecki 4:6ebe8982de0e 218 m++;
jsobiecki 4:6ebe8982de0e 219 }
shut 6:1c84602323c8 220
shut 6:1c84602323c8 221 for (int i = 0; i < aSize; i++) {
shut 6:1c84602323c8 222 for (int j = 0; j < aSize; j++) {
jsobiecki 4:6ebe8982de0e 223 activeReg[i][j].calForce();
jsobiecki 4:6ebe8982de0e 224 }
jsobiecki 4:6ebe8982de0e 225 }
jsobiecki 4:6ebe8982de0e 226 activeReg[5][5].amplitude=0;
jsobiecki 4:6ebe8982de0e 227 activeReg[5][5].amplitude=0;
shut 7:5e59f8a011fd 228 /*for (int j = 10; j >= 0; j--) {
jsobiecki 4:6ebe8982de0e 229 for (int i = 0; i < 11; i++) {
jsobiecki 4:6ebe8982de0e 230 cout << "[" << activeReg[i][j].cellVal << "]";
jsobiecki 4:6ebe8982de0e 231 }
jsobiecki 4:6ebe8982de0e 232 cout << endl;
shut 7:5e59f8a011fd 233 }*/
jsobiecki 4:6ebe8982de0e 234 calcSectors(theta);
jsobiecki 4:6ebe8982de0e 235 }
jsobiecki 4:6ebe8982de0e 236 void calcSectors(float theta){
jsobiecki 4:6ebe8982de0e 237 for (int k = 0; k < 36; ++k) {
jsobiecki 4:6ebe8982de0e 238 secVal[k]=0;
jsobiecki 4:6ebe8982de0e 239 for (int i = 0; i < aSize; ++i) {
jsobiecki 4:6ebe8982de0e 240 for (int j = 0; j < aSize; ++j) {
jsobiecki 4:6ebe8982de0e 241 if(activeReg[i][j].sectorK==k)
jsobiecki 4:6ebe8982de0e 242 secVal[k]+=activeReg[i][j].amplitude;
jsobiecki 4:6ebe8982de0e 243 }
LuisRA 0:2b691d200d6f 244 }
jsobiecki 4:6ebe8982de0e 245 }
jsobiecki 4:6ebe8982de0e 246
jsobiecki 4:6ebe8982de0e 247 smooth[0]=(secVal[34]+2*secVal[35]+2*secVal[0]+2*secVal[1]+secVal[2])/5;
jsobiecki 4:6ebe8982de0e 248 smooth[1]=(secVal[35]+2*secVal[0]+2*secVal[1]+2*secVal[2]+secVal[3])/5;
jsobiecki 4:6ebe8982de0e 249 smooth[34]=(secVal[32]+2*secVal[33]+2*secVal[34]+2*secVal[35]+secVal[0])/5;
jsobiecki 4:6ebe8982de0e 250 smooth[35]=(secVal[33]+2*secVal[34]+2*secVal[35]+2*secVal[0]+secVal[1])/5;
jsobiecki 4:6ebe8982de0e 251 for (int i = 2; i < 34; ++i) {
jsobiecki 4:6ebe8982de0e 252 smooth[i]=(secVal[i-L]+2*secVal[i-L+1]+2*secVal[i]+2*secVal[i+L-1]+secVal[i+L])/5;
jsobiecki 4:6ebe8982de0e 253 }
jsobiecki 4:6ebe8982de0e 254
jsobiecki 4:6ebe8982de0e 255 const int thresh=200;//100
jsobiecki 4:6ebe8982de0e 256 int temp[36];
jsobiecki 4:6ebe8982de0e 257 int counter = 0, aux = 0;
jsobiecki 4:6ebe8982de0e 258 int valley[36];
jsobiecki 4:6ebe8982de0e 259 for(int i=0;i<36;++i){
jsobiecki 4:6ebe8982de0e 260 //pc.printf("|%lf", smooth[i]);
jsobiecki 4:6ebe8982de0e 261 if(smooth[i]<thresh){
jsobiecki 4:6ebe8982de0e 262 temp[i]=1;
jsobiecki 4:6ebe8982de0e 263 //valley[aux][aux] =
jsobiecki 4:6ebe8982de0e 264 counter++;
jsobiecki 4:6ebe8982de0e 265 }
jsobiecki 4:6ebe8982de0e 266 else{
jsobiecki 4:6ebe8982de0e 267 valley[aux] = counter;
jsobiecki 4:6ebe8982de0e 268 counter = 0;
jsobiecki 4:6ebe8982de0e 269 aux++;
jsobiecki 4:6ebe8982de0e 270 temp[i]=0;
jsobiecki 4:6ebe8982de0e 271 //pc.printf("#%d", i);
jsobiecki 4:6ebe8982de0e 272 }
jsobiecki 4:6ebe8982de0e 273
jsobiecki 4:6ebe8982de0e 274 }
shut 6:1c84602323c8 275 //float best=999;
jsobiecki 4:6ebe8982de0e 276 float theta_deg;
jsobiecki 4:6ebe8982de0e 277 theta_deg =(theta*180.0f)/M_PI;
jsobiecki 4:6ebe8982de0e 278 //pc.printf("theta (degrees): = %lf\n\n", theta_deg);
jsobiecki 4:6ebe8982de0e 279 int destSec = theta_deg / 10;
jsobiecki 4:6ebe8982de0e 280 if(destSec<0) destSec=36+destSec;
jsobiecki 4:6ebe8982de0e 281 //cout<<"destination sector: "<<destSec<<endl;
jsobiecki 4:6ebe8982de0e 282
jsobiecki 4:6ebe8982de0e 283 int L=destSec;
jsobiecki 4:6ebe8982de0e 284 int R=destSec;
jsobiecki 4:6ebe8982de0e 285 while(temp[L]==0){
jsobiecki 4:6ebe8982de0e 286 L--;
jsobiecki 4:6ebe8982de0e 287 if(L<0) L=35;
jsobiecki 4:6ebe8982de0e 288 }
jsobiecki 4:6ebe8982de0e 289 while(temp[R]==0){
jsobiecki 4:6ebe8982de0e 290 R++;
jsobiecki 4:6ebe8982de0e 291 if(R>35) R=0;
LuisRA 0:2b691d200d6f 292 }
jsobiecki 4:6ebe8982de0e 293
jsobiecki 4:6ebe8982de0e 294 float dirSet, dirC,dirL,dirR;
jsobiecki 4:6ebe8982de0e 295 if(temp[destSec]==1){
jsobiecki 4:6ebe8982de0e 296 int k=destSec-1;
jsobiecki 4:6ebe8982de0e 297 if(k<0) k=35;
jsobiecki 4:6ebe8982de0e 298 int size=1;
jsobiecki 4:6ebe8982de0e 299 while(temp[k]==1){
jsobiecki 4:6ebe8982de0e 300 size++;
jsobiecki 4:6ebe8982de0e 301 k--;
jsobiecki 4:6ebe8982de0e 302 if(k<0) k=35;
jsobiecki 4:6ebe8982de0e 303 if(k==destSec) break;
jsobiecki 4:6ebe8982de0e 304 if(size>=5) break;
jsobiecki 4:6ebe8982de0e 305 }
jsobiecki 4:6ebe8982de0e 306 int right=k+1;
jsobiecki 4:6ebe8982de0e 307 if(right<0) right=35;
jsobiecki 4:6ebe8982de0e 308 k=destSec+1;
jsobiecki 4:6ebe8982de0e 309 if(k>35) k=0;
jsobiecki 4:6ebe8982de0e 310 while(temp[k]==1){
jsobiecki 4:6ebe8982de0e 311 size++;
jsobiecki 4:6ebe8982de0e 312 k++;
jsobiecki 4:6ebe8982de0e 313 if(k>35) k=0;
jsobiecki 4:6ebe8982de0e 314 if(k==destSec) break;
jsobiecki 4:6ebe8982de0e 315 if(size>=5) break;
jsobiecki 4:6ebe8982de0e 316 }
jsobiecki 4:6ebe8982de0e 317 int left=k-1;
jsobiecki 4:6ebe8982de0e 318 if(left>35) left=0;
jsobiecki 4:6ebe8982de0e 319 if(size>=5) {
jsobiecki 4:6ebe8982de0e 320 //wide
jsobiecki 4:6ebe8982de0e 321 dirC=destSec*10;
jsobiecki 4:6ebe8982de0e 322 //cout << "wide"<<endl;
jsobiecki 4:6ebe8982de0e 323 }
jsobiecki 4:6ebe8982de0e 324
jsobiecki 4:6ebe8982de0e 325 else if(size>4 && size<5) //narrow
jsobiecki 4:6ebe8982de0e 326 {
jsobiecki 4:6ebe8982de0e 327 dirC=0.5*(left*10+right*10);
jsobiecki 4:6ebe8982de0e 328 //cout<<"narrow"<<endl;
jsobiecki 4:6ebe8982de0e 329 } else {
jsobiecki 4:6ebe8982de0e 330 int secL = L;
jsobiecki 4:6ebe8982de0e 331 while (temp[secL] != 1) {
jsobiecki 4:6ebe8982de0e 332 secL++;
jsobiecki 4:6ebe8982de0e 333 if (secL > 35) secL = 0;
jsobiecki 4:6ebe8982de0e 334 }
jsobiecki 4:6ebe8982de0e 335 int rightL = secL;
jsobiecki 4:6ebe8982de0e 336 int size = 1;
jsobiecki 4:6ebe8982de0e 337
jsobiecki 4:6ebe8982de0e 338 int i = secL + 1;
jsobiecki 4:6ebe8982de0e 339 if (i > 35) i = 0;
jsobiecki 4:6ebe8982de0e 340 while (temp[i] == 1) {
jsobiecki 4:6ebe8982de0e 341 size++;
jsobiecki 4:6ebe8982de0e 342 i++;
jsobiecki 4:6ebe8982de0e 343 if (i > 35) i = 0;
jsobiecki 4:6ebe8982de0e 344 if (i == secL) break;
jsobiecki 4:6ebe8982de0e 345 // Smax here
shut 7:5e59f8a011fd 346 if (size >= 5) break; //tried 10, same behaviour
jsobiecki 4:6ebe8982de0e 347 }
jsobiecki 4:6ebe8982de0e 348 int leftL = i - 1;
jsobiecki 4:6ebe8982de0e 349 if (leftL < 0) leftL = 35;
jsobiecki 4:6ebe8982de0e 350 if (size >= 5) //wide
jsobiecki 4:6ebe8982de0e 351 dirL = rightL * 10 + 0.5 * 10 * 5;
jsobiecki 4:6ebe8982de0e 352 else if(size>4 && size<5) //narrow
jsobiecki 4:6ebe8982de0e 353 dirL = 0.5 * (leftL * 10 + rightL * 10);
jsobiecki 4:6ebe8982de0e 354 else
jsobiecki 4:6ebe8982de0e 355 dirL=9999;
jsobiecki 4:6ebe8982de0e 356 ///////////////////////////////////////////////////////////////////
jsobiecki 4:6ebe8982de0e 357 int secR = R;
jsobiecki 4:6ebe8982de0e 358 while (temp[secR] != 1) {
jsobiecki 4:6ebe8982de0e 359 secR--;
jsobiecki 4:6ebe8982de0e 360 if (secR < 0) secR = 35;
jsobiecki 4:6ebe8982de0e 361 }
jsobiecki 4:6ebe8982de0e 362
jsobiecki 4:6ebe8982de0e 363 int leftR = secR;
jsobiecki 4:6ebe8982de0e 364 int sizeR = 1;
jsobiecki 4:6ebe8982de0e 365
jsobiecki 4:6ebe8982de0e 366 int j = secR - 1;
jsobiecki 4:6ebe8982de0e 367 if (j < 0) j = 35;
jsobiecki 4:6ebe8982de0e 368 while (temp[j] == 1) {
jsobiecki 4:6ebe8982de0e 369 sizeR++;
jsobiecki 4:6ebe8982de0e 370 j--;
jsobiecki 4:6ebe8982de0e 371 if (j < 0) j = 35;
jsobiecki 4:6ebe8982de0e 372 if (j == secR) break;
jsobiecki 4:6ebe8982de0e 373 if (sizeR >= 5) break;
jsobiecki 4:6ebe8982de0e 374 }
jsobiecki 4:6ebe8982de0e 375 int rightR = j + 1;
jsobiecki 4:6ebe8982de0e 376 if (rightR > 35) rightR = 0;
jsobiecki 4:6ebe8982de0e 377 if (sizeR >= 5) //wide
jsobiecki 4:6ebe8982de0e 378 dirR = leftR * 10 + 0.5 * 10 * 5;
jsobiecki 4:6ebe8982de0e 379 else if(sizeR>4 && sizeR<5)//narrow
jsobiecki 4:6ebe8982de0e 380 dirR = 0.5 * (rightR * 10 + leftR * 10);
jsobiecki 4:6ebe8982de0e 381 else
jsobiecki 4:6ebe8982de0e 382 dirR=9999;
jsobiecki 4:6ebe8982de0e 383
jsobiecki 4:6ebe8982de0e 384 if(dirL>360) dirL=fabs(dirL-360);
jsobiecki 4:6ebe8982de0e 385 if(dirR>360) dirR=fabs(dirR-360);
jsobiecki 4:6ebe8982de0e 386 if(fabs(theta_deg-dirL)>fabs(theta_deg-dirR))
jsobiecki 4:6ebe8982de0e 387 dirC=dirR;
jsobiecki 4:6ebe8982de0e 388 else
jsobiecki 4:6ebe8982de0e 389 dirC=dirL;
jsobiecki 4:6ebe8982de0e 390 }
jsobiecki 4:6ebe8982de0e 391 dirSet=dirC;
jsobiecki 4:6ebe8982de0e 392 //cout<<"dirSet: 1"<<endl;
jsobiecki 4:6ebe8982de0e 393
shut 7:5e59f8a011fd 394 ///////////////////////////////////////////////////////////
jsobiecki 4:6ebe8982de0e 395 } else {
jsobiecki 4:6ebe8982de0e 396 int secL = destSec;
jsobiecki 4:6ebe8982de0e 397 while (temp[secL] != 1) {
jsobiecki 4:6ebe8982de0e 398 secL++;
jsobiecki 4:6ebe8982de0e 399 if (secL > 35) secL = 0;
jsobiecki 4:6ebe8982de0e 400 }
jsobiecki 4:6ebe8982de0e 401 int rightL = secL;
jsobiecki 4:6ebe8982de0e 402 int size = 1;
jsobiecki 4:6ebe8982de0e 403
jsobiecki 4:6ebe8982de0e 404 int i = secL + 1;
jsobiecki 4:6ebe8982de0e 405 if (i > 35) i = 0;
jsobiecki 4:6ebe8982de0e 406 while (temp[i] == 1) {
jsobiecki 4:6ebe8982de0e 407 size++;
jsobiecki 4:6ebe8982de0e 408 i++;
jsobiecki 4:6ebe8982de0e 409 if (i > 35) i = 0;
jsobiecki 4:6ebe8982de0e 410 if (i == secL) break;
jsobiecki 4:6ebe8982de0e 411 // Smax here
shut 7:5e59f8a011fd 412 if (size >= 5) break; //5
jsobiecki 4:6ebe8982de0e 413 }
jsobiecki 4:6ebe8982de0e 414 int leftL = i - 1;
jsobiecki 4:6ebe8982de0e 415 if (leftL < 0) leftL = 35;
jsobiecki 4:6ebe8982de0e 416 if (size >= 5) //wide
jsobiecki 4:6ebe8982de0e 417 dirL = rightL * 10 + 0.5 * 10 * 5;
jsobiecki 4:6ebe8982de0e 418 else if(size>4 && size<5) //narrow
jsobiecki 4:6ebe8982de0e 419 dirL = 0.5 * (leftL * 10 + rightL * 10);
jsobiecki 4:6ebe8982de0e 420 else
jsobiecki 4:6ebe8982de0e 421 dirL=9999;
jsobiecki 4:6ebe8982de0e 422 ///////////////////////////////////////////////////////////////////
jsobiecki 4:6ebe8982de0e 423 int secR = destSec;
jsobiecki 4:6ebe8982de0e 424 while (temp[secR] != 1) {
jsobiecki 4:6ebe8982de0e 425 secR--;
jsobiecki 4:6ebe8982de0e 426 if (secR < 0) secR = 35;
jsobiecki 4:6ebe8982de0e 427 }
jsobiecki 4:6ebe8982de0e 428
jsobiecki 4:6ebe8982de0e 429 int leftR = secR;
jsobiecki 4:6ebe8982de0e 430 int sizeR = 1;
jsobiecki 4:6ebe8982de0e 431
jsobiecki 4:6ebe8982de0e 432 int j = secR - 1;
jsobiecki 4:6ebe8982de0e 433 if (j < 0) j = 35;
jsobiecki 4:6ebe8982de0e 434 while (temp[j] == 1) {
jsobiecki 4:6ebe8982de0e 435 sizeR++;
jsobiecki 4:6ebe8982de0e 436 j--;
jsobiecki 4:6ebe8982de0e 437 if (j < 0) j = 35;
jsobiecki 4:6ebe8982de0e 438 if (j == secR) break;
jsobiecki 4:6ebe8982de0e 439 if (sizeR >= 5) break;
jsobiecki 4:6ebe8982de0e 440 }
jsobiecki 4:6ebe8982de0e 441 int rightR = j + 1;
jsobiecki 4:6ebe8982de0e 442 if (rightR > 35) rightR = 0;
jsobiecki 4:6ebe8982de0e 443 if (sizeR >= 5) //wide
jsobiecki 4:6ebe8982de0e 444 dirR = leftR * 10 + 0.5 * 10 * 5;
jsobiecki 4:6ebe8982de0e 445 else if(sizeR>4 && sizeR<5)//narrow
jsobiecki 4:6ebe8982de0e 446 dirR = 0.5 * (rightR * 10 + leftR * 10);
jsobiecki 4:6ebe8982de0e 447 else
jsobiecki 4:6ebe8982de0e 448 dirR=9999;
jsobiecki 4:6ebe8982de0e 449
jsobiecki 4:6ebe8982de0e 450 if(dirL>360) dirL=fabs(dirL-360);
jsobiecki 4:6ebe8982de0e 451 if(dirR>360) dirR=fabs(dirR-360);
jsobiecki 4:6ebe8982de0e 452 if(fabs(theta_deg-dirL)>fabs(theta_deg-dirR))
jsobiecki 4:6ebe8982de0e 453 dirSet=dirR;
jsobiecki 4:6ebe8982de0e 454 else
jsobiecki 4:6ebe8982de0e 455 dirSet=dirL;
jsobiecki 4:6ebe8982de0e 456 //cout<<"dirSet:2 dirR: "<<dirR<<" dirL: "<<dirL<<endl;
jsobiecki 4:6ebe8982de0e 457 }
jsobiecki 4:6ebe8982de0e 458 //cout<<"dirSet: "<<dirSet<<endl;
jsobiecki 4:6ebe8982de0e 459 fX=cos(dirSet*M_PI/180.0f);
jsobiecki 4:6ebe8982de0e 460 fY=sin(dirSet*M_PI/180.0f);
jsobiecki 4:6ebe8982de0e 461
jsobiecki 4:6ebe8982de0e 462 }
shut 6:1c84602323c8 463
shut 6:1c84602323c8 464 void mapping(){
shut 6:1c84602323c8 465 float thetaR_deg, readAngle, lDist, lAng;
shut 7:5e59f8a011fd 466 int xR, yR, xL, yL, cX[400], cY[400], npts, xF, yF;
shut 6:1c84602323c8 467 //------------Processing LIDAR readings------------
shut 6:1c84602323c8 468 lDist = data.distance / 10; //mm -> cm
shut 7:5e59f8a011fd 469 if(lDist == 0 || lDist > 200) return; //TO DO (opt): if less than ~10cm! -> make a turn around
shut 6:1c84602323c8 470 lAng = data.angle;
shut 6:1c84602323c8 471 thetaR_deg = (p[2] * 180.0f) / M_PI; //TO DO: Add the robot's angle in world frame to the lidar's reagin angle (readAngle)
shut 6:1c84602323c8 472 if(thetaR_deg < 0) thetaR_deg = 360 + thetaR_deg;
shut 6:1c84602323c8 473 readAngle = 270 - lAng + thetaR_deg; //Align LIDAR's frame with robot's one
shut 6:1c84602323c8 474 if(readAngle > 360) readAngle -= 360; // " " " " "
shut 6:1c84602323c8 475 else if(readAngle < 0) readAngle += 360;// " " " " "
shut 7:5e59f8a011fd 476 //pc.printf("ReadAngle: %f | data_deg: %f | Distance: %f | pos: (%f,%f)\n", readAngle, lAng, lDist, p[0], p[1]);
shut 6:1c84602323c8 477 readAngle = (readAngle * M_PI) /180.0f; // deg -> rads
shut 6:1c84602323c8 478 xL = lDist * cos(readAngle);
shut 6:1c84602323c8 479 yL = lDist * sin(readAngle);
shut 6:1c84602323c8 480 xR = p[0]/5, yR = p[1]/5, xL = xL/5, yL = yL/5; // cm -> cell index units
shut 7:5e59f8a011fd 481 xF = xR + xL; //(xR - xL) -> delta distance THINK IS WRONG should be delta = yL-yR;
shut 7:5e59f8a011fd 482 yF = yR + yL;//(yR - yL) -> delta distance THINK IS WRONG should be delta = yL-yR;
shut 7:5e59f8a011fd 483 //if(xF < 0 || yF < 0 || xF > 80 || yF > 80) return;
shut 7:5e59f8a011fd 484 //pc.printf("xF: %d | yF: %d", xF, yF);
shut 6:1c84602323c8 485 npts = Bresenham(xR, yR, xF, yF, cX, cY);
shut 7:5e59f8a011fd 486 log_cell(cX, cY, npts);
shut 6:1c84602323c8 487 prob_calc();
shut 6:1c84602323c8 488 }
shut 6:1c84602323c8 489 //Bresenham algo -> trace the Lidar's reading line into a bitmap a.k.a. cell's index
shut 6:1c84602323c8 490 int Bresenham(int x1, int y1, int x2, int y2, int cX[], int cY[]){
shut 6:1c84602323c8 491 int aux = 0;
shut 6:1c84602323c8 492 int delta_x(x2 - x1);
shut 6:1c84602323c8 493 // if xR == x2, then it does not matter what we set here
shut 6:1c84602323c8 494 signed char const ix((delta_x > 0) - (delta_x < 0));
shut 6:1c84602323c8 495 delta_x = std::abs(delta_x) << 1;
shut 6:1c84602323c8 496 int delta_y(y2 - y1);
shut 6:1c84602323c8 497 // if yR == y2, then it does not matter what we set here
shut 6:1c84602323c8 498 signed char const iy((delta_y > 0) - (delta_y < 0));
shut 6:1c84602323c8 499 delta_y = std::abs(delta_y) << 1;
shut 6:1c84602323c8 500 cX[aux] = x1, cY[aux] = y1, aux++;
shut 7:5e59f8a011fd 501 //pc.printf("x = %d | y = %d | cx = %d | cy = %d | aux = %d \n", x1, y1, cX[aux-1], cY[aux-1], aux-1);
shut 6:1c84602323c8 502 if (delta_x >= delta_y){
shut 6:1c84602323c8 503 // error may go below zero
shut 6:1c84602323c8 504 int error(delta_y - (delta_x >> 1));
shut 6:1c84602323c8 505 while (x1 != x2)
shut 6:1c84602323c8 506 {
shut 6:1c84602323c8 507 // reduce error, while taking into account the corner case of error == 0
shut 6:1c84602323c8 508 if ((error > 0) || (!error && (ix > 0))){
shut 6:1c84602323c8 509 error -= delta_x;
shut 6:1c84602323c8 510 y1 += iy;
shut 6:1c84602323c8 511 }
shut 6:1c84602323c8 512 error += delta_y;
shut 6:1c84602323c8 513 x1 += ix;
shut 6:1c84602323c8 514 cX[aux] = x1, cY[aux] = y1, aux++;
shut 7:5e59f8a011fd 515 //pc.printf("x = %d | y = %d | cx = %d | cy = %d | aux = %d \n", x1, y1, cX[aux-1], cY[aux-1], aux-1);
shut 6:1c84602323c8 516 }
shut 6:1c84602323c8 517 }
shut 6:1c84602323c8 518 else{
shut 6:1c84602323c8 519 // error may go below zero
shut 6:1c84602323c8 520 int error(delta_x - (delta_y >> 1));
shut 6:1c84602323c8 521 while (y1 != y2){
shut 6:1c84602323c8 522 // reduce error, while taking into account the corner case of error == 0
shut 6:1c84602323c8 523 if ((error > 0) || (!error && (iy > 0))){
shut 6:1c84602323c8 524 error -= delta_y;
shut 6:1c84602323c8 525 x1 += ix;
shut 6:1c84602323c8 526 }
shut 6:1c84602323c8 527 error += delta_x;
shut 6:1c84602323c8 528 y1 += iy;
shut 6:1c84602323c8 529 cX[aux] = x1, cY[aux] = y1, aux++;
shut 7:5e59f8a011fd 530 //pc.printf("x = %d | y = %d | cx = %d | cy = %d | aux = %d \n", x1, y1, cX[aux-1], cY[aux-1], aux-1);
shut 6:1c84602323c8 531 }
shut 6:1c84602323c8 532 }
shut 6:1c84602323c8 533 return aux;
shut 6:1c84602323c8 534 }
shut 6:1c84602323c8 535 void log_cell(int cX[], int cY[], int npts){
shut 6:1c84602323c8 536 float l_occ = 0.65;
shut 6:1c84602323c8 537 float l_free = -0.65;
shut 6:1c84602323c8 538 for(int i = 0; i < (npts-1); i++){
shut 7:5e59f8a011fd 539 if(cX[i] < 0 || cY[i] < 0) break;
shut 6:1c84602323c8 540 histogram[cX[i]][cY[i]].logodds = histogram[cX[i]][cY[i]].logodds + l_free; //l0 já inicializado com o array
shut 6:1c84602323c8 541 }
shut 7:5e59f8a011fd 542 histogram[cX[npts-1]][cY[npts-1]].logodds = histogram[cX[npts-1]][cY[npts-1]].logodds + l_occ;
shut 7:5e59f8a011fd 543 /*for (int i = -1; i < 2; i++){
shut 7:5e59f8a011fd 544 for (int j = -1; j < 2; j++){
shut 7:5e59f8a011fd 545 histogram[cX[npts-1]+i][cY[npts-1]+j].logodds = histogram[cX[npts-1]+i][cY[npts-1]+j].logodds + l_occ;
shut 7:5e59f8a011fd 546 }
shut 7:5e59f8a011fd 547 }*/
shut 6:1c84602323c8 548 //Força precisa de ser calculada de log odds para probabilidades: <50% de ocupação = livre
shut 6:1c84602323c8 549 //Se o valor da repulsiva óptimo era 3, então vai oscilar entre 0 e 3 - 50 e 100%
shut 6:1c84602323c8 550 }
shut 6:1c84602323c8 551 void prob_calc(){
shut 6:1c84602323c8 552 float aux;
shut 6:1c84602323c8 553 for (int i = 0; i < hSize; i++) {
shut 6:1c84602323c8 554 for (int j = 0; j < hSize; j++) {
shut 6:1c84602323c8 555 aux = (1.0f - (1.0f/(1.0f+exp(histogram[i][j].logodds))));
shut 6:1c84602323c8 556 if(aux > 0.5f){
shut 7:5e59f8a011fd 557 histogram[i][j].cellVal = 20*aux; //6
shut 6:1c84602323c8 558 }
shut 6:1c84602323c8 559 }
shut 6:1c84602323c8 560 }
shut 6:1c84602323c8 561 }