Diff: main.cpp

Revision:

8:262c6c40bff9

Parent:

7:8248af58df5a

Child:

9:326b8f261ef0

diff -r 8248af58df5a -r 262c6c40bff9 main.cpp
--- 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