Diff: main.cpp

Revision:

5:8ef79eebbc97

Parent:

4:cb50c6fa340b

Child:

6:858a5116688e

--- a/main.cpp	Tue Oct 22 11:52:32 2019 +0000
+++ b/main.cpp	Tue Oct 22 15:06:44 2019 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
+
 #include "Sensor.h"
+#include "Motor.h"
 
 // Set up serial to pc
 Serial pc(SERIAL_TX, SERIAL_RX); 
@@ -10,17 +12,48 @@
 #define addr3   (0x2B)
 #define addr4   (0x2C)
 
-int main() {
+// 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);
+
+// Motors
+Motor motor1(PC_6, PB_15, PB_13);
+Motor motor2(PA_15, PC_7, PB_4);
+
+void CheckObstacle()
+{
+    // BY DEFAULT, WHEN OBSTACLE, TURN TO THE RIGHT
+    
+    // When obstacle ahead
+    if ( (sensor1.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();
+    }
+    // No obstacle
+    else
+    {
+        motor1.moveForward();
+        motor2.moveForward();
+    }
+}
+
+int main() 
+{
     int range1; 
     int range2;
     int range3;
     int range4; 
-    
-    // Create 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);
 
     // load settings onto VL6180X sensors 
     sensor2.init();
@@ -44,8 +77,14 @@
         range3 = sensor3.read();
         range4 = sensor4.read();         
 
-        pc.printf("Range one = %d | range two = %d | range three = %d | range four = %d\r\n",range1, range2, range3, range4);       
+        pc.printf("Range one = %d | range two = %d | range three = %d | range four = %d\r\n",range1, range2, range3, range4); 
+        
+        // TODO : better name for this method ?? 
+        CheckObstacle();
+              
         wait_ms(10); 
     } 
 }
 
+
+