with the tof code
Dependencies: mbed Servo ros_lib_kinetic
Diff: main.cpp
- Revision:
- 8:262c6c40bff9
- Parent:
- 7:8248af58df5a
- Child:
- 9:326b8f261ef0
--- 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