Diff: main.cpp

Revision:

4:5a892f5ab5a8

Parent:

2:c507076bfd93

Child:

5:1649f59c37de

--- a/main.cpp	Fri Mar 22 15:22:15 2019 +0000
+++ b/main.cpp	Sun Mar 24 01:11:20 2019 +0000
@@ -1,21 +1,33 @@
 #include "mbed.h"
 #include "Robot.h"
 #include "math.h"
+#include "ActiveCell.h"
+#include "HistogramCell.h"
 #define M_PI 3.14159265358979323846
 //EXERCICIO 1
 //Luis Cruz N2011164454
-//Jacek Sobecki 
+//Jacek Sobecki N2018319609
 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
 DigitalIn button(PC_13);
 void poseEst(float p[], float radius, float enc_res, float b);
 void SpeedLim(float w[]);
+void initializeArrays();
+void calcForce();
+void updateActive(double xR, double yR);
 //int ReadSensors();
-void updateHist(int hist[][], int active[][],int x,int y);
+//void updateHist(int hist[][], int active[][],int x,int y);
+const int m = 200, n = 200, activeSize = 11;
+const int hSize = 200;
+const int aSize = 11;
+ActiveCell activeReg[aSize][aSize];
+HistogramCell histogram[hSize][hSize];
+
 int main(){
 
     button.mode(PullUp);
     getCountsAndReset();
     setSpeeds(0, 0);
+    initializeArrays();
     while(button==1);
 
     //w[0] = Omega     | w[1] = X     | w[2] = Y
@@ -24,7 +36,6 @@
     //b = Distance between wheels, enc_res = Encoder Resolution, v = Calculated speed
     //k_v = Speed gain, k_s = Curvature gain, wratio = Angular speed ratio control command
     //Cells dim: 5x5cm |
-    const int = m = 200, n = 200, activeSize = 11;
     int hist[m][n] , active[activeSize][activeSize];
     float  w[3], v, p[3], p_obj[3], theta, theta_error, err, integral = 0.0;
     const float radius = 3.5, b = 13.3, enc_res = 1440, k_v = 7, 
@@ -54,7 +65,7 @@
         p[2] = atan2(sin(p[2]),cos(p[2]));
         theta_error = theta-p[2];
         w[0] = k_s*(theta_error); //direction gain
-        integral += err
+        integral += err;
         v = k_v*err+k_i*integral; //Speed gain
         w[1] = (v-(b/2)*w[0])/radius;
         w[2] = (v+(b/2)*w[0])/radius;
@@ -120,16 +131,52 @@
         }
     }
 }
-void updateHist(int hist[i][j], int active[activeSize][activeSize],int x,int y){
-    int aX=0;
-    for(int i=x-(activeSize/2);i<x+(activeSize/2+1);i++){
-        int aY=0;
-        for(int j=y-(activeSize/2);j<y+(activeSize/2+1);j++){
-            if(i>=0 && j>=0){
-                hist[i][j]=active[aX][aY];
-                aY++;
+
+void initializeArrays() {
+    for (int i = 0; i < hSize; i++) {
+        for (int j = 0; j < hSize; j++) {
+            histogram[i][j].calculate(i, j);
+        }
+    }
+    for (int i = 0; i < aSize; i++) {
+        for (int j = 0; j < aSize; j++) {
+            activeReg[i][j].calDist(i, j);
+        }
+    }
+}
+void calcForce(){
+    for (int i = 0; i < aSize; i++) {
+        for (int j = 0; j < aSize; j++) {
+            activeReg[i][j].calForce();
+        }
+    }
+}
+//every time robot changes position we need to call this function to update active region
+//and calculate forces
+//xR, yR - robots position in coordinates system
+void updateActive(double xR, double yR) {
+    int idXr = 0;
+    int idYr = 0;
+    for (int i = 0; i < hSize; i++) {
+        for (int j = 0; j < hSize; j++) {
+            if (xR > histogram[i][j].x - 2.5 && xR < histogram[i][j].x + 2.5 && yR > histogram[i][j].y - 2.5 &&
+                yR < histogram[i][j].y + 2.5) {
+                idXr = i;
+                idYr = j;
+                break;
             }
         }
-        aX++;
     }
+    int m = idXr - aSize / 2;
+    for (int k = 0; k < aSize; k++) {
+        int n = idYr - aSize / 2;
+        for (int l = 0; l < aSize; l++) {
+            if (m > 0 && n > 0 && m < hSize && n < hSize) {
+                activeReg[k][l].cellVal = histogram[m][n].cellVal;
+            }
+            n++;
+        }
+        m++;
+    }
+    calcForce();
 }
\ No newline at end of file