For coursework of group 3 in SOFT564Z
Dependencies: Motordriver ros_lib_kinetic
Diff: VL6180.cpp
- Revision:
- 11:0b9098ec11c7
- Parent:
- 10:c752a8d76ee2
- Child:
- 12:82b8fe254222
diff -r c752a8d76ee2 -r 0b9098ec11c7 VL6180.cpp --- a/VL6180.cpp Tue Dec 17 16:33:46 2019 +0000 +++ b/VL6180.cpp Thu Dec 19 00:13:38 2019 +0000 @@ -5,11 +5,17 @@ #include "VL6180.hpp" #include <vector> -#define DEBUG_TOF +//#define DEBUG_TOF #ifdef DEBUG_TOF Serial debug_term(SERIAL_TX, SERIAL_RX); #endif + +#define ROS_TOF +int Global_TOF_Ranges[num_VL6180]; +bool TOF_Range_Flag; +Mutex Global_TOF_Range_Mutex; + /*------------------------------------------------------------------------------ // Main Program loop ------------------------------------------------------------------------------*/ @@ -44,6 +50,18 @@ wait_ms(10); } + +#ifdef ROS_TOF + Global_TOF_Range_Mutex.lock(); + for(int IDX = 0; IDX < num_VL6180; IDX++) { + Global_TOF_Ranges[IDX] = Range[IDX]; + } + if(TOF_Range_Flag == false) + { + TOF_Range_Flag = true; + } + Global_TOF_Range_Mutex.unlock(); +#endif #ifdef DEBUG_TOF debug_term.printf("|--------------------------------\n\r"