sda
Dependencies: mbed
Diff: main.cpp
- Revision:
- 12:654ad1fe2951
- Parent:
- 11:ce9832af1c3b
--- a/main.cpp Tue Apr 09 14:27:00 2019 +0000
+++ b/main.cpp Wed Apr 10 16:58:05 2019 +0000
@@ -204,6 +204,13 @@
}
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);
}
@@ -218,8 +225,8 @@
}
}
- smooth[0]=(secVal[28]+2*secVal[29]+2*secVal[0]+2*secVal[1]+secVal[2])/5;
- smooth[1]=(secVal[29]+2*secVal[0]+2*secVal[1]+2*secVal[2]+secVal[3])/5;
+ smooth[0]=(secVal[34]+2*secVal[35]+2*secVal[0]+2*secVal[1]+secVal[2])/5;
+ smooth[1]=(secVal[35]+2*secVal[0]+2*secVal[1]+2*secVal[2]+secVal[3])/5;
smooth[34]=(secVal[32]+2*secVal[33]+2*secVal[34]+2*secVal[35]+secVal[0])/5;
smooth[35]=(secVal[33]+2*secVal[34]+2*secVal[35]+2*secVal[0]+secVal[1])/5;
for (int i = 2; i < 34; ++i) {
@@ -250,17 +257,188 @@
float theta_deg;
theta_deg =(theta*180.0f)/M_PI;
pc.printf("theta (degrees): = %lf\n\n", theta_deg);
- float sectorA, sectorB;
- for (int i = 0; i < 36; ++i) {
- sectorA = 10*i;
- 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]);
- if(temp[i]!=0 && (fabs(sectorA-theta_deg)<fabs(sectorB-theta_deg))) best=i;
+ int destSec = theta_deg / 10;
+ if(destSec<0) destSec=36+destSec;
+ cout<<"destination sector: "<<destSec<<endl;
+
+ int L=destSec;
+ int R=destSec;
+ while(temp[L]==0){
+ L--;
+ if(L<0) L=35;
+ }
+ while(temp[R]==0){
+ R++;
+ if(R>35) R=0;
}
- 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);
+
+ float dirSet, dirC,dirL,dirR;
+ if(temp[destSec]==1){
+ int k=destSec-1;
+ if(k<0) k=35;
+ int size=1;
+ while(temp[k]==1){
+ size++;
+ k--;
+ if(k<0) k=35;
+ if(k==destSec) break;
+ if(size>=5) break;
+ }
+ int right=k+1;
+ if(right<0) right=35;
+ k=destSec+1;
+ if(k>35) k=0;
+ while(temp[k]==1){
+ size++;
+ k++;
+ if(k>35) k=0;
+ if(k==destSec) break;
+ if(size>=5) break;
+ }
+ int left=k-1;
+ if(left>35) left=0;
+ if(size>=5) {
+ //wide
+ dirC=destSec*10;
+ cout << "wide"<<endl;
+ }
+
+ else if(size>4 && size<5) //narrow
+ {
+ dirC=0.5*(left*10+right*10);
+ cout<<"narrow"<<endl;
+ } else {
+ int secL = L;
+ while (temp[secL] != 1) {
+ secL++;
+ if (secL > 35) secL = 0;
+ }
+ int rightL = secL;
+ int size = 1;
+
+ int i = secL + 1;
+ if (i > 35) i = 0;
+ while (temp[i] == 1) {
+ size++;
+ i++;
+ if (i > 35) i = 0;
+ if (i == secL) break;
+ // Smax here
+ if (size >= 5) break;
+ }
+ int leftL = i - 1;
+ if (leftL < 0) leftL = 35;
+ if (size >= 5) //wide
+ dirL = rightL * 10 + 0.5 * 10 * 5;
+ else if(size>4 && size<5) //narrow
+ dirL = 0.5 * (leftL * 10 + rightL * 10);
+ else
+ dirL=9999;
+ ///////////////////////////////////////////////////////////////////
+ int secR = R;
+ while (temp[secR] != 1) {
+ secR--;
+ if (secR < 0) secR = 35;
+ }
+
+ int leftR = secR;
+ int sizeR = 1;
+
+ int j = secR - 1;
+ if (j < 0) j = 35;
+ while (temp[j] == 1) {
+ sizeR++;
+ j--;
+ if (j < 0) j = 35;
+ if (j == secR) break;
+ if (sizeR >= 5) break;
+ }
+ int rightR = j + 1;
+ if (rightR > 35) rightR = 0;
+ if (sizeR >= 5) //wide
+ dirR = leftR * 10 + 0.5 * 10 * 5;
+ else if(sizeR>4 && sizeR<5)//narrow
+ dirR = 0.5 * (rightR * 10 + leftR * 10);
+ else
+ dirR=9999;
+
+ if(dirL>360) dirL=fabs(dirL-360);
+ if(dirR>360) dirR=fabs(dirR-360);
+ if(fabs(theta_deg-dirL)>fabs(theta_deg-dirR))
+ dirC=dirR;
+ else
+ dirC=dirL;
+ }
+ dirSet=dirC;
+ cout<<"dirSet: 1"<<endl;
+
+ ///////////////////////////////////////////////////////////
+ } else {
+ int secL = destSec;
+ while (temp[secL] != 1) {
+ secL++;
+ if (secL > 35) secL = 0;
+ }
+ int rightL = secL;
+ int size = 1;
+
+ int i = secL + 1;
+ if (i > 35) i = 0;
+ while (temp[i] == 1) {
+ size++;
+ i++;
+ if (i > 35) i = 0;
+ if (i == secL) break;
+ // Smax here
+ if (size >= 5) break;
+ }
+ int leftL = i - 1;
+ if (leftL < 0) leftL = 35;
+ if (size >= 5) //wide
+ dirL = rightL * 10 + 0.5 * 10 * 5;
+ else if(size>4 && size<5) //narrow
+ dirL = 0.5 * (leftL * 10 + rightL * 10);
+ else
+ dirL=9999;
+ ///////////////////////////////////////////////////////////////////
+ int secR = destSec;
+ while (temp[secR] != 1) {
+ secR--;
+ if (secR < 0) secR = 35;
+ }
+
+ int leftR = secR;
+ int sizeR = 1;
+
+ int j = secR - 1;
+ if (j < 0) j = 35;
+ while (temp[j] == 1) {
+ sizeR++;
+ j--;
+ if (j < 0) j = 35;
+ if (j == secR) break;
+ if (sizeR >= 5) break;
+ }
+ int rightR = j + 1;
+ if (rightR > 35) rightR = 0;
+ if (sizeR >= 5) //wide
+ dirR = leftR * 10 + 0.5 * 10 * 5;
+ else if(sizeR>4 && sizeR<5)//narrow
+ dirR = 0.5 * (rightR * 10 + leftR * 10);
+ else
+ dirR=9999;
+
+ if(dirL>360) dirL=fabs(dirL-360);
+ if(dirR>360) dirR=fabs(dirR-360);
+ if(fabs(theta_deg-dirL)>fabs(theta_deg-dirR))
+ dirSet=dirR;
+ else
+ dirSet=dirL;
+ cout<<"dirSet:2 dirR: "<<dirR<<" dirL: "<<dirL<<endl;
+ }
+ cout<<"dirSet: "<<dirSet<<endl;
+
+ fX=cos(dirSet*M_PI/180.0f);
+ fY=sin(dirSet*M_PI/180.0f);
}
\ No newline at end of file