Revision 8:262c6c40bff9, committed 2019-12-10

Comitter:

Stumi

Date:

Tue Dec 10 11:52:53 2019 +0000

Parent:

7:8248af58df5a

Child:

9:326b8f261ef0

Commit message:

Communicates keyboard inputs to the robot using rosnodes;

Changed in this revision

--- a/Motors/Motor.cpp	Tue Nov 19 12:55:44 2019 +0000
+++ b/Motors/Motor.cpp	Tue Dec 10 11:52:53 2019 +0000
@@ -5,6 +5,8 @@
 #include "Buzzer.h"
 #include "Motor.h"
 #include "LED.h"
+#include <ros.h>
+#include <std_msgs/UInt8.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
@@ -14,155 +16,195 @@
 cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
 
 
-/*--------------------------------------------------------------------------------
-Function name: update_power_levels
-Input Parameters: vBatt
-Output Parameters: N/A
-Description: Function used to send the battery level over to the LED class locally from global source files
-----------------------------------------------------------------------------------*/
-void update_power_levels(float vBatt)
+void keySub(const std_msgs::UInt8 &keyPress)
 {
-    cRGB_LED.record_power(vBatt);
+    printf("%d", keyPress.data);
+    
+    if (keyPress.data == 8) 
+    {
+        MotorR.Forwards(1.0f);
+        MotorL.Forwards(1.0f);
+        wait(0.5);
+    }
+    
+    else if (keyPress.data == 4) 
+    {
+        MotorR.Forwards(1.0f);
+        MotorL.Backwards(1.0f);
+        wait(0.5);
+    } 
+    
+    else if (keyPress.data  == 6) 
+    {
+        MotorR.Backwards(1.0f);
+        MotorL.Forwards(1.0f);
+        wait(0.5);
+    } 
+    
+    else if (keyPress.data  == 2) 
+    {
+        MotorR.Backwards(1.0f);
+        MotorL.Backwards(1.0f);
+        wait(0.5);
+    } 
+    
+    else if (keyPress.data  == 5) 
+    {
+        MotorR.Stop();
+        MotorL.Stop();
+        wait(0.5);
+    } 
+    
+    else 
+    {
+        MotorR.Stop();
+        MotorL.Stop(); 
+    }
 }
 
-/*--------------------------------------------------------------------------------
-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) {
+    /*--------------------------------------------------------------------------------
+    Function name: update_power_levels
+    Input Parameters: vBatt
+    Output Parameters: N/A
+    Description: Function used to send the battery level over to the LED class locally from global source files
+    ----------------------------------------------------------------------------------*/
+    void update_power_levels(float vBatt) {
+        cRGB_LED.record_power(vBatt);
+    }
+
+    /*--------------------------------------------------------------------------------
+    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[2]>150) {   //Provided its 15cm away...
+                cBuzzer.Beep();
+                cRGB_LED.blue_led();
+                wait(0.02);
+                cRGB_LED.display_power();
+                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) {
             cBuzzer.Beep();
             cRGB_LED.blue_led();
             wait(0.02);
             cRGB_LED.display_power();
-            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);
+            if(TOFRange[1]<200) {
+                MotorR.Forwards(1.0f);
+                MotorL.Forwards(1.0f);
+            } else {
 
-            } else {
-                MotorR.Forwards(0.8f);
-                MotorL.Forwards(0.2f);
+                MotorR.Forwards(0.9f);
+                MotorL.Forwards(0.1f);
             }
 
-        } else {
-
-            if(TOFRange[3]<200) {
-                MotorR.Backwards(0.1f);
-                MotorL.Backwards(0.9f);
+        } else if(TOFRange[1]<200) {
+            cBuzzer.Beep();
+            cRGB_LED.blue_led();
+            wait(0.02);
+            cRGB_LED.display_power();
+            MotorR.Forwards(0.1f);
+            MotorL.Forwards(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[0]<200) {
+            cBuzzer.Beep();
+            cRGB_LED.blue_led();
+            wait(0.02);
+            cRGB_LED.display_power();
+            MotorR.Forwards(1.0f);
+            MotorL.Forwards(1.0f);
         }
 
-    } else if(TOFRange[3]<200) {
-        cBuzzer.Beep();
-        cRGB_LED.blue_led();
-        wait(0.02);
-        cRGB_LED.display_power();
-
-        if(TOFRange[1]<200) {
+        else {
             MotorR.Forwards(1.0f);
             MotorL.Forwards(1.0f);
-        } else {
-
-            MotorR.Forwards(0.9f);
-            MotorL.Forwards(0.1f);
         }
-
-    } else if(TOFRange[1]<200) {
-        cBuzzer.Beep();
-        cRGB_LED.blue_led();
-        wait(0.02);
-        cRGB_LED.display_power();
-        MotorR.Forwards(0.1f);
-        MotorL.Forwards(0.9f);
-
-    } else if(TOFRange[0]<200) {
-        cBuzzer.Beep();
-        cRGB_LED.blue_led();
-        wait(0.02);
-        cRGB_LED.display_power();
-        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) {
 
-/*--------------------------------------------------------------------------------
-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)
-{
+        // Set initial condition of PWM
+        _pwm.period(0.001); //1KHz
+        _pwm = 0;
 
-    // Set initial condition of PWM
-    _pwm.period(0.001); //1KHz
-    _pwm = 0;
-
-    // Initial condition of output enables
-    _fwd = 0;
-    _rev = 0;
-}
+        // Initial condition of output enables
+        _fwd = 0;
+        _rev = 0;
+    }
 
-/*--------------------------------------------------------------------------------
-Function name: Forwards
-Input Parameters: float speed - PWM duty between 0-1
-Output Parameters: N/A
-Description: Drives the motor forwards at a designated speed
-----------------------------------------------------------------------------------*/
-void cMotor::Forwards(float speed)
-{
-    _fwd = 1;
-    _rev = 0;
-    _pwm = speed;
-}
+    /*--------------------------------------------------------------------------------
+    Function name: Forwards
+    Input Parameters: float speed - PWM duty between 0-1
+    Output Parameters: N/A
+    Description: Drives the motor forwards at a designated speed
+    ----------------------------------------------------------------------------------*/
+    void cMotor::Forwards(float speed) {
+        _fwd = 1;
+        _rev = 0;
+        _pwm = speed;
+    }
 
-/*--------------------------------------------------------------------------------
-Function name: Backwards
-Input Parameters: float speed - PWM duty between 0-1
-Output Parameters: N/A
-Description: Drives the motor backwards at a designated speed
-----------------------------------------------------------------------------------*/
-void cMotor::Backwards(float speed)
-{
-    _fwd = 0;
-    _rev = 1;
-    _pwm = speed;
-}
+    /*--------------------------------------------------------------------------------
+    Function name: Backwards
+    Input Parameters: float speed - PWM duty between 0-1
+    Output Parameters: N/A
+    Description: Drives the motor backwards at a designated speed
+    ----------------------------------------------------------------------------------*/
+    void cMotor::Backwards(float speed) {
+        _fwd = 0;
+        _rev = 1;
+        _pwm = speed;
+    }
 
-/*--------------------------------------------------------------------------------
-Function name: Stop
-Input Parameters: N/A
-Output Parameters: N/A
-Description: Drives the motor backwards at a designated speed
-----------------------------------------------------------------------------------*/
-void cMotor::Stop()
-{
-    _fwd = 0;
-    _rev = 0;
-    _pwm = 0;
-}
\ No newline at end of file
+    /*--------------------------------------------------------------------------------
+    Function name: Stop
+    Input Parameters: N/A
+    Output Parameters: N/A
+    Description: Drives the motor backwards at a designated speed
+    ----------------------------------------------------------------------------------*/
+    void cMotor::Stop() {
+        _fwd = 0;
+        _rev = 0;
+        _pwm = 0;
+    }
\ No newline at end of file

--- a/Motors/Motor.h	Tue Nov 19 12:55:44 2019 +0000
+++ b/Motors/Motor.h	Tue Dec 10 11:52:53 2019 +0000
@@ -7,7 +7,8 @@
 
 #include "mbed.h"
 #include "Buzzer.h"
-
+#include <ros.h>
+#include <std_msgs/UInt8.h>
 
 /*--------------------------------------------------------------------------------
 ---------------------------------DEFINES------------------------------------------
@@ -52,6 +53,7 @@
 --------------------------------------------------------------------------------*/
 void Check_for_obstacles(uint8_t TOFRange[4]); //Obstacle avoidance functionality
 void update_power_levels(float vBatt);  //Sends power level to the LED class for further processing
+void keySub(const std_msgs::UInt8 &keyPress);
 /*--------------------------------------------------------------------------------
 --------------------------------External Variables--------------------------------
 --------------------------------------------------------------------------------*/

--- a/main.cpp	Tue Nov 19 12:55:44 2019 +0000
+++ b/main.cpp	Tue Dec 10 11:52:53 2019 +0000
@@ -8,11 +8,23 @@
 #include "power.h"
 #include "Buzzer.h"
 #include "LED.h"
+#include <ros.h>
+#include <std_msgs/UInt8.h>
+
+class mySTM32 : public MbedHardware
+{
+public:
+    mySTM32(): MbedHardware(PD_5, PD_6, 57600) {};
+};
+
+ros::NodeHandle_<mySTM32> nh;
+
+ros::Subscriber<std_msgs::UInt8> sub("keyControl", &keySub);
 
 DigitalIn user_button(USER_BUTTON);
 float power_levels[3]; //Array to voltage levels
 
-Serial pc(SERIAL_TX, SERIAL_RX, 9600);    // set-up serial to pc
+Serial pc(SERIAL_TX, SERIAL_RX, 9600);    //set-up serial to pc
 
 Ticker power_monitor;
 
@@ -47,15 +59,21 @@
 ----------------------------------------------------------------------------------*/
 int main()
 {
+    nh.initNode();
+    nh.subscribe(sub);
+    
     power_monitor.attach(power_check, 30.0); //Monitor battery every 30 seconds
+    
     uint8_t TOFRange[4]; //Array to store TOF measurements 0-sensor1, 1-sensor4, 2-sensor6, 3-sensor7
     
     //Wait for user button to be pressed
     pc.printf("Press user button to start\n\r");
+    
     while(user_button != 1) {}
     
     while(1) {
-
+        
+        /*
         //Perform TOF measurements
         TOFRange[0] = serviceTOF(VL6180X, ADDR1);
         TOFRange[1] = serviceTOF(VL6180X, ADDR4);
@@ -63,5 +81,9 @@
         TOFRange[3] = serviceTOF(VL6180X, ADDR7);
 
         Check_for_obstacles(TOFRange); //Run obstacle avoidance
+        */
+        pc.printf("Spinning...");
+        nh.spinOnce();
+        wait_ms(1);
     }
 }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_kinetic.lib	Tue Dec 10 11:52:53 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f