Diff: main.cpp

Revision:

19:dbc5fbad4975

Parent:

15:44ab4626f1ad

Child:

20:6a9062d54eb0

--- a/main.cpp	Mon Mar 27 16:44:37 2017 +0000
+++ b/main.cpp	Mon Apr 03 17:01:13 2017 +0000
@@ -8,8 +8,11 @@
 
 int goToPointWithAngle(float target_x, float target_y, float target_angle);
 
+int randomizeAndMap();
 int updateSonarValues();
+int computeObstacle();
 
+bool map[25][25];
 float alpha; //angle error
 float rho; //distance from target
 float beta;
@@ -25,13 +28,14 @@
 int frontMm;
 int rightMm;
 
+
 //Diameter of a wheel and distance between the 2
 float r=3.25, b=7.2;
 
 int speed=999; // Max speed at beggining of movement
 
 //Target example x,y values
-float target_x=46.8, target_y=78.6, target_angle=1.7;
+float target_x=46.8, target_y=78.6, target_angle=1.57;
 
 //Timeout time;
 int main(){
@@ -41,20 +45,27 @@
 
     measure_always_on();
     wait_ms(50);
+    
+    //Fill map
+    for (int i = 0; i<25; i++) {
+        for (int j = 0; j<25; j++) {
+            map[i][j] = false;        
+        }
+    }
 
     //Resetting coordinates before moving
     theta=0; 
     X=0;
     Y=0;
 
-    alpha = atan2((target_y-Y),(target_x-X))-theta;
+    /*alpha = atan2((target_y-Y),(target_x-X))-theta;
     alpha = atan(sin(alpha)/cos(alpha));
     rho = dist(X, Y, target_x, target_y);
     
-    beta = -alpha-theta+target_angle;
-    //beta = atan(sin(beta)/cos(beta));
-
-    goToPointWithAngle(target_x, target_y, target_angle);
+    beta = -alpha-theta+target_angle;*/
+    for (int i = 0; i<10; i++) {
+        randomizeAndMap();
+    }
 
     //Stop at the end
     leftMotor(1,0);
@@ -76,6 +87,34 @@
     return 0;
 }
 
+int randomizeAndMap() {
+    target_x = (rand()%2500)/10;//for decimal precision
+    target_y = (rand()%2500)/10;
+    target_angle = ((rand()%31416)-15708)/1000;
+    pc.printf("\n\r targ_X=%f", target_x);
+    pc.printf("\n\r targ_Y=%f", target_y);
+    pc.printf("\n\r targ_Angle=%f", target_angle);
+    goToPointWithAngle(target_x, target_y, target_angle);
+    return 0;    
+}
+
+int computeObstacle() {
+    //get the sensor values
+    //compute the probabilistic shit of empty/non-empty in the direction of the sensor below 10 cm
+    //update the map object with true where it's likely to be obstacled
+    int xObstacle, yObstacle;
+    if (leftMm < 10) {
+    
+    } else if (frontMm < 10) {
+    
+    } else if (rightMm < 10) {
+    
+    }
+    
+    map[xObstacle][yObstacle] = true;
+    return 0;
+}
+
 int goToPointWithAngle(float target_x, float target_y, float target_angle) {
      do {
         pc.printf("\n\n\r entered while");
@@ -97,9 +136,7 @@
         alpha = atan(sin(alpha)/cos(alpha));
         rho = dist(X, Y, target_x, target_y);
         d2 = rho;
-        beta = -alpha-theta+target_angle;
-        //beta = atan(sin(beta)/cos(beta));
-        
+        beta = -alpha-theta+target_angle;        
         
         //Computing angle error and distance towards the target value
         rho += dt*(-kRho*cos(alpha)*rho);
@@ -150,5 +187,8 @@
         t.stop();
     } while(d2>1 & !tooClose);
     
+    if (tooClose) {
+        computeObstacle();
+    }
     return 0;
 }
\ No newline at end of file