Diff: Motors/Motor.cpp

Revision:

4:36a04230554d

Parent:

3:df6160e2f6d9

Child:

6:2cc2aac35868

--- a/Motors/Motor.cpp	Thu Oct 24 14:37:51 2019 +0000
+++ b/Motors/Motor.cpp	Thu Nov 07 15:31:52 2019 +0000
@@ -5,14 +5,88 @@
 
 #include "Motor.h"
 
+cMotor MotorL(M1_PWM, M1_IN1, M1_IN2);  //Left motor class and pin initialisation
+cMotor MotorR(M2_PWM, M2_IN1, M2_IN2);  //Right motor class and pin initialisation
+
+/*--------------------------------------------------------------------------------
+Function name: Check_for_obstacles
+Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements
+Output Parameters: N/A
+Description: Simple obstacle avoidance functionality
+----------------------------------------------------------------------------------*/
+void Check_for_obstacles(uint8_t TOFRange[4])
+{
+    //If top right sensor(6) detects something
+    if (TOFRange[2]<200) {
+        
+        if(TOFRange[2]>150) {   //Provided its 15cm away...
+            
+            if(TOFRange[3]<200) {   //...and the back sensor detects something
+                //Smooth turn right
+                MotorR.Forwards(0.8f);
+                MotorL.Forwards(0.2f);
+            
+            } else if(TOFRange[1]<200) { //...and the top left sensor detects something
+                //Smooth turn left
+                MotorR.Forwards(0.2f);
+                MotorL.Forwards(0.8f);
+                
+            } else {
+                MotorR.Forwards(0.8f);
+                MotorL.Forwards(0.2f);
+            }
+            
+        } else {
+
+            if(TOFRange[3]<200) {
+                MotorR.Backwards(0.1f);
+                MotorL.Backwards(0.9f);
+                
+            } else if(TOFRange[1]<200) {
+                MotorR.Backwards(0.9f);
+                MotorL.Backwards(0.1f);
+                
+            } else {
+                MotorR.Backwards(0.1f);
+                MotorL.Backwards(0.9f);
+            }
+        }
+        
+    } else if(TOFRange[3]<200) {
+        
+        if(TOFRange[1]<200) {
+            MotorR.Forwards(1.0f);
+            MotorL.Forwards(1.0f);
+        } else {
+            
+            MotorR.Forwards(0.9f);
+            MotorL.Forwards(0.1f);
+        }
+        
+    } else if(TOFRange[1]<200) {
+        MotorR.Forwards(0.1f);
+        MotorL.Forwards(0.9f);
+        
+    } else if(TOFRange[0]<200) {
+        MotorR.Forwards(1.0f);
+        MotorL.Forwards(1.0f);
+    }
+
+    else {
+        MotorR.Forwards(1.0f);
+        MotorL.Forwards(1.0f);
+    }
+}
+
 /*--------------------------------------------------------------------------------
 Function name: cMotor
 Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2
 Output Parameters: N/A
 Description: Class constructor (Initialisation upon creating class)
 ----------------------------------------------------------------------------------*/
-cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev){
-    
+cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev)
+{
+
     // Set initial condition of PWM
     _pwm.period(0.001); //1KHz
     _pwm = 0;
@@ -28,7 +102,8 @@
 Output Parameters: N/A
 Description: Drives the motor forwards at a designated speed
 ----------------------------------------------------------------------------------*/
-void cMotor::Forwards(float speed){
+void cMotor::Forwards(float speed)
+{
     _fwd = 1;
     _rev = 0;
     _pwm = speed;
@@ -40,7 +115,8 @@
 Output Parameters: N/A
 Description: Drives the motor backwards at a designated speed
 ----------------------------------------------------------------------------------*/
-void cMotor::Backwards(float speed){
+void cMotor::Backwards(float speed)
+{
     _fwd = 0;
     _rev = 1;
     _pwm = speed;
@@ -52,7 +128,8 @@
 Output Parameters: N/A
 Description: Drives the motor backwards at a designated speed
 ----------------------------------------------------------------------------------*/
-void cMotor::Stop(){
+void cMotor::Stop()
+{
     _fwd = 0;
     _rev = 0;
     _pwm = 0;