SRM / Mbed 2 deprecated LabWork2VFH

sda

Dependencies:   mbed

main.cpp@12:654ad1fe2951, 2019-04-10 (annotated)

Committer:
jsobiecki
Date:
Wed Apr 10 16:58:05 2019 +0000
Revision:
12:654ad1fe2951
Parent:
11:ce9832af1c3b 
working version

Who changed what in which revision?

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