POST No. 2406779
Adding Sensor to TB3, Tutorial? (part two)
2018-04-26 05:04:21 kurteck


OST said that they already had a plan for the additional sensor tutorial for TurtleBot3 and it'll be completed within a couple weeks with OpenCR firmware update.

Most likely the example will be bumper sensor and PSD sensor for floor detection :D


You may use OLLO pins with 3.3V, GND and ADC input.

Since the sensor output is only high or low depending on the existence of an object, using ADC pin would make the sensor connection simpler.

 @Will Son - Any updates on this?   Would have responded to first thread, but as it is now on page 5, not sure it would be seen?   I am guessing that you would suggest that we should continue this on the RobotSource?   I may also talk about it on Trossen forum...

I am in the next day or two, I will have my TB3 back in one piece with the UP board and R200 camera.  More later about this on RobotSource...

But before I allow the TB3 to wonder in my house, I want to make sure it does not fall down the stairs.    Hopefully using something like the: http://wiki.ros.org/kobuki_safety_controller but adapted to the TB3. 

To start with this I purchased two VL6180 sensors from Adafruit. 

They connect by I2C.  They only have one normal I2C address, so deciding how I might use multiple of them,  I do have an Adafruit I2C Multiplexer that I can use.   They have a range down to 5cm, so probably mounting on 2nd deck probably starting out with both up front, near wheels. 

Some Things I need to do or need to figure out.

a) Mounting.  WIll 3D Print some bracket to mount, should not be difficult

b) What to connect to?  Will see about adding it to OpenCR board.   May turn out to be simpler to add 2nd Arduino board, like a Teensy?  But I believe that there is at least one I2C setup on OpenCR board.  Would be nice to not have to run multiople pyserial setups.

EDIT: looks like wire library is not really implemented yet on OpenCR, to use the hardware I2C.... There is another I2clib... Not sure how well it works.

c) Decide how to use the sensors?  Could poll the sensors and have them generate a ROS topic.  Note this sensor was mentioned in the TB3 docs: https://roscon.ros.org/2016/presentations/ROSCon2016_Turtlebot3_ROBOTIS.pdf

Also looks like there is at least one ROS implementation for this sensor: https://github.com/SV-ROS/tof_range_finder

I believe this creates the rostopic of tof_range_finder

d) But I am wondering if for cliff detection if it would be best to monitor a topic such as this, or I believe the devices can be configured to set the interrupt line when the sensor detects something outside some specific bounds.    Maybe this would be better?

e) Assuming I end up adding these devices to OPenCR - What workflow makes sense to update the Arduino code running on the OpenCR board?   Some options include:

1) Build on main machine (Windows or Linux), then you have to unplug the USB coming from RPI (or UP board), plug the OpenCR into PC to download...  Then unplug OpenCR and plug it back into RPI/UP... Sort of a Pain.

2) Build on RPI/UP - Wonder if you can build for OpenCR on Linux ARM?  UP no problem... Not sure if I would use VNC?  Or wonder if command line run of Arduino works with OpenCR board type? 

3) Build on Host,  use the export compiled Binary command, the use something like FTP/SCP to copy the binary down to RPI and then use SSH to run upload program to update the OpenCR... Will probably go mostly this route.

3a) Wonder about way to automate 3)?  Maybe similar to what was on Edison.  That is maybe setup the build setup to have it automatically copy the binary remote (RCP?), to a known location on your UP/RPI. 

Maybe have some demon on the RPI/UP that detects a file has been updated and automatically run the upload program to reprogram the openCR...

e) Wondering which OpenCR sources to start updating.  I am guessing I will start with the develop branches for OpenCR and Turtlebot3.

Again I am just starting to play with some of this stuff and would appreciate suggestions.




2018-04-26 05:04:21
2018-04-26 11:21:09 Will Son

Hello Kurt,

The sensor implementation for TurtleBot3 has been delayed until mid May and OST is planning to release them on May 18th.

However, examples will be covering some basic sensors such as bumpers and sonars which are based on ADC or Digital Input.

Recently I was playing with OV7725 and 2.8" TFT LCD on OpenCR, and I was able to use GPIO #3 and #4 for I2C2 SCL and SDA respectively.

However, I wasn't working on Arduino IDE, but on Eclipse IDE for developing bootloader and firmware for this project.

Hopefully I can port it as an Arduino example in the near future.

Arduino shield 14 and 15 can be used as I2C1 SDA and SCL, but current I2C is a software emulated, therefore it does not support high speed communication. 

I hope I2C hardware interface will be released soon. 

I think connecting multiple TOF sensors with a multiplexer would work fine, and you can connect them in a single I2C channel to detect the threshold value.

I probably wouldn't try to monitor the sensor values over network all the time as I don't have too much faith in realtime communication over wifi.

For updating OpenCR firmware, I would recommend to use method 3) Build on Host and use compiled binary to update from RPI3 or UP.

You can find shell script that can be executed from ARM processor.



The develop branch for both OpenCR and TurtleBot3 has the recent changes so it'll be a good starting point.

Have a lot of fun for your project and let us know how things are progressing.

Thank you.

2018-04-26 11:21:09
2018-04-26 23:20:03 Kurt

Thanks Will,

As mentioned the Wire library is currently only bit banged... What about the I2CDev library?  Is it using the Hardware I2C?   I noticed it was mentioned in the OpenCR github issues: https://github.com/ROBOTIS-GIT/OpenCR/issues/64

Where it suggested to the other user to maybe look at a couple examples including: https://github.com/ROBOTIS-GIT/OpenCM9.04/wiki/arduino_examples_mpu6050_dmp

Which should be updated to: http://emanual.robotis.com/docs/en/parts/controller/opencm904/#mpu6050-dmp

I downloaded the ZIP file and added it to my Arduino sketch folder and it does build for the OpenCR board... I have not tested any of it yet to see for example if it talks to any of the IO pins...

Update: I tried downloading... So far not seeing anything output to the IO pins... Once I download it, I think it hangs the board... Had to do the firmware recovery (SW2 RESET) buttons in order to upload new program.

Also FYI - Output trying to talk to VL6180X using Wire library... Will look next to see what differences there are from it working with Teensy... Note: I have added 2.2K resistor PU resistor to breadboard for SCL/SDA...


And as you mentioned, I will probably go with the option to use the shell script to install the compiled binary... If however I get more energetic, I may hack up the Arduino setup to see if it would be hard to add an ability to have it do the RCP the binary to the host and then maybe have an updated sketch which looks for new binary....

2018-04-26 23:20:03
2018-04-27 10:04:19 Will Son

Yes, I2CDev use hardware I2C and seems like you already did some tweak on it.

RCP would make our lives much easier. I'll look forward to hear the good news from you.

Thank you for inspiring us with brilliant idea!

2018-04-27 10:04:19
2018-04-27 11:11:11 Kurt

@Will Son,

Thanks again.

I will continue to play.  The interesting thing with the Software Wire code.  The clock timings were not consistent and was running at somewhere near 300khz.   When I run it on a Teensy 3.6, the device runs at 100khz as this is the default speed for wire, unless the code calls off to Wire.setClock().

Sorry if some of this is a little offtrack but then I wondered about the timings of the clock in the software implementation of the clock... So looked in sources and it more or less is simply doing two calls to digitalWrite... And it was only getting about 300khz... So made me wonder if there was something like digitalWriteFast or code like you see on the AVR boards or Teensy, where you might hold onto something like a port handle and mask and use that in your code...

So I wondered how hard it would be to unwind the digitalWrite code, and if it would make a difference in speed...

Well here is my attempt:

GPIO_TypeDef *pin1_port;
uint32_t  pin1_set_mask;
uint32_t  pin1_clear_mask;
uint16_t loop_count;

void setup() {
  // put your setup code here, to run once:
  while (!Serial && millis() < 4000) ;
  Serial.println("Start Test");
  pinMode (0, OUTPUT);
  for(int i=0; i < 5; i++) {
    digitalWrite(0, HIGH);
    digitalWrite(0, LOW);
  pinMode (1, OUTPUT);
  for(int i=0; i < 5; i++) {
    digitalWrite(1, HIGH);
    digitalWrite(1, LOW);

  // Lets get and cache for pin 1...
  pin1_port = g_Pin2PortMapArray[1].GPIOx_Port;
  pin1_set_mask = g_Pin2PortMapArray[1].Pin_abstraction;
  pin1_clear_mask = (uint32_t)g_Pin2PortMapArray[1].Pin_abstraction << 16;


void loop() {
  // put your main code here, to run repeatedly:
  digitalWrite(0, HIGH);
  digitalWrite(0, LOW);
  digitalWrite(0, HIGH);
  digitalWrite(0, LOW);
  digitalWrite(0, HIGH);
  digitalWrite(0, LOW);
  // Lets try the HAL way?
  pin1_port->BSRR = pin1_set_mask;
  pin1_port->BSRR = pin1_clear_mask;
  pin1_port->BSRR = pin1_set_mask;
  pin1_port->BSRR = pin1_clear_mask;
  pin1_port->BSRR = pin1_set_mask;
  pin1_port->BSRR = pin1_clear_mask;

Then I wanted to see, if it worked and if it would make a difference:

Yep!  Top line is the digitalWrites and bottom line is the going direct...

Again Not sure if anyone will make usage of this, but...

Edit: Side Question:

Is there a specified mapping (like 1=1) of the Arduino Pins as shown in:


and the top row pins with names like D67 in the top level hardware section:


Edit2 - Never mind, they are pins 50-67...

2018-04-30 11:25:28 willson
Using software I2C definitely slows down the communication, but writing directly on the register makes GPIO high/low much faster.
I was able to reach up to 108Mhz, but was not stable enough.

You can refer this variant.cpp file for pin mapping.
2018-04-30 11:25:28
2018-04-27 11:11:11
2018-04-29 02:20:09 Kurt

In case anyone is interested, I am making some progress.  I was able to get the bitbang wire code to work with the device. 

So I hacked up a test program, that does the range finding.  I rolled my own version of the library (started from Adafruit), but wanted it to allow Async support.  That is I want to be able to tell it to start doing a range sense and then have the code return.  When the device completes the range operation, it change an IO pin, which I now have connected to pin 8, which luckily is one of only maybe 8 pins with interrupt support.  The interrupt handler sets a flag, which I can detect during the main loop code, where I can then call off to get the results...

Test program appears to be working now.   Next up try to integrate into Turtlebot3 Arduino code...  

Note: I also mentioned some of this progress up on https://community.robotsource.org/t/convert-a-turtlebot3-waffle-pi-to-use-up-board-with-r200-camera/1397/5

Wonder in cases like this better to continue on one topic on RobotSource or have new topics for things like adding sensors, which is not specific to UP board...

Could upload code some place if anyone is interested in seeing WIP...

2018-04-30 11:26:23 willson
Thank you for sharing it!
Now you are ready for playing with R200 :)
2018-04-30 11:26:23
2018-04-29 02:20:09
2018-04-30 07:13:48 Kurt

Maybe just talking to myself ;)

More Progress, actually have the turtlebot3_core sketch actually read from the sensor and currently it is creating a new topic: /tof_range_front of type std_msgs::UInt8

Currently it may not be as well integrated as possible, but it at least appears to be making readings...   If I stay with stand alone message, will probably create it's own custom message, so callers can see the error states as well.

But to create the new topic, I added the define for it in the turtlebot3_core_config.h file, like the other publishers.  Like in my case:

// KurtE - VL6180 device(s)
std_msgs::UInt8 distance_msg;
ros::Publisher tof_dist_pub("tof_range_front", &distance_msg);

Then in Setup needed to add an advertise, like the others:

 // KurtE

I also needed to initialize the sensor:   I can show the source for this if anyone is interested...

Then in the main loop, I do like the other functions and check timer, note: I probably should have been consistent with other ones who had #defines that end with _PERIOD for the frequency per second, but I did instead:

if ((t-tTime[5]) >= (1000 / VL6180_RANGE_FREQUENCY))
    if (!vl_front.rangeActive()) {
      distance_msg.data = vl_front.value();
      uint8_t status = vl_front.rangeStatus();

      if (status != VL6180X_ERROR_NONE) {
        distance_msg.data = 0xff; // Set max value on error. 

      // Start new range
    tTime[5] = t;

So about 50 times per second it will see if the previous range find ended and if so get the results and publish them and then start up the next read...

Now to Update the code on the OpenCR board.  I could unplug the board from the UP board, plug into main machine and download, and unplug from PC plug up back in... Which is not a lot of fun, so decided to build on pc, transfer file to UP board (WinSCP) and use the opencr_update scripts to program the board.   I ran into some issues with this:

a) I tried to build the sketch on PC and keep the binary by using the sketch->Export compiled binary command - But this failed as the stuff is missing from protocol.txt.

Opened new github issue: https://github.com/ROBOTIS-GIT/OpenCR/issues/86

b) Once I had that fixed I downloaded the binary file to UP board and tried the update script.  But looks like the opencr_ld_shell_x86 does not appear to like the files built by arduino.  I resolved this by copying down the opencr_ld program from the Arduino install and got it to work...

 ./opencr_ld /dev/ttyACM0 115200 turtlebot3_core.ino.OpenCR.bin 1

I was then able to start up ROS.  Note: I have my UP board as the main ROS node, so on the UP, in one windows I type:


Then in a second KiTTy(Putty) window I launch the core:

roslaunch turtlebot3_bringup turtlebot3_core.launch

 The roslaunch window showed the different topics that were subscribed or advertised.

So I went to a third KiTTy window and typed:

kurt@kurt-UP-Turtle:~$ rostopic list

 I could then watch the the output

kurt@kurt-UP-Turtle:~$ rostopic echo /tof_range_front
data: 49
data: 49
data: 47
data: 52
data: 45
data: 51
data: 50
data: 50
data: 44
data: 49
data: 47
data: 50
data: 47
data: 46
data: 47
data: 53

 So it looks like it might be working :D...  However I will now probably change everything and not necessarily add this new topic., but instead may simply add this into the support for

SensorState message... That is if you look at:

kurt@kurt-UP-Turtle:~$ rosmsg list | grep -i cliff
kurt@kurt-UP-Turtle:~$ rosmsg list | grep -i cliff^C
kurt@kurt-UP-Turtle:~$ rosmsg info turtlebot3_msgs/SensorState
uint8 CLIFF_LEFT=4
uint8 BUTTON0=1
uint8 BUTTON1=2
uint8 BUTTON2=4
uint8 TORQUE_ON=1
uint8 TORQUE_OFF=2
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint8 bumper
uint8 cliff
uint8 button
bool torque
int32 left_encoder
int32 right_encoder
float32 battery

There is already a field for cliff, which in header files has three bits defined for left, center, and right cliff, which is probably what I am wanting... Not sure if it still makes sense to export the raw data as well...

Anyway probably enough for this update

2018-04-30 11:31:04 willson
You've made huge progress during last weekend!
I've enjoyed more than half of my morning time reading your progress :D

Yes, there are some items not used in SensorState topic that can be used for your sensor.
Even I was forgetting the existence of those CLIFF bits, but glad that you've found them!

Kei and I mentioned about creating binary file on the Github issue page, so please check it out.
2018-04-30 11:31:04
2018-04-30 07:13:48
2018-04-30 22:41:59 Kurt

Thanks @Will son -  Thought I would comment on all three comments in one...

Software I2C (Wire library) - As you mentioned, it can be sped up talking directly to the Set/Clear register of the GPIO pin.  As for this case, these devices say they run at 400khz so the current stuff was not that far off of it.   Would be nice to go to Hardware implementation, so at some point may look at other library, but hopefully soon you will have new version of Wire library that uses the hardware?

The other thing would be to see if it can be made to lessen the impact on the rest of the ROS code.  That is, with the hardware or software library, most of the I2C communications is Synchronous. so either way the code is waiting for the I2C communications to complete, before returning... Which is why for example I have the sensor code library just set a flag when it receives the interrupt that Range operation completed and then have the main line code check the flag and then get the results... I do this as I would not want to be holding up interrupts for the duration of time it took to do the Wire communications.  I also don't know enough about the interrupt system on these boards.  Do they have Priorities that can be set?  Can higher priority interrupts be processed while in a lower priority?...   LIkewise, I don't know if there are ways to do the Wire communications Aynch?  DMA? ...   But for the most part just doing this as an exercise to learn more about system.

Variant.h/.cpp - Thanks - Actually this is the first place I looked for information on the pins.  Which is nice that it is also in the e-manual:

I am still interested in maybe implementing a digitalWriteFast, very similar to what Teensy does:.  But basically it checks to see if you passed in a constant value for PIN and then goes direct to the registers... It starts off looking like:

static inline void digitalWriteFast(uint8_t pin, uint8_t val) __attribute__((always_inline, unused));
static inline void digitalWriteFast(uint8_t pin, uint8_t val)
	if (__builtin_constant_p(pin)) {
		if (val) {
			if (pin == 0) {
			} else if (pin == 1) {
			} else if (pin == 2) {
			} else if (pin == 3) {
			} else if (pin == 4) {

EDIT:  I updated my test program I used earlier to see about the concept of digitalWriteFast.  I added a header file with it and tried it out... Worked great.   I opened up an issue: https://github.com/ROBOTIS-GIT/OpenCR/issues/88

With a zip file that has the test program including the header file with the ditialwriteFast as well as screenshot showing that same speed as hard coded registers.


There is another section for else for val and if the pin is not a built in constant, it simply calls to digitalWrite.  So the code is setup the compiler optimizer will remove everything that will never be called.  So if val is a built in constant like HIGH/LOW, then it gets reduced down to just a set or clear operation...


Yes soon will be putting Turtle back in one piece - I ordered another couple of the sensors, so need to make quick and dirty mounts for them and then wire them up with Multiplexer.  Alternatively I could create multiple wire objects using GPIO pins as nothing really that special about bitbanging on those specific pins.

Then will find out if R200 sensor is too high and interfere with lidar.  If so can turn bracket over and mount below top deck.


Thanks - Yes it was a reasonably productive few days.  Need to get back and play more with the standard stuff and work my way through the book and e-manual to understand more with ROS, but find doing little projects like this help me get a more complete understanding...

As mentioned on github, there is another step, once you transfer the binary file to the SBC, you run the opencr ld shell to create a version of the binary file with the appropriate tags.  Something like:

opencr_ld_shell_x86 make waffle_turlebot3_core.ino.bin waffle_custum.opencr 1.0.0

After it completes you can then run the update.sh to update the binary.

Hopefully I am not taking up too much of your time ;) :lol:

2018-04-30 22:41:59
웹에디터 시작 웹 에디터 끝