Diff: main.cpp

Revision:

6:858a5116688e

Parent:

5:8ef79eebbc97

Child:

7:2cf57f28255d

--- a/main.cpp	Tue Oct 22 15:06:44 2019 +0000
+++ b/main.cpp	Wed Oct 23 10:38:36 2019 +0000
@@ -12,39 +12,57 @@
 #define addr3   (0x2B)
 #define addr4   (0x2C)
 
+#define S1 PC_8
+#define S2 PC_9
+#define S3 PC_10
+#define S4 PC_11
+#define S5 PC_12
+#define S6 PD_2
+#define S7 PG_2
+#define S8 PG_3 
+
 // VL6180x sensors
-Sensor sensor1(I2C_SDA, I2C_SCL, PC_9);
-Sensor sensor2(I2C_SDA, I2C_SCL, PC_11);
-Sensor sensor3(I2C_SDA, I2C_SCL, PD_2);
-Sensor sensor4(I2C_SDA, I2C_SCL, PG_3);
+Sensor sensor_forward(I2C_SDA, I2C_SCL, S1);
+Sensor sensor_right(I2C_SDA, I2C_SCL, S3);
+Sensor sensor_back(I2C_SDA, I2C_SCL, S5);
+Sensor sensor_left(I2C_SDA, I2C_SCL, S7);
 
 // Motors
-Motor motor1(PC_6, PB_15, PB_13);
-Motor motor2(PA_15, PC_7, PB_4);
+Motor motor_left(PC_6, PB_15, PB_13);
+Motor motor_right(PA_15, PC_7, PB_4);
 
 void CheckObstacle()
-{
-    // BY DEFAULT, WHEN OBSTACLE, TURN TO THE RIGHT
-    
+{    
     // When obstacle ahead
-    if ( (sensor1.getIsObstacle()))
+    if (sensor_forward.getIsObstacle())
     {
-        //Turn to the right
-        motor1.turnRight();
-        motor2.turnRight();
-    }
-    // When obstacle ahead and to the right 
-    else if ( (sensor1.getIsObstacle()) && (sensor2.getIsObstacle()) )
-    {
-        //Turn to the left
-        motor1.turnLeft();
-        motor2.turnLeft();
+        if ( (sensor_right.getIsObstacle()) && (sensor_left.getIsObstacle()) )
+        {
+            //Turn backward
+            while(!sensor_back.getIsObstacle())
+            {
+                motor_left.moveForward();
+                motor_right.moveBackward();  
+            }
+        }
+        if (sensor_left.getIsObstacle())
+        {
+            //Turn to the right
+            motor_left.moveForward();
+            motor_right.moveBackward();
+        }
+        else 
+        {
+            // By default : turn to the left
+            motor_left.moveBackward();
+            motor_right.moveForward();      
+        }
     }
     // No obstacle
     else
     {
-        motor1.moveForward();
-        motor2.moveForward();
+        motor_left.moveForward();
+        motor_right.moveForward();
     }
 }
 
@@ -56,28 +74,32 @@
     int range4; 
 
     // load settings onto VL6180X sensors 
-    sensor2.init();
+    sensor_forward.init();
     // change default address of sensor 2
-    sensor2.changeAddress(addr2);
+    sensor_forward.changeAddress(addr2);
     
-    sensor3.init();
+    sensor_right.init();
     // change default address of sensor 3
-    sensor3.changeAddress(addr3);
+    sensor_right.changeAddress(addr3);
 
-    sensor4.init();
+    sensor_back.init();
     // change default address of sensor 4
-    sensor4.changeAddress(addr4);
+    sensor_back.changeAddress(addr4);
+    
+    sensor_left.init();
     
-    sensor1.init();
+    //Set Speeds
+    motor_left.setSpeed(0.5f);
+    motor_right.setSpeed(0.5f);
   
     while (1)
     {            
-        range1 = sensor1.read();
-        range2 = sensor2.read();
-        range3 = sensor3.read();
-        range4 = sensor4.read();         
+        range1 = sensor_forward.read();
+        range2 = sensor_right.read();
+        range3 = sensor_back.read();
+        range4 = sensor_left.read();         
 
-        pc.printf("Range one = %d | range two = %d | range three = %d | range four = %d\r\n",range1, range2, range3, range4); 
+        pc.printf("Range forward = %d | range back = %d | range right = %d | range left = %d\r\n",range1, range3, range2, range4); 
         
         // TODO : better name for this method ?? 
         CheckObstacle();