Revision 12:9a47af1cc7d7, committed 2019-04-09

Comitter:

shut

Date:

Tue Apr 09 16:57:53 2019 +0000

Parent:

11:ce9832af1c3b

Commit message:

VALLEYS

Changed in this revision

diff -r ce9832af1c3b -r 9a47af1cc7d7 main.cpp
--- a/main.cpp	Tue Apr 09 14:27:00 2019 +0000
+++ b/main.cpp	Tue Apr 09 16:57:53 2019 +0000
@@ -50,10 +50,10 @@
 // =================================== COORDS ====================================
 // =============================================================================== 
     //Target coordinates
-    p_final[0] = 100, p_final[1] = 20, p_final[2] = 0;
+    p_final[0] = 100, p_final[1] = 70, p_final[2] = 0;
     //p_obj[0] = 20, p_obj[1] = 20, p_obj[2] = 0;
     //Initial coordinates:
-    p[0] = 20, p[1] = 20, p[2] = 0;
+    p[0] = 20, p[1] = 70, p[2] = 0;
 // ===============================================================================
 // =================================== EXECUTION =================================
 // ===============================================================================
@@ -157,9 +157,9 @@
     for (int i = 0; i < hSize; i++) {
         for (int j = 0; j < hSize; j++) {
             histogram[i][j].calculate(i, j);
-            if(((i >= 8 && i <= 12) && (j == 0 || j == 8)) || ((i == 8 || i == 12) && (j >= 0 && j <= 8))) histogram[i][j].cellVal=3;
-            if(((i >= 0 && i <= 3) && (j == 8 || j == 12)) || ((i == 0 || i == 3) && (j >= 8 && j <= 12))) histogram[i][j].cellVal=3;
-            if(((i >= 14 && i <= 20) && (j == 8 || j == 9)) || ((i == 14 || i == 20) && (j >= 8 && j <= 9))) histogram[i][j].cellVal=3;
+            if(((i >= 8 && i <= 12) && (j == 0 || j == 18)) || ((i == 8 || i == 12) && (j >= 0 && j <= 18))) histogram[i][j].cellVal=3;
+            if(((i >= 0 && i <= 3) && (j == 18 || j == 22)) || ((i == 0 || i == 3) && (j >= 18 && j <= 22))) histogram[i][j].cellVal=3;
+            if(((i >= 14 && i <= 20) && (j == 18 || j == 19)) || ((i == 14 || i == 20) && (j >= 18 && j <= 19))) histogram[i][j].cellVal=3;
         }
     }
     for (int i = 0; i < aSize; i++) {
@@ -204,7 +204,12 @@
     }
     activeReg[5][5].amplitude=0;
     activeReg[5][5].amplitude=0;
-
+    for (int j = 10; j >= 0; j--) {
+        for (int i = 0; i < 11; i++) {
+            cout << "[" << activeReg[i][j].cellVal << "]";
+        }
+        cout << endl;
+    }
     calcSectors(theta);
 }
 void calcSectors(float theta){
@@ -228,24 +233,28 @@
     
     const int thresh=200;//100
     int temp[36];
-    int counter = 0, aux = 0;
-    int valley[36];
+    int counter[5], aux = 0;
+    bool val_count = true;
+    int kl[5], kr[5];
     for(int i=0;i<36;++i){
         //pc.printf("|%lf", smooth[i]);
-        if(smooth[i]<thresh){
+        if(smooth[i] < thresh){
             temp[i]=1;
-            //valley[aux][aux] = 
-            counter++;
+            counter[aux]++;
+            val_count = true;
         }
+        /*else if(smooth[i] >= thresh && val_count == true){
+            kr[aux] = i-counter[aux];
+            kl[aux] = i-1;
+            temp[i]=0;
+            aux++;
+        }*/
         else{
-            valley[aux] = counter;
-            counter = 0;
-            aux++;
-            temp[i]=0;
-            //pc.printf("#%d", i);
-        }
-        
+            temp[i] = 0;
+            val_count = false;
+            }
     }
+    
     float best=999;
     float theta_deg;
     theta_deg =(theta*180.0f)/M_PI;
@@ -256,11 +265,34 @@
         sectorB = 10*best;
         if(sectorA > 180) sectorA=-(360-sectorA);
         if(sectorB > 180) sectorB=-(360-sectorB);
-        //pc.printf("sectorA = %lf sectorB = %lf temp[%d] = %d smooth[%d] = %lf secVal[%d] = %lf \n", sectorA, sectorB, i, temp[i], i, smooth[i], i, secVal[i]);
+        pc.printf("sectorA = %lf sectorB = %lf temp[%d] = %d smooth[%d] = %lf secVal[%d] = %lf \n", sectorA, sectorB, i, temp[i], i, smooth[i], i, secVal[i]);
         if(temp[i]!=0 && (fabs(sectorA-theta_deg)<fabs(sectorB-theta_deg))) best=i;
     }
+    int i=best-1;
+    int size=1;
+    while(temp[i]==1){
+        size++;
+        i--;
+        if(i<0) i=35;
+    }
+    int right=i-1;
+    if(right<0) i=35;
+    i=best+1;
+    while(temp[i]==1){
+        size++;
+        i++;
+        if(i>35) i=0;
+    }
+    int left=i+1;
+    if(left>35) left=0;
+    float dir;
+    if(size>5) //wide
+        dir=best*10+0.5*10*5;
+    else  //narrow
+        dir=0.5*(left*10+right*10);
+    
     pc.printf("best = %lf\n\n", best);
-    fX=cos(best*10.0f*M_PI/180.0f);
-    fY=sin(best*10.0f*M_PI/180.0f);
+    fX=cos(dir*M_PI/180.0f);
+    fY=sin(dir*M_PI/180.0f);
     
 }
\ No newline at end of file