Diff: ActiveCell.h

Revision:

7:3a755ebe4eaf

Parent:

6:2cd6ae395c0f

Child:

9:699054d8510b

--- a/ActiveCell.h	Fri Apr 05 14:28:09 2019 +0000
+++ b/ActiveCell.h	Fri Apr 05 14:45:04 2019 +0000
@@ -16,8 +16,18 @@
     //robots position
     int x0;
     int y0;
+    ///////////VFH//////////////
+    static const int a=70,b=2;
+    float angle;
+    float amplitude;
+    int sectorK;
+    //resolution - if changing change also secVal in main & calcSectors
+    static const int res=12;
 
     ActiveCell(){
+        angle=0;
+        amplitude=0;
+        sectorK=0;
         cellVal=0;
         forceX=0;
         forceY=0;
@@ -29,6 +39,7 @@
     void calForce() {
         forceX = (repulsiveForce * cellVal / pow(distance, 2)) * (xt - x0) / distance;
         forceY = (repulsiveForce * cellVal / pow(distance, 2)) * (yt - y0) / distance;
+        amplitude = cellVal*cellVal*(a-b*distance);
     }
 //Calculating distance from the robot 
     void calDist(int idX, int idY) {
@@ -47,6 +58,12 @@
             yt = y0 + (idY - 5) * 5;
         }
         distance = sqrt(pow((float)abs(x0 - xt), 2) + pow((float)abs(y0 - yt), 2));
+        
+        //////angle/////////////
+        angle = atan2((float)yt-y0,(float)xt-x0)*180/3.14159265358979323846;
+        sectorK=angle/res;
+        if(sectorK<0)
+            sectorK=30+sectorK;
     }
 };