Revision 6:858a5116688e, committed 2019-10-23

Comitter:

florine_van

Date:

Wed Oct 23 10:38:36 2019 +0000

Parent:

5:8ef79eebbc97

Child:

7:2cf57f28255d

Commit message:

Correct motors issue; Clean code

Changed in this revision

--- 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();

--- a/motor/Motor.cpp	Tue Oct 22 15:06:44 2019 +0000
+++ b/motor/Motor.cpp	Wed Oct 23 10:38:36 2019 +0000
@@ -1,47 +1,33 @@
+#include "mbed.h"
+
 #include "Motor.h"
 
 ///////////////////////////////////////////////////////////////////
 // Constructor
 ///////////////////////////////////////////////////////////////////
 Motor::Motor(PinName in1, PinName in2, PinName pwm)
-: in1(in1), in2(in2), pwm(pwm) {}
+: in1(in1), in2(in2), pwm(pwm) 
+{
+    this->pwm.period_ms(10);
+}
 
 ///////////////////////////////////////////////////////////////////
 // Public methods
 ///////////////////////////////////////////////////////////////////
 
-//TODO
 void Motor::moveForward()
 {
     in1 = 1; 
-    in2 = 0;
-    
-    pwm.period_ms(10);
-    pwm.write(0.5f);
-}
-
-//TODO
-void Motor::moveBack()
-{
-    pwm.period_ms(10);
-    pwm.write(0.5f);
+    in2 = 0;    
 }
 
-//TODO
-void Motor::turnLeft()
+void Motor::moveBackward()
 {
-    in1 = 0;
+    in1 = 0; 
     in2 = 1;
-    
-    pwm.period_ms(10);
-    pwm.write(0.5f);
 }
 
-//TODO
-void Motor::turnRight()
+void Motor::setSpeed(float speed)
 {
-    
-    pwm.period_ms(10);
-    pwm.write(0.5f);
+    pwm.write(speed);
 }
-

--- a/motor/Motor.h	Tue Oct 22 15:06:44 2019 +0000
+++ b/motor/Motor.h	Wed Oct 23 10:38:36 2019 +0000
@@ -20,21 +20,17 @@
     /** Move back
     *
     */
-    void moveBack();
+    void moveBackward();
     
-    /** Turn Left
+    /** Set the speed
     *
+    *   @param speed value to set
     */
-    void turnLeft();
-    
-    /** Turn right
-    *
-    */
-    void turnRight();
-    
+    void setSpeed(float speed);
+   
 private:
     DigitalOut in1;
     DigitalOut in2;
     PwmOut pwm;    
-    
+  
 };
\ No newline at end of file