Diff: Motors/Motor.cpp

Revision:

10:276cc357015c

Parent:

9:326b8f261ef0

--- a/Motors/Motor.cpp	Sat Jan 04 21:35:25 2020 +0000
+++ b/Motors/Motor.cpp	Sun Jan 05 21:47:14 2020 +0000
@@ -6,12 +6,19 @@
 #include "Buzzer.h"
 #include "Motor.h"
 #include "LED.h"
+#include "TOF.h"
 #include <ros.h>
 #include <Servo.h>
 #include <std_msgs/UInt8.h>
+#include "mbed.h"
 //#include <std_msgs/UInt16.h>
 
+DigitalOut TOF1(PC_8);
+DigitalOut TOF3(PC_11);
+DigitalOut TOF5(PC_12);
+DigitalOut TOF7(PD_2);
 
+cAdafruit_VL6180X VL6180X(TOF1,TOF3,TOF5,TOF7);
 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
 
@@ -22,28 +29,69 @@
 cBuzzer cBuzzer(Buzz);
 cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
 
+uint8_t serviceTOF(uint8_t address){
+        
+        uint8_t range = 0;
+        
+        // poll the VL6180X till new sample ready
+        VL6180X.Poll_Range(address);
+
+        // read range result
+        range = VL6180X.Read_Range(address);
+        
+        // clear the interrupt on VL6180X
+        VL6180X.Clear_Interrupts(address);
+        
+        return range;
+}
+
 int Servo1Pos;
 int Servo2Pos;
 
+
+
 /*--------------------------------------------------------------------------------
 Function name: KeySub
 Input Parameters: std_msgs::UInt8 &KeyPress
 Output Parameters:
 Description:
 ----------------------------------------------------------------------------------*/
+int main(){
+    uint8_t TOFRange[4];
+    
+while(1){
+       TOFRange[0] = serviceTOF(ADDR1);
+        TOFRange[1] = serviceTOF(ADDR2);
+        TOFRange[2] = serviceTOF(ADDR3);
+        TOFRange[3] = serviceTOF(ADDR4);
+                
+        //Send range to pc by serial
+        pc.printf("TOF1:%d TOF3: %d TOF5: %d TOF7: %d\r\n", TOFRange[0], TOFRange[1], TOFRange[2],TOFRange[3]);
+        
+        //Short delay
+        wait(0.1);
 void MotorKeySub(const std_msgs::UInt8 &keyPress)
-{
+{ 
+
+      
+    
     printf("%d", keyPress.data);
-
+    
     // Lowercase chars //
     if (keyPress.data == 119) { // w
         if (keyPress.data  == 88 || 120) { // X
             MotorR.Stop();
             MotorL.Stop();
         }
+          else if(TOFRange[2]<150){
+            MotorR.Stop();
+            MotorL.Stop();
+            }
+       else { 
         MotorR.Forwards(1.0f);
         MotorL.Forwards(1.0f);
         wait(0.05);
+        }
     }
 
     else if (keyPress.data == 97) { // a
@@ -51,9 +99,15 @@
             MotorR.Stop();
             MotorL.Stop();
         }
+        else if (TOFRange[3]<150){
+            MotorR.Stop();
+            MotorL.Stop();
+            }
+        else{    
         MotorR.Forwards(1.0f);
         MotorL.Backwards(1.0f);
         wait(0.05);
+         } 
     }
 
     else if (keyPress.data  == 100) { // d
@@ -61,9 +115,15 @@
             MotorR.Stop();
             MotorL.Stop();
         }
-        MotorR.Backwards(1.0f);
+       else if(TOFRange[1]<150){
+            MotorR.Stop();
+            MotorL.Stop();
+            }
+       else{
+         MotorR.Backwards(1.0f);
         MotorL.Forwards(1.0f);
         wait(0.05);
+        }
     }
 
     else if (keyPress.data  == 115) { // s
@@ -71,9 +131,15 @@
             MotorR.Stop();
             MotorL.Stop();
         }
+       else if(TOFRange[0]<150){
+            MotorR.Stop();
+            MotorL.Stop();
+            }
+        else{
         MotorR.Backwards(1.0f);
         MotorL.Backwards(1.0f);
         wait(0.05);
+        }
     }
 
 
@@ -83,9 +149,15 @@
             MotorR.Stop();
             MotorL.Stop();
         }
-        MotorR.Forwards(1.0f);
+       else if(TOFRange[2]<150){
+            MotorR.Stop();
+            MotorL.Stop();
+            }
+      else{
+          MotorR.Forwards(1.0f);
         MotorL.Forwards(1.0f);
         wait(0.387);
+        }
     }
 
     else if (keyPress.data == 65) { // A
@@ -93,9 +165,15 @@
             MotorR.Stop();
             MotorL.Stop();
         }
+       else if (TOFRange[3]<150){
+            MotorR.Stop();
+            MotorL.Stop();
+            }
+        else{    
         MotorR.Forwards(1.0f);
         MotorL.Backwards(1.0f);
         wait(0.387);
+        }
     }
 
     else if (keyPress.data  == 68) { // D
@@ -103,9 +181,15 @@
             MotorR.Stop();
             MotorL.Stop();
         }
+        else if(TOFRange[1]<150){
+            MotorR.Stop();
+            MotorL.Stop();
+            }
+        else{ 
         MotorR.Backwards(1.0f);
         MotorL.Forwards(1.0f);
         wait(0.387);
+        }
     }
 
     else if (keyPress.data  == 83) { // S
@@ -113,9 +197,15 @@
             MotorR.Stop();
             MotorL.Stop();
         }
+    else if(TOFRange[0]<150){
+            MotorR.Stop();
+            MotorL.Stop();
+            }
+        else{    
         MotorR.Backwards(1.0f);
         MotorL.Backwards(1.0f);
         wait(0.387);
+        }
     }
 
 
@@ -123,7 +213,7 @@
         MotorR.Stop();
         MotorL.Stop();
     }
-}
+} } }
 
 /*--------------------------------------------------------------------------------
 Function name: update_power_levels
@@ -295,6 +385,7 @@
 
 void servoKeySub(const std_msgs::UInt8 &ServoKeyPress)
 {
+    
     printf("%d", ServoKeyPress.data);
 
     if (ServoKeyPress.data == 106) {   // j for Pan Left