Revision 10:276cc357015c, committed 2020-01-05

Comitter:

hongyunAHN

Date:

Sun Jan 05 21:47:14 2020 +0000

Parent:

9:326b8f261ef0

Commit message:

a

Changed in this revision

--- 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

--- a/TOFs/TOF.cpp	Sat Jan 04 21:35:25 2020 +0000
+++ b/TOFs/TOF.cpp	Sun Jan 05 21:47:14 2020 +0000
@@ -9,28 +9,6 @@
 I2C i2c(I2C_SDA, I2C_SCL); // Set up I²C on the STM board
 
 /*--------------------------------------------------------------------------------
-Function name: ServiceTOF
-Input Parameters: address - address of target TOF sensor
-Output Parameters: range - distance measurement in mm
-Description: performs measurement routine on a given sensor
-----------------------------------------------------------------------------------*/
-uint8_t serviceTOF(cAdafruit_VL6180X VL6180X, 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;
-}
-
-/*--------------------------------------------------------------------------------
 Function name: cAdafruit_VL6180X
 Input Parameters: N/A
 Output Parameters: N/A
@@ -57,23 +35,23 @@
     //Enable and configure sensor 2
     sensor2 = 1;
     wait(0.01);
-    Setup(DEFAULT_ADDR, ADDR4); //Change address
-    Init(ADDR4);        //Perform standard initialisation routine
-    Start_Range(ADDR4); //Begin sampling in continuous mode
+    Setup(DEFAULT_ADDR, ADDR2); //Change address
+    Init(ADDR2);        //Perform standard initialisation routine
+    Start_Range(ADDR2); //Begin sampling in continuous mode
     
     //Enable and configure sensor 3
     sensor3 = 1;
     wait(0.01);
-    Setup(DEFAULT_ADDR, ADDR6); //Change address
-    Init(ADDR6);        //Perform standard initialisation routine
-    Start_Range(ADDR6); //Begin sampling in continuous mode
+    Setup(DEFAULT_ADDR, ADDR3); //Change address
+    Init(ADDR3);        //Perform standard initialisation routine
+    Start_Range(ADDR3); //Begin sampling in continuous mode
     
     //Enable and configure sensor 4
     sensor4 = 1;
     wait(0.01);
-    Setup(DEFAULT_ADDR, ADDR7); //Change address
-    Init(ADDR7);        //Perform standard initialisation routine
-    Start_Range(ADDR7); //Begin sampling in continuous mode
+    Setup(DEFAULT_ADDR, ADDR4); //Change address
+    Init(ADDR4);        //Perform standard initialisation routine
+    Start_Range(ADDR4); //Begin sampling in continuous mode
     
     printf("INITIALISED\n\r");
 }