SRM / Mbed OS LabWork3Lidar

Dependencies:   BufferedSerial

main.cpp@4:6ebe8982de0e, 2019-05-06 (annotated)

Committer:
jsobiecki
Date:
Mon May 06 17:15:52 2019 +0000
Revision:
4:6ebe8982de0e
Parent:
3:0a718d139ed1
Child:
5:5653e559a67b
add lidar

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jsobiecki 4:6ebe8982de0e 1 // Coded by Luís Afonso 11-04-2019
LuisRA 0:2b691d200d6f 2 #include "mbed.h"
LuisRA 0:2b691d200d6f 3 #include "BufferedSerial.h"
LuisRA 0:2b691d200d6f 4 #include "rplidar.h"
fabiofaria 1:dc87724abce8 5 #include "Robot.h"
fabiofaria 1:dc87724abce8 6 #include "Communication.h"
jsobiecki 4:6ebe8982de0e 7 #include "math.h"
jsobiecki 4:6ebe8982de0e 8 #include "ActiveCell.h"
jsobiecki 4:6ebe8982de0e 9 #include "HistogramCell.h"
jsobiecki 4:6ebe8982de0e 10 #define M_PI 3.14159265358979323846f
LuisRA 0:2b691d200d6f 11
LuisRA 0:2b691d200d6f 12 RPLidar lidar;
LuisRA 0:2b691d200d6f 13 BufferedSerial se_lidar(PA_9, PA_10);
fabiofaria 3:0a718d139ed1 14 PwmOut rplidar_motor(D3);
LuisRA 0:2b691d200d6f 15
jsobiecki 4:6ebe8982de0e 16 //EXERCICIO 1
jsobiecki 4:6ebe8982de0e 17 //Luis Cruz N2011164454
jsobiecki 4:6ebe8982de0e 18 //Jacek Sobecki N2018319609
jsobiecki 4:6ebe8982de0e 19 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
jsobiecki 4:6ebe8982de0e 20 DigitalIn button(PC_13);
jsobiecki 4:6ebe8982de0e 21 void poseEst(float p[], float radius, float enc_res, float b);
jsobiecki 4:6ebe8982de0e 22 void SpeedLim(float w[]);
jsobiecki 4:6ebe8982de0e 23 void initializeArrays();
jsobiecki 4:6ebe8982de0e 24 void calcSectors(float theta);
jsobiecki 4:6ebe8982de0e 25 void sumForces();
jsobiecki 4:6ebe8982de0e 26 void updateActive(float xR, float yR,float theta);
jsobiecki 4:6ebe8982de0e 27 //int ReadSensors();
jsobiecki 4:6ebe8982de0e 28 //const int m = 200, n = 200, activeSize = 11;
jsobiecki 4:6ebe8982de0e 29 //histogram size | aSize active region size
jsobiecki 4:6ebe8982de0e 30 const int hSize = 80, aSize = 11;
jsobiecki 4:6ebe8982de0e 31 ActiveCell activeReg[aSize][aSize];
jsobiecki 4:6ebe8982de0e 32 HistogramCell histogram[hSize][hSize];
jsobiecki 4:6ebe8982de0e 33 //Repulsive force sums
jsobiecki 4:6ebe8982de0e 34 float p[3], p_obj[3], p_final[3], fX, fY;
jsobiecki 4:6ebe8982de0e 35 const float Fca=6;/*5*/
jsobiecki 4:6ebe8982de0e 36
jsobiecki 4:6ebe8982de0e 37 //VFH
jsobiecki 4:6ebe8982de0e 38 const int L=2;
jsobiecki 4:6ebe8982de0e 39 float secVal[36];
jsobiecki 4:6ebe8982de0e 40 float smooth[36];
jsobiecki 4:6ebe8982de0e 41
LuisRA 0:2b691d200d6f 42 int main()
LuisRA 0:2b691d200d6f 43 {
fabiofaria 1:dc87724abce8 44 float odomX, odomY, odomTheta;
fabiofaria 1:dc87724abce8 45 struct RPLidarMeasurement data;
fabiofaria 1:dc87724abce8 46
fabiofaria 1:dc87724abce8 47 pc.baud(115200);
fabiofaria 1:dc87724abce8 48 init_communication(&pc);
LuisRA 0:2b691d200d6f 49
fabiofaria 1:dc87724abce8 50 // Lidar initialization
LuisRA 0:2b691d200d6f 51 rplidar_motor.period(0.001f);
LuisRA 0:2b691d200d6f 52 lidar.begin(se_lidar);
LuisRA 0:2b691d200d6f 53 lidar.setAngle(0,360);
LuisRA 0:2b691d200d6f 54
LuisRA 0:2b691d200d6f 55 pc.printf("Program started.\n");
jsobiecki 4:6ebe8982de0e 56
LuisRA 0:2b691d200d6f 57 lidar.startThreadScan();
LuisRA 0:2b691d200d6f 58
jsobiecki 4:6ebe8982de0e 59 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jsobiecki 4:6ebe8982de0e 60 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jsobiecki 4:6ebe8982de0e 61 button.mode(PullUp);
jsobiecki 4:6ebe8982de0e 62 getCountsAndReset();
jsobiecki 4:6ebe8982de0e 63 setSpeeds(0, 0);
jsobiecki 4:6ebe8982de0e 64 initializeArrays();
jsobiecki 4:6ebe8982de0e 65 while(button==1);
jsobiecki 4:6ebe8982de0e 66 rplidar_motor.write(0.7f);
jsobiecki 4:6ebe8982de0e 67 //w[0] = Omega | w[1] = Left | w[2] = Right
jsobiecki 4:6ebe8982de0e 68 //p[0] = X | p[1] = Y | p[2] = Theta
jsobiecki 4:6ebe8982de0e 69 //p_obj[0] = X | p_obj[1] = Y | p_obj[2] = Theta
jsobiecki 4:6ebe8982de0e 70 //b = Distance between wheels, enc_res = Encoder Resolution, v = Calculated speed
jsobiecki 4:6ebe8982de0e 71 //k_v = Speed gain, k_s = Curvature gain, wratio = Angular speed ratio control command
jsobiecki 4:6ebe8982de0e 72 //Cells dim: 5x5cm |
jsobiecki 4:6ebe8982de0e 73 float w[3], v, theta, theta_error, err, integral = 0.0, k_i = 0.01/*0.02*/;
jsobiecki 4:6ebe8982de0e 74 const float radius = 3.5, b = 13.3, enc_res = 1440, k_v = 8/*7*/,
jsobiecki 4:6ebe8982de0e 75 k_s = 12/*10*/, sample_time = 0.05, d_stalker = 5, k_f = 12.5; /*12.5*/ //VFF
jsobiecki 4:6ebe8982de0e 76 float theta_final;
jsobiecki 4:6ebe8982de0e 77 // ===============================================================================
jsobiecki 4:6ebe8982de0e 78 // =================================== COORDS ====================================
jsobiecki 4:6ebe8982de0e 79 // ===============================================================================
jsobiecki 4:6ebe8982de0e 80 //Target coordinates
jsobiecki 4:6ebe8982de0e 81 p_final[0] = 100, p_final[1] = 20, p_final[2] = 0;
jsobiecki 4:6ebe8982de0e 82 //p_obj[0] = 20, p_obj[1] = 20, p_obj[2] = 0;
jsobiecki 4:6ebe8982de0e 83 //Initial coordinates:
jsobiecki 4:6ebe8982de0e 84 p[0] = 20, p[1] = 20, p[2] = 0;
jsobiecki 4:6ebe8982de0e 85 // ===============================================================================
jsobiecki 4:6ebe8982de0e 86 // =================================== EXECUTION =================================
jsobiecki 4:6ebe8982de0e 87 // ===============================================================================
jsobiecki 4:6ebe8982de0e 88 while(1){
LuisRA 0:2b691d200d6f 89 // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
LuisRA 0:2b691d200d6f 90 if(lidar.pollSensorData(&data) == 0)
LuisRA 0:2b691d200d6f 91 {
jsobiecki 4:6ebe8982de0e 92 pc.printf("dist:%f angle:%f %d %c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
jsobiecki 4:6ebe8982de0e 93 }
jsobiecki 4:6ebe8982de0e 94
jsobiecki 4:6ebe8982de0e 95
jsobiecki 4:6ebe8982de0e 96 getCountsAndReset();
jsobiecki 4:6ebe8982de0e 97 //pc.printf("Speeds: Left=%lf Right=%lf\n", w[1], w[2]);
jsobiecki 4:6ebe8982de0e 98 //pc.printf("OBJECTIVE X: %lf OBJECTIVE Y: %lf\n", p_obj[0], p_obj[1]);
jsobiecki 4:6ebe8982de0e 99 //pc.printf("Position: X=%lf Y=%lf Theta=%lf\n\n", p[0], p[1], p[2]);
jsobiecki 4:6ebe8982de0e 100 //pc.printf("Force (X): X=%lf Force(Y)=%lf\n", fX, fY);
jsobiecki 4:6ebe8982de0e 101
jsobiecki 4:6ebe8982de0e 102
jsobiecki 4:6ebe8982de0e 103 //------------Process lidar readings
jsobiecki 4:6ebe8982de0e 104 //theta of the robot in degrees
jsobiecki 4:6ebe8982de0e 105 float thetaR_deg = 0;
jsobiecki 4:6ebe8982de0e 106 //float thetaR_deg = (p[2]*180.0f)/M_PI;
jsobiecki 4:6ebe8982de0e 107 if(thetaR_deg <0) thetaR_deg=360+thetaR_deg;
jsobiecki 4:6ebe8982de0e 108 //real angle of the reading
jsobiecki 4:6ebe8982de0e 109 if(data.angle<360 && data.angle>0) data.angle=360-data.angle;
jsobiecki 4:6ebe8982de0e 110 float readAngle = data.angle - 180 + thetaR_deg;
jsobiecki 4:6ebe8982de0e 111 if(readAngle>=360) readAngle=fmod(readAngle,360);
jsobiecki 4:6ebe8982de0e 112
jsobiecki 4:6ebe8982de0e 113 pc.printf("Robots_deg: %f data_deg: %f readAngle: %f\n",thetaR_deg,data.angle,readAngle);
jsobiecki 4:6ebe8982de0e 114
jsobiecki 4:6ebe8982de0e 115 //update cells according to a reading
jsobiecki 4:6ebe8982de0e 116 if(data.distance!=0 && data.distance<200){
jsobiecki 4:6ebe8982de0e 117 for(int i=0;i<11;i++){
jsobiecki 4:6ebe8982de0e 118 for (int j = 0; j < 11; j++) {
jsobiecki 4:6ebe8982de0e 119 if(readAngle<=activeReg[i][j].sectorK*10+5 && readAngle>=activeReg[i][j].sectorK*10-5){
jsobiecki 4:6ebe8982de0e 120 if(data.distance<=((double)activeReg[i][j].distance+2.5)*10 && data.distance>=((double)activeReg[i][j].distance-2.5)*10) {
jsobiecki 4:6ebe8982de0e 121 //cell is occupied
jsobiecki 4:6ebe8982de0e 122 activeReg[i][j].cellVal = 3;
jsobiecki 4:6ebe8982de0e 123 } else if (data.distance>((double)activeReg[i][j].distance-2.5)*10){
jsobiecki 4:6ebe8982de0e 124 //cell is unoccupied
jsobiecki 4:6ebe8982de0e 125 activeReg[i][j].cellVal = 1;
jsobiecki 4:6ebe8982de0e 126 }
jsobiecki 4:6ebe8982de0e 127 }
jsobiecki 4:6ebe8982de0e 128 }
jsobiecki 4:6ebe8982de0e 129 }
jsobiecki 4:6ebe8982de0e 130 }
jsobiecki 4:6ebe8982de0e 131
jsobiecki 4:6ebe8982de0e 132 for (int j = 10; j >= 0; j--) {
jsobiecki 4:6ebe8982de0e 133 for (int i = 0; i < 11; i++) {
jsobiecki 4:6ebe8982de0e 134 cout << "[" << activeReg[i][j].cellVal << "]";
jsobiecki 4:6ebe8982de0e 135 }
jsobiecki 4:6ebe8982de0e 136 cout << endl;
jsobiecki 4:6ebe8982de0e 137 }
jsobiecki 4:6ebe8982de0e 138
jsobiecki 4:6ebe8982de0e 139
jsobiecki 4:6ebe8982de0e 140 //Path calculation
jsobiecki 4:6ebe8982de0e 141 poseEst(p, radius, enc_res, b); //Pose estimation
jsobiecki 4:6ebe8982de0e 142 theta_final = atan2(p_final[1]-p[1],p_final[0]-p[0]);
jsobiecki 4:6ebe8982de0e 143 theta_final = atan2(sin(theta_final),cos(theta_final));
jsobiecki 4:6ebe8982de0e 144 //updateActive(p[0], p[1], theta_final);
jsobiecki 4:6ebe8982de0e 145 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 146 p_obj[1] = p[1]+k_f*fY;
jsobiecki 4:6ebe8982de0e 147 //Control Law
jsobiecki 4:6ebe8982de0e 148 err = sqrt(pow((p_obj[0]-p[0]),2)+pow((p_obj[1]-p[1]),2)) - d_stalker; //distance to the point
jsobiecki 4:6ebe8982de0e 149 theta = atan2(p_obj[1]-p[1],p_obj[0]-p[0]);
jsobiecki 4:6ebe8982de0e 150 //pc.printf("theta MAIN: = %lf\n\n", theta);
jsobiecki 4:6ebe8982de0e 151 theta = atan2(sin(theta),cos(theta));
jsobiecki 4:6ebe8982de0e 152
jsobiecki 4:6ebe8982de0e 153
jsobiecki 4:6ebe8982de0e 154
jsobiecki 4:6ebe8982de0e 155 p[2] = atan2(sin(p[2]),cos(p[2]));
jsobiecki 4:6ebe8982de0e 156 theta_error = theta-p[2];
jsobiecki 4:6ebe8982de0e 157 theta_error = atan2(sin(theta_error),cos(theta_error));
jsobiecki 4:6ebe8982de0e 158 //pc.printf("theta_error = %lf | p[2]= %lf\n\n", theta_error, p[2]);
jsobiecki 4:6ebe8982de0e 159 w[0] = k_s*(theta_error); //direction gain
jsobiecki 4:6ebe8982de0e 160 integral += err;
jsobiecki 4:6ebe8982de0e 161 v = k_v*err+k_i*integral; //Speed calculation
jsobiecki 4:6ebe8982de0e 162 w[1] = (v-(b/2)*w[0])/radius;
jsobiecki 4:6ebe8982de0e 163 w[2] = (v+(b/2)*w[0])/radius;
jsobiecki 4:6ebe8982de0e 164
jsobiecki 4:6ebe8982de0e 165 SpeedLim(w);
jsobiecki 4:6ebe8982de0e 166 //if((fabs(p[0]-p_final[0])+fabs(p[1]-p_final[1])) < 70) k_i = -0.005;
jsobiecki 4:6ebe8982de0e 167 if((fabs(p[0]-p_final[0])+fabs(p[1]-p_final[1])) < 5){
jsobiecki 4:6ebe8982de0e 168 setSpeeds(0,0);
jsobiecki 4:6ebe8982de0e 169 }
jsobiecki 4:6ebe8982de0e 170 else{
jsobiecki 4:6ebe8982de0e 171 //setSpeeds(w[1], w[2]);
jsobiecki 4:6ebe8982de0e 172 }
jsobiecki 4:6ebe8982de0e 173 wait(sample_time);
jsobiecki 4:6ebe8982de0e 174 }
jsobiecki 4:6ebe8982de0e 175 }
jsobiecki 4:6ebe8982de0e 176 // ===============================================================================
jsobiecki 4:6ebe8982de0e 177 // =================================== FUNCTIONS =================================
jsobiecki 4:6ebe8982de0e 178 // ===============================================================================
jsobiecki 4:6ebe8982de0e 179 //Pose Estimation function
jsobiecki 4:6ebe8982de0e 180 void poseEst(float p[], float radius, float enc_res, float b){
jsobiecki 4:6ebe8982de0e 181 float deltaDl, deltaDr, deltaD, deltaT;
jsobiecki 4:6ebe8982de0e 182 deltaDl = ((float)countsLeft)*(2.0f*M_PI*radius/enc_res);
jsobiecki 4:6ebe8982de0e 183 deltaDr = ((float)countsRight)*(2.0f*M_PI*radius/enc_res);
jsobiecki 4:6ebe8982de0e 184 deltaD = (deltaDr + deltaDl)/2.0f;
jsobiecki 4:6ebe8982de0e 185 deltaT = (deltaDr - deltaDl)/b;
jsobiecki 4:6ebe8982de0e 186 if(fabs(deltaT) == 0){
jsobiecki 4:6ebe8982de0e 187 p[0] = p[0] + deltaD*cos(p[2]) + deltaT/2;
jsobiecki 4:6ebe8982de0e 188 p[1] = p[1] + deltaD*sin(p[2]) + deltaT/2;
jsobiecki 4:6ebe8982de0e 189 return;
jsobiecki 4:6ebe8982de0e 190 }
jsobiecki 4:6ebe8982de0e 191 p[0] = p[0] + deltaD*(sin(deltaT/2.0f)/(deltaT/2.0f))*cos(p[2]+deltaT/2.0f);
jsobiecki 4:6ebe8982de0e 192 p[1] = p[1] + deltaD*(sin(deltaT/2.0f)/(deltaT/2.0f))*sin(p[2]+deltaT/2.0f);
jsobiecki 4:6ebe8982de0e 193 p[2] = p[2] + deltaT;
jsobiecki 4:6ebe8982de0e 194 }
jsobiecki 4:6ebe8982de0e 195 //Speed limiter function
jsobiecki 4:6ebe8982de0e 196 void SpeedLim(float w[]){
jsobiecki 4:6ebe8982de0e 197 float wratio;
jsobiecki 4:6ebe8982de0e 198 wratio = fabs(w[2]/w[1]);
jsobiecki 4:6ebe8982de0e 199 if(w[2] > 150 || w[1] > 150){
jsobiecki 4:6ebe8982de0e 200 if(wratio < 1){
jsobiecki 4:6ebe8982de0e 201 w[1] = 150;
jsobiecki 4:6ebe8982de0e 202 w[2] = w[1]*wratio;
jsobiecki 4:6ebe8982de0e 203 }
jsobiecki 4:6ebe8982de0e 204 else if(wratio > 1){
jsobiecki 4:6ebe8982de0e 205 w[2] = 150;
jsobiecki 4:6ebe8982de0e 206 w[1] = w[2]/wratio;
jsobiecki 4:6ebe8982de0e 207 }
jsobiecki 4:6ebe8982de0e 208 else{
jsobiecki 4:6ebe8982de0e 209 w[2] = 150;
jsobiecki 4:6ebe8982de0e 210 w[1] = 150;
jsobiecki 4:6ebe8982de0e 211 }
jsobiecki 4:6ebe8982de0e 212 }
jsobiecki 4:6ebe8982de0e 213 if(w[2] < 50 || w[1] < 50){
jsobiecki 4:6ebe8982de0e 214 if(wratio < 1){
jsobiecki 4:6ebe8982de0e 215 w[1] = 50;
jsobiecki 4:6ebe8982de0e 216 w[2] = w[1]*wratio;
jsobiecki 4:6ebe8982de0e 217 }
jsobiecki 4:6ebe8982de0e 218 else if(wratio > 1){
jsobiecki 4:6ebe8982de0e 219 w[2] = 50;
jsobiecki 4:6ebe8982de0e 220 w[1] = w[2]/wratio;
jsobiecki 4:6ebe8982de0e 221 }
jsobiecki 4:6ebe8982de0e 222 else{
jsobiecki 4:6ebe8982de0e 223 w[2] = 50;
jsobiecki 4:6ebe8982de0e 224 w[1] = 50;
jsobiecki 4:6ebe8982de0e 225 }
jsobiecki 4:6ebe8982de0e 226 }
jsobiecki 4:6ebe8982de0e 227 }
jsobiecki 4:6ebe8982de0e 228
jsobiecki 4:6ebe8982de0e 229 void initializeArrays() {
jsobiecki 4:6ebe8982de0e 230 for (int i = 0; i < hSize; i++) {
jsobiecki 4:6ebe8982de0e 231 for (int j = 0; j < hSize; j++) {
jsobiecki 4:6ebe8982de0e 232 histogram[i][j].calculate(i, j);
jsobiecki 4:6ebe8982de0e 233 //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 234 //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 235 //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 236 }
jsobiecki 4:6ebe8982de0e 237 }
jsobiecki 4:6ebe8982de0e 238 for (int i = 0; i < aSize; i++) {
jsobiecki 4:6ebe8982de0e 239 for (int j = 0; j < aSize; j++) {
jsobiecki 4:6ebe8982de0e 240 activeReg[i][j].calDist(i, j);
jsobiecki 4:6ebe8982de0e 241 }
jsobiecki 4:6ebe8982de0e 242 }
jsobiecki 4:6ebe8982de0e 243 }
jsobiecki 4:6ebe8982de0e 244
jsobiecki 4:6ebe8982de0e 245 //every time robot changes position we need to call this function to update active region and calculate forces
jsobiecki 4:6ebe8982de0e 246 //xR, yR - robots position in coordinates system
jsobiecki 4:6ebe8982de0e 247 void updateActive(float xR, float yR,float theta) {
jsobiecki 4:6ebe8982de0e 248 int idXr = 0;
jsobiecki 4:6ebe8982de0e 249 int idYr = 0;
jsobiecki 4:6ebe8982de0e 250 for (int i = 0; i < hSize; i++) {
jsobiecki 4:6ebe8982de0e 251 for (int j = 0; j < hSize; j++) {
jsobiecki 4:6ebe8982de0e 252 if (xR > histogram[i][j].x - 2.5 && xR < histogram[i][j].x + 2.5 && yR > histogram[i][j].y - 2.5 &&
jsobiecki 4:6ebe8982de0e 253 yR < histogram[i][j].y + 2.5) {
jsobiecki 4:6ebe8982de0e 254 idXr = i;
jsobiecki 4:6ebe8982de0e 255 idYr = j;
jsobiecki 4:6ebe8982de0e 256 break;
jsobiecki 4:6ebe8982de0e 257 }
jsobiecki 4:6ebe8982de0e 258 }
jsobiecki 4:6ebe8982de0e 259 }
jsobiecki 4:6ebe8982de0e 260 int m = idXr - aSize / 2;
jsobiecki 4:6ebe8982de0e 261 for (int k = 0; k < aSize; k++) {
jsobiecki 4:6ebe8982de0e 262 int n = idYr - aSize / 2;
jsobiecki 4:6ebe8982de0e 263 for (int l = 0; l < aSize; l++) {
jsobiecki 4:6ebe8982de0e 264 if (m >= 0 && n >= 0 && m < hSize && n < hSize) {
jsobiecki 4:6ebe8982de0e 265 histogram[m][n].cellVal=activeReg[k][l].cellVal;
jsobiecki 4:6ebe8982de0e 266 }
jsobiecki 4:6ebe8982de0e 267 n++;
jsobiecki 4:6ebe8982de0e 268 }
jsobiecki 4:6ebe8982de0e 269 m++;
jsobiecki 4:6ebe8982de0e 270 }
jsobiecki 4:6ebe8982de0e 271 for (int i = 0; i < aSize; ++i) {
jsobiecki 4:6ebe8982de0e 272 for (int j = 0; j < aSize; ++j) {
jsobiecki 4:6ebe8982de0e 273 activeReg[i][j].calForce();
jsobiecki 4:6ebe8982de0e 274 }
jsobiecki 4:6ebe8982de0e 275 }
jsobiecki 4:6ebe8982de0e 276 activeReg[5][5].amplitude=0;
jsobiecki 4:6ebe8982de0e 277 activeReg[5][5].amplitude=0;
jsobiecki 4:6ebe8982de0e 278
jsobiecki 4:6ebe8982de0e 279 for (int j = 10; j >= 0; j--) {
jsobiecki 4:6ebe8982de0e 280 for (int i = 0; i < 11; i++) {
jsobiecki 4:6ebe8982de0e 281 cout << "[" << activeReg[i][j].cellVal << "]";
jsobiecki 4:6ebe8982de0e 282 }
jsobiecki 4:6ebe8982de0e 283 cout << endl;
jsobiecki 4:6ebe8982de0e 284 }
jsobiecki 4:6ebe8982de0e 285
jsobiecki 4:6ebe8982de0e 286 calcSectors(theta);
jsobiecki 4:6ebe8982de0e 287 }
jsobiecki 4:6ebe8982de0e 288 void calcSectors(float theta){
jsobiecki 4:6ebe8982de0e 289 for (int k = 0; k < 36; ++k) {
jsobiecki 4:6ebe8982de0e 290 secVal[k]=0;
jsobiecki 4:6ebe8982de0e 291 for (int i = 0; i < aSize; ++i) {
jsobiecki 4:6ebe8982de0e 292 for (int j = 0; j < aSize; ++j) {
jsobiecki 4:6ebe8982de0e 293 if(activeReg[i][j].sectorK==k)
jsobiecki 4:6ebe8982de0e 294 secVal[k]+=activeReg[i][j].amplitude;
jsobiecki 4:6ebe8982de0e 295 }
LuisRA 0:2b691d200d6f 296 }
jsobiecki 4:6ebe8982de0e 297 }
jsobiecki 4:6ebe8982de0e 298
jsobiecki 4:6ebe8982de0e 299 smooth[0]=(secVal[34]+2*secVal[35]+2*secVal[0]+2*secVal[1]+secVal[2])/5;
jsobiecki 4:6ebe8982de0e 300 smooth[1]=(secVal[35]+2*secVal[0]+2*secVal[1]+2*secVal[2]+secVal[3])/5;
jsobiecki 4:6ebe8982de0e 301 smooth[34]=(secVal[32]+2*secVal[33]+2*secVal[34]+2*secVal[35]+secVal[0])/5;
jsobiecki 4:6ebe8982de0e 302 smooth[35]=(secVal[33]+2*secVal[34]+2*secVal[35]+2*secVal[0]+secVal[1])/5;
jsobiecki 4:6ebe8982de0e 303 for (int i = 2; i < 34; ++i) {
jsobiecki 4:6ebe8982de0e 304 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 305 }
jsobiecki 4:6ebe8982de0e 306
jsobiecki 4:6ebe8982de0e 307 const int thresh=200;//100
jsobiecki 4:6ebe8982de0e 308 int temp[36];
jsobiecki 4:6ebe8982de0e 309 int counter = 0, aux = 0;
jsobiecki 4:6ebe8982de0e 310 int valley[36];
jsobiecki 4:6ebe8982de0e 311 for(int i=0;i<36;++i){
jsobiecki 4:6ebe8982de0e 312 //pc.printf("|%lf", smooth[i]);
jsobiecki 4:6ebe8982de0e 313 if(smooth[i]<thresh){
jsobiecki 4:6ebe8982de0e 314 temp[i]=1;
jsobiecki 4:6ebe8982de0e 315 //valley[aux][aux] =
jsobiecki 4:6ebe8982de0e 316 counter++;
jsobiecki 4:6ebe8982de0e 317 }
jsobiecki 4:6ebe8982de0e 318 else{
jsobiecki 4:6ebe8982de0e 319 valley[aux] = counter;
jsobiecki 4:6ebe8982de0e 320 counter = 0;
jsobiecki 4:6ebe8982de0e 321 aux++;
jsobiecki 4:6ebe8982de0e 322 temp[i]=0;
jsobiecki 4:6ebe8982de0e 323 //pc.printf("#%d", i);
jsobiecki 4:6ebe8982de0e 324 }
jsobiecki 4:6ebe8982de0e 325
jsobiecki 4:6ebe8982de0e 326 }
jsobiecki 4:6ebe8982de0e 327 float best=999;
jsobiecki 4:6ebe8982de0e 328 float theta_deg;
jsobiecki 4:6ebe8982de0e 329 theta_deg =(theta*180.0f)/M_PI;
jsobiecki 4:6ebe8982de0e 330 //pc.printf("theta (degrees): = %lf\n\n", theta_deg);
jsobiecki 4:6ebe8982de0e 331 int destSec = theta_deg / 10;
jsobiecki 4:6ebe8982de0e 332 if(destSec<0) destSec=36+destSec;
jsobiecki 4:6ebe8982de0e 333 //cout<<"destination sector: "<<destSec<<endl;
jsobiecki 4:6ebe8982de0e 334
jsobiecki 4:6ebe8982de0e 335 int L=destSec;
jsobiecki 4:6ebe8982de0e 336 int R=destSec;
jsobiecki 4:6ebe8982de0e 337 while(temp[L]==0){
jsobiecki 4:6ebe8982de0e 338 L--;
jsobiecki 4:6ebe8982de0e 339 if(L<0) L=35;
jsobiecki 4:6ebe8982de0e 340 }
jsobiecki 4:6ebe8982de0e 341 while(temp[R]==0){
jsobiecki 4:6ebe8982de0e 342 R++;
jsobiecki 4:6ebe8982de0e 343 if(R>35) R=0;
LuisRA 0:2b691d200d6f 344 }
jsobiecki 4:6ebe8982de0e 345
jsobiecki 4:6ebe8982de0e 346 float dirSet, dirC,dirL,dirR;
jsobiecki 4:6ebe8982de0e 347 if(temp[destSec]==1){
jsobiecki 4:6ebe8982de0e 348 int k=destSec-1;
jsobiecki 4:6ebe8982de0e 349 if(k<0) k=35;
jsobiecki 4:6ebe8982de0e 350 int size=1;
jsobiecki 4:6ebe8982de0e 351 while(temp[k]==1){
jsobiecki 4:6ebe8982de0e 352 size++;
jsobiecki 4:6ebe8982de0e 353 k--;
jsobiecki 4:6ebe8982de0e 354 if(k<0) k=35;
jsobiecki 4:6ebe8982de0e 355 if(k==destSec) break;
jsobiecki 4:6ebe8982de0e 356 if(size>=5) break;
jsobiecki 4:6ebe8982de0e 357 }
jsobiecki 4:6ebe8982de0e 358 int right=k+1;
jsobiecki 4:6ebe8982de0e 359 if(right<0) right=35;
jsobiecki 4:6ebe8982de0e 360 k=destSec+1;
jsobiecki 4:6ebe8982de0e 361 if(k>35) k=0;
jsobiecki 4:6ebe8982de0e 362 while(temp[k]==1){
jsobiecki 4:6ebe8982de0e 363 size++;
jsobiecki 4:6ebe8982de0e 364 k++;
jsobiecki 4:6ebe8982de0e 365 if(k>35) k=0;
jsobiecki 4:6ebe8982de0e 366 if(k==destSec) break;
jsobiecki 4:6ebe8982de0e 367 if(size>=5) break;
jsobiecki 4:6ebe8982de0e 368 }
jsobiecki 4:6ebe8982de0e 369 int left=k-1;
jsobiecki 4:6ebe8982de0e 370 if(left>35) left=0;
jsobiecki 4:6ebe8982de0e 371 if(size>=5) {
jsobiecki 4:6ebe8982de0e 372 //wide
jsobiecki 4:6ebe8982de0e 373 dirC=destSec*10;
jsobiecki 4:6ebe8982de0e 374 //cout << "wide"<<endl;
jsobiecki 4:6ebe8982de0e 375 }
jsobiecki 4:6ebe8982de0e 376
jsobiecki 4:6ebe8982de0e 377 else if(size>4 && size<5) //narrow
jsobiecki 4:6ebe8982de0e 378 {
jsobiecki 4:6ebe8982de0e 379 dirC=0.5*(left*10+right*10);
jsobiecki 4:6ebe8982de0e 380 //cout<<"narrow"<<endl;
jsobiecki 4:6ebe8982de0e 381 } else {
jsobiecki 4:6ebe8982de0e 382 int secL = L;
jsobiecki 4:6ebe8982de0e 383 while (temp[secL] != 1) {
jsobiecki 4:6ebe8982de0e 384 secL++;
jsobiecki 4:6ebe8982de0e 385 if (secL > 35) secL = 0;
jsobiecki 4:6ebe8982de0e 386 }
jsobiecki 4:6ebe8982de0e 387 int rightL = secL;
jsobiecki 4:6ebe8982de0e 388 int size = 1;
jsobiecki 4:6ebe8982de0e 389
jsobiecki 4:6ebe8982de0e 390 int i = secL + 1;
jsobiecki 4:6ebe8982de0e 391 if (i > 35) i = 0;
jsobiecki 4:6ebe8982de0e 392 while (temp[i] == 1) {
jsobiecki 4:6ebe8982de0e 393 size++;
jsobiecki 4:6ebe8982de0e 394 i++;
jsobiecki 4:6ebe8982de0e 395 if (i > 35) i = 0;
jsobiecki 4:6ebe8982de0e 396 if (i == secL) break;
jsobiecki 4:6ebe8982de0e 397 // Smax here
jsobiecki 4:6ebe8982de0e 398 if (size >= 5) break;
jsobiecki 4:6ebe8982de0e 399 }
jsobiecki 4:6ebe8982de0e 400 int leftL = i - 1;
jsobiecki 4:6ebe8982de0e 401 if (leftL < 0) leftL = 35;
jsobiecki 4:6ebe8982de0e 402 if (size >= 5) //wide
jsobiecki 4:6ebe8982de0e 403 dirL = rightL * 10 + 0.5 * 10 * 5;
jsobiecki 4:6ebe8982de0e 404 else if(size>4 && size<5) //narrow
jsobiecki 4:6ebe8982de0e 405 dirL = 0.5 * (leftL * 10 + rightL * 10);
jsobiecki 4:6ebe8982de0e 406 else
jsobiecki 4:6ebe8982de0e 407 dirL=9999;
jsobiecki 4:6ebe8982de0e 408 ///////////////////////////////////////////////////////////////////
jsobiecki 4:6ebe8982de0e 409 int secR = R;
jsobiecki 4:6ebe8982de0e 410 while (temp[secR] != 1) {
jsobiecki 4:6ebe8982de0e 411 secR--;
jsobiecki 4:6ebe8982de0e 412 if (secR < 0) secR = 35;
jsobiecki 4:6ebe8982de0e 413 }
jsobiecki 4:6ebe8982de0e 414
jsobiecki 4:6ebe8982de0e 415 int leftR = secR;
jsobiecki 4:6ebe8982de0e 416 int sizeR = 1;
jsobiecki 4:6ebe8982de0e 417
jsobiecki 4:6ebe8982de0e 418 int j = secR - 1;
jsobiecki 4:6ebe8982de0e 419 if (j < 0) j = 35;
jsobiecki 4:6ebe8982de0e 420 while (temp[j] == 1) {
jsobiecki 4:6ebe8982de0e 421 sizeR++;
jsobiecki 4:6ebe8982de0e 422 j--;
jsobiecki 4:6ebe8982de0e 423 if (j < 0) j = 35;
jsobiecki 4:6ebe8982de0e 424 if (j == secR) break;
jsobiecki 4:6ebe8982de0e 425 if (sizeR >= 5) break;
jsobiecki 4:6ebe8982de0e 426 }
jsobiecki 4:6ebe8982de0e 427 int rightR = j + 1;
jsobiecki 4:6ebe8982de0e 428 if (rightR > 35) rightR = 0;
jsobiecki 4:6ebe8982de0e 429 if (sizeR >= 5) //wide
jsobiecki 4:6ebe8982de0e 430 dirR = leftR * 10 + 0.5 * 10 * 5;
jsobiecki 4:6ebe8982de0e 431 else if(sizeR>4 && sizeR<5)//narrow
jsobiecki 4:6ebe8982de0e 432 dirR = 0.5 * (rightR * 10 + leftR * 10);
jsobiecki 4:6ebe8982de0e 433 else
jsobiecki 4:6ebe8982de0e 434 dirR=9999;
jsobiecki 4:6ebe8982de0e 435
jsobiecki 4:6ebe8982de0e 436 if(dirL>360) dirL=fabs(dirL-360);
jsobiecki 4:6ebe8982de0e 437 if(dirR>360) dirR=fabs(dirR-360);
jsobiecki 4:6ebe8982de0e 438 if(fabs(theta_deg-dirL)>fabs(theta_deg-dirR))
jsobiecki 4:6ebe8982de0e 439 dirC=dirR;
jsobiecki 4:6ebe8982de0e 440 else
jsobiecki 4:6ebe8982de0e 441 dirC=dirL;
jsobiecki 4:6ebe8982de0e 442 }
jsobiecki 4:6ebe8982de0e 443 dirSet=dirC;
jsobiecki 4:6ebe8982de0e 444 //cout<<"dirSet: 1"<<endl;
jsobiecki 4:6ebe8982de0e 445
jsobiecki 4:6ebe8982de0e 446 ///////////////////////////////////////////////////////////
jsobiecki 4:6ebe8982de0e 447 } else {
jsobiecki 4:6ebe8982de0e 448 int secL = destSec;
jsobiecki 4:6ebe8982de0e 449 while (temp[secL] != 1) {
jsobiecki 4:6ebe8982de0e 450 secL++;
jsobiecki 4:6ebe8982de0e 451 if (secL > 35) secL = 0;
jsobiecki 4:6ebe8982de0e 452 }
jsobiecki 4:6ebe8982de0e 453 int rightL = secL;
jsobiecki 4:6ebe8982de0e 454 int size = 1;
jsobiecki 4:6ebe8982de0e 455
jsobiecki 4:6ebe8982de0e 456 int i = secL + 1;
jsobiecki 4:6ebe8982de0e 457 if (i > 35) i = 0;
jsobiecki 4:6ebe8982de0e 458 while (temp[i] == 1) {
jsobiecki 4:6ebe8982de0e 459 size++;
jsobiecki 4:6ebe8982de0e 460 i++;
jsobiecki 4:6ebe8982de0e 461 if (i > 35) i = 0;
jsobiecki 4:6ebe8982de0e 462 if (i == secL) break;
jsobiecki 4:6ebe8982de0e 463 // Smax here
jsobiecki 4:6ebe8982de0e 464 if (size >= 5) break;
jsobiecki 4:6ebe8982de0e 465 }
jsobiecki 4:6ebe8982de0e 466 int leftL = i - 1;
jsobiecki 4:6ebe8982de0e 467 if (leftL < 0) leftL = 35;
jsobiecki 4:6ebe8982de0e 468 if (size >= 5) //wide
jsobiecki 4:6ebe8982de0e 469 dirL = rightL * 10 + 0.5 * 10 * 5;
jsobiecki 4:6ebe8982de0e 470 else if(size>4 && size<5) //narrow
jsobiecki 4:6ebe8982de0e 471 dirL = 0.5 * (leftL * 10 + rightL * 10);
jsobiecki 4:6ebe8982de0e 472 else
jsobiecki 4:6ebe8982de0e 473 dirL=9999;
jsobiecki 4:6ebe8982de0e 474 ///////////////////////////////////////////////////////////////////
jsobiecki 4:6ebe8982de0e 475 int secR = destSec;
jsobiecki 4:6ebe8982de0e 476 while (temp[secR] != 1) {
jsobiecki 4:6ebe8982de0e 477 secR--;
jsobiecki 4:6ebe8982de0e 478 if (secR < 0) secR = 35;
jsobiecki 4:6ebe8982de0e 479 }
jsobiecki 4:6ebe8982de0e 480
jsobiecki 4:6ebe8982de0e 481 int leftR = secR;
jsobiecki 4:6ebe8982de0e 482 int sizeR = 1;
jsobiecki 4:6ebe8982de0e 483
jsobiecki 4:6ebe8982de0e 484 int j = secR - 1;
jsobiecki 4:6ebe8982de0e 485 if (j < 0) j = 35;
jsobiecki 4:6ebe8982de0e 486 while (temp[j] == 1) {
jsobiecki 4:6ebe8982de0e 487 sizeR++;
jsobiecki 4:6ebe8982de0e 488 j--;
jsobiecki 4:6ebe8982de0e 489 if (j < 0) j = 35;
jsobiecki 4:6ebe8982de0e 490 if (j == secR) break;
jsobiecki 4:6ebe8982de0e 491 if (sizeR >= 5) break;
jsobiecki 4:6ebe8982de0e 492 }
jsobiecki 4:6ebe8982de0e 493 int rightR = j + 1;
jsobiecki 4:6ebe8982de0e 494 if (rightR > 35) rightR = 0;
jsobiecki 4:6ebe8982de0e 495 if (sizeR >= 5) //wide
jsobiecki 4:6ebe8982de0e 496 dirR = leftR * 10 + 0.5 * 10 * 5;
jsobiecki 4:6ebe8982de0e 497 else if(sizeR>4 && sizeR<5)//narrow
jsobiecki 4:6ebe8982de0e 498 dirR = 0.5 * (rightR * 10 + leftR * 10);
jsobiecki 4:6ebe8982de0e 499 else
jsobiecki 4:6ebe8982de0e 500 dirR=9999;
jsobiecki 4:6ebe8982de0e 501
jsobiecki 4:6ebe8982de0e 502 if(dirL>360) dirL=fabs(dirL-360);
jsobiecki 4:6ebe8982de0e 503 if(dirR>360) dirR=fabs(dirR-360);
jsobiecki 4:6ebe8982de0e 504 if(fabs(theta_deg-dirL)>fabs(theta_deg-dirR))
jsobiecki 4:6ebe8982de0e 505 dirSet=dirR;
jsobiecki 4:6ebe8982de0e 506 else
jsobiecki 4:6ebe8982de0e 507 dirSet=dirL;
jsobiecki 4:6ebe8982de0e 508 //cout<<"dirSet:2 dirR: "<<dirR<<" dirL: "<<dirL<<endl;
jsobiecki 4:6ebe8982de0e 509 }
jsobiecki 4:6ebe8982de0e 510 //cout<<"dirSet: "<<dirSet<<endl;
jsobiecki 4:6ebe8982de0e 511
jsobiecki 4:6ebe8982de0e 512 fX=cos(dirSet*M_PI/180.0f);
jsobiecki 4:6ebe8982de0e 513 fY=sin(dirSet*M_PI/180.0f);
jsobiecki 4:6ebe8982de0e 514
jsobiecki 4:6ebe8982de0e 515 }