Diff: main.cpp

Revision:

21:62154d644531

Parent:

20:6a9062d54eb0

Child:

22:ebb37a249b5f

--- a/main.cpp	Mon Apr 03 17:07:37 2017 +0000
+++ b/main.cpp	Fri Apr 07 15:47:51 2017 +0000
@@ -11,6 +11,7 @@
 int randomizeAndMap();
 int updateSonarValues();
 int computeObstacle();
+int printFinalMap();
 
 bool map[25][25];
 float alpha; //angle error
@@ -22,7 +23,7 @@
 float temp;
 float d2;
 
-//bool tooClose = false;
+bool tooClose = false;
 
 int leftMm;
 int frontMm;
@@ -40,16 +41,16 @@
 
 //Timeout time;
 int main(){
-    target_x=200*rand();
+    /*target_x=200*rand();
     target_y=200*rand();
-    target_angle=3.1416*2*rand()-3.1416;
+    target_angle=3.1416*2*rand()-3.1416;*/
     
     i2c1.frequency(100000);
     initRobot(); //Initializing the robot
     pc.baud(9600); // baud for the pc communication
 
-    measure_always_on();
-    wait_ms(50);
+    //measure_always_on();
+    //wait_ms(50);
     
     //Fill map
     for (int i = 0; i<25; i++) {
@@ -60,23 +61,37 @@
 
     //Resetting coordinates before moving
     theta=0; 
-    X=0;
-    Y=0;
+    X=125;
+    Y=125;
 
     /*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;*/
-    for (int i = 0; i<10; i++) {
+    for (int i = 0; i<25; i++) {
         randomizeAndMap();
     }
     //Stop at the end
     leftMotor(1,0);
     rightMotor(1,0);
 
-    pc.printf("\n\r %f -- arrived!", rho);
+    //pc.printf("\n\r %f -- arrived!", rho);
+    printFinalMap();
 }
 
+int printFinalMap() {
+    for (int i = 0; i<25; i++) {
+        for (int j = 0; j<25; j++) {
+            if (map[i][j]) {
+                pc.printf("X ");
+            } else {
+                pc.printf("O ");
+            }        
+        }
+        pc.printf("\n\r");
+    }    
+    return 0;
+}
 //Distance computation function
 float dist(float robot_x, float robot_y, float target_x, float target_y){
     return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
@@ -84,8 +99,11 @@
 
 //Updates sonar values
 int updateSonarValues() {
+    //start_read_left_sensor();
     leftMm = get_distance_left_sensor();
+    //start_read_front_sensor();
     frontMm = get_distance_front_sensor();
+    //start_read_right_sensor();
     rightMm = get_distance_right_sensor();
     return 0;
 }
@@ -93,7 +111,7 @@
 int randomizeAndMap() {
     target_x = (rand()%2500)/10;//for decimal precision
     target_y = (rand()%2500)/10;
-    target_angle = ((rand()%31416)-15708)/1000;
+    target_angle = ((float)(rand()%31416)-15708)/10000.0;
     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);
@@ -107,11 +125,11 @@
     //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;
@@ -132,6 +150,7 @@
         
         updateSonarValues();
         if (leftMm < 100 || frontMm < 100 || rightMm < 100) {
+            tooClose = true;
             break;    
         }