Tag: micro-ROS

Handling multiple sensors on an Arduino-like processor, specifically a Teensy 4.1 in my case, can be a bit tricky if you want to get high loop rates.

In my robot, I’m using the Teensy to send data from 8 time-of-flight sensors, 4 SONAR sensors, 2 motor current sensors and 2 motor temperature sensors while also controlling a RoboClaw motor controller, an 8-channel relay board and a touch screen display. On top of this, it runs micro-ROS to send and receive data messages to the rest of the robot.

In this article I will talk only about the issue of using the HC-SR04, inexpensive SONAR sensors. Here is one article that gives you some background in what the sensor is and how it works.

For a robot that uses the ROS software stack, getting data at fast frame rates is crucial, otherwise the robot will have moved far along from where it was when the sensor data was gathered, so the data no longer represents the truth from the point where the robot is now.

It can take on order of 20 milliseconds to get a reading from the SONAR at the maximum distance. If you had a single sensor and read it once per loop, you could, in theory, send about 50 readings a second to your robot. With 4 sensors, however, if you sensed one SONAR after the other in a round-robin manner, the frame rate would drop down to about 50/4 or 12.5 frames per second, which may be too low a rate for your robot.

If your robot is moving at the rate of a slow walk for a person, about 1 mile per hour, that translates to 0.45 meters per second. At a frame rate of 12.5 frames per second, the robot will move 36 millimeters or about 1.4 inches between readings. If you are traveling along a narrow hallway or in a crowded room, that may not be good enough to avoid crashing into nearby obstacles. If you can get back up to nearly 50 frames per second, though, the robot will only travel about 9 millimeters or just over a third of an inch. Add in the other things your Arduino processor is doing besides reading SONAR sensors and frame rates could get much worse.

To get around this problem, you need to use interrupt handlers to control the SONAR sensors. Briefly, what I do is:

• Use a timer to drive a state machine. The timer generates an interrupt every 20 microseconds.
• The timer handler has a counter. Every 500 counts, or 500 * 20 microseconds which is 0.01 seconds, the next SONAR in the set of 4 begins a reading. I used this fixed spacing to solve the issue of multiple SONARs interfering with each other. Whenever a SONAR begins sending its ping sound, it’s possible for all the other SONAR sensors to receive the echo. I want to make it pretty likely that a SONAR hears only its own echo. Sound travels at about 340 meters per second. In 0.01 seconds, a SONAR ping can travel about 3.4 meters. Since SONAR measures round-trip time delay, the SONAR can detect objects up to about 1.7 meters in that 0.01 seconds. Practically, the HC-SR04 sensors are not so accurate or sensitive after about that distance, anyway.
• Once a SONAR starts transmitting its ping, 20 microseconds later the transmitter is turned off. Then the next SONAR in the set of 4 will get the on/off sequence after 0.01 seconds.
• When the transmitter stops for a SONAR, it begins listening for the echo. The echo is processed via an interrupt handler. The interrupt handler computes the time it took to hear the echo, uses the speed of sound to convert echo time to the distance to the obstacle. Then a ROS message is sent for the sensor reading.

On a Teensy4.1, my code is publishing about 25 frames per second for each of the 4 SONAR sensors, or 100 frames per second alltogether. The sensors are spaced out in time, the readings don’t come altogether at once, as described above. Also, the whole code is driven by my TModule code module, which I’ll describe in another article, but you can take this code by itself and combine the two loop and setup methods together and run it without my TModule module.

Here is tsonar.h

#pragma once

#include <Wire.h>
#include <stdint.h>

#include "tmodule.h"

class TSonar : TModule {
 public:
  // Which SONAR sensor.
  typedef enum Sonar {
    kFront,
    kRight,
    kBack,
    kLeft,
    kNumberSonars  // Number of SONAR sensors.
  } Sonar;

  // Get sensed range for the device. A value of < 0 => no sensor 'device'
  // detected during setup.
  int GetValueMm(Sonar device);

  // Get the average range for the device.
  float GetAverageValueM(Sonar device);

  // Singleton constructor.
  static TSonar& singleton();

 protected:
  // From TModule.‰
  void loop();

  // From TModule.‰
  const char* name() { return "Sonr"; }

  // From TModule.‰
  void setup();

 private:
  // Number of readings to average together to reduce noise.
  static const uint8_t kNumberReadingsToAverage = 4;

  // Private constructor.
  TSonar();

  // Common interrupt handler.
  static void CommonInterruptHandler(uint8_t pin, long& end_time,
                                     long& start_time, uint8_t& average_index,
                                     size_t sonar_index);

  // Interrupt handler for per device echo.
  static void Echo0InterruptHandler();
  static void Echo1InterruptHandler();
  static void Echo2InterruptHandler();
  static void Echo3InterruptHandler();
  static void TimerInterruptHandler();

  // Next device being handled in the round-robin processing.
  static uint8_t g_next_sensor_index_;

  // Singleton instance.
  static TSonar* g_singleton_;

  // Last captured sensed distance for each SONAR device.
  static int g_values_mm_[kNumberSonars];

  // List of last values, for computing an average.
  static int g_values_mm_history_[kNumberSonars][kNumberReadingsToAverage];

  // Last captured average distance for each SONAR device.
  static float g_average_value_m_[kNumberSonars];

  // GPIO pins for controlling the SONAR sensors.
  enum {
    kPinEcho0 = 35,
    kPinTrigger0 = 34,
    kPinEcho1 = 37,
    kPinTrigger1 = 36,
    kPinEcho2 = 41,
    kPinTrigger2 = 40,
    kPinEcho3 = 15,
    kPinTrigger3 = 14
  };

  // Microseconds per timer interrupt.
  static const uint16_t kTimerPeriodUSec = 20;

  // Desired milliseconds to wait between a low pulse and the next high pulse.
  static const uint16_t kTimerSamplingPeriodMSec = 10;

  // Timer interrupts to expire before a low pulse to high pulse transition is
  // taken..
  static const uint16_t kTimerCountsPerSamplingPeriod =
      (kTimerSamplingPeriodMSec * 1000) / kTimerPeriodUSec;

  // For converting SONAR echo time to millimeters.
  static const float g_time_to_mm_scaler_;
};

And here is tsonar.cpp

#include "tsonar.h"

#include <stdint.h>

#include "Arduino.h"
#include "TimerOne.h"
#include "Wire.h"
#include "tmicro_ros.h"

void TSonar::CommonInterruptHandler(uint8_t pin, long& end_time,
                                    long& start_time, uint8_t& average_index,
                                    size_t sonar_index) {
  switch (digitalRead(pin)) {
    case HIGH:
      // When the SONAR begins transmitting, the echo pin will
      // go HIGH. Capture the start time.
      end_time = 0;
      start_time = micros();
      break;

    case LOW:
      // When an echo is received, the echo pin will go LOW.
      // Compute the distance to the object and send a ROS message.
      end_time = micros();
      g_values_mm_[sonar_index] =
          (end_time - start_time) * g_time_to_mm_scaler_;
      g_values_mm_history_[sonar_index][average_index++] =
          g_values_mm_[sonar_index];
      if (average_index >= kNumberReadingsToAverage) {
        average_index = 0;
      }

      // Compute an average distance over the last few readings
      // to help reduce the sensor noise.
      int average_sum_mm = 0;
      for (size_t i = 0; i < kNumberReadingsToAverage; i++) {
        average_sum_mm += g_values_mm_history_[sonar_index][i];
      }

      g_average_value_m_[sonar_index] =
          (average_sum_mm * 0.001) / kNumberReadingsToAverage;

      // Publish the average distance for the sensor.
      TMicroRos::PublishSonar(sonar_index, g_average_value_m_[sonar_index]);
      break;
  }
}

float TSonar::GetAverageValueM(Sonar device) {
  return g_average_value_m_[device];
}

void TSonar::Echo0InterruptHandler() {
  // Each sensor has it's own distance timer and it's own
  // set of values used to compute an averate distance.
  static long end_time = 0;
  static long start_time = 0;
  static uint8_t average_index = 0;
  CommonInterruptHandler(kPinEcho0, end_time, start_time, average_index, 0);
}

void TSonar::Echo1InterruptHandler() {
  static long end_time = 0;
  static long start_time = 0;
  static uint8_t average_index = 0;
  CommonInterruptHandler(kPinEcho1, end_time, start_time, average_index, 1);
}

void TSonar::Echo2InterruptHandler() {
  static long end_time = 0;
  static long start_time = 0;
  static uint8_t average_index = 0;
  CommonInterruptHandler(kPinEcho2, end_time, start_time, average_index, 2);
}

void TSonar::Echo3InterruptHandler() {
  static long end_time = 0;
  static long start_time = 0;
  static uint8_t average_index = 0;
  CommonInterruptHandler(kPinEcho3, end_time, start_time, average_index, 3);
}

int TSonar::GetValueMm(Sonar device) {
  if (static_cast<int>(device) >= kNumberSonars) {
    return -1;
  } else {
    return g_values_mm_[static_cast<int>(device)];
  }
}

void TSonar::loop() {
  // Nothing needs to be done in the loop. Everything is drivven
  // by interrupts.
}

void TSonar::setup() {
  // Set up the Teensy pins to talk to the SONAR devices.
  pinMode(kPinEcho0, INPUT);
  pinMode(kPinTrigger0, OUTPUT);
  pinMode(kPinEcho1, INPUT);
  pinMode(kPinTrigger1, OUTPUT);
  pinMode(kPinEcho2, INPUT);
  pinMode(kPinTrigger2, OUTPUT);
  pinMode(kPinEcho3, INPUT);
  pinMode(kPinTrigger3, OUTPUT);

  // Setup a timer to drive the state machine.
  Timer1.initialize(kTimerPeriodUSec);
  Timer1.attachInterrupt(TimerInterruptHandler);

  // Attach an interrupt handler to each SONAR's echo pin.
  attachInterrupt(kPinEcho0, Echo0InterruptHandler, CHANGE);
  attachInterrupt(kPinEcho1, Echo1InterruptHandler, CHANGE);
  attachInterrupt(kPinEcho2, Echo2InterruptHandler, CHANGE);
  attachInterrupt(kPinEcho3, Echo3InterruptHandler, CHANGE);
}

void TSonar::TimerInterruptHandler() {
  // The states of the state machine.
  typedef enum {
    COUNTDOWN,   // Count down timer ticks.
    PULSE_HIGH,  // Begin transmitting the SONAR pulse.
    PULSE_LOW  // Stop transmitting the SONAR pulse and begin listening for the
               // echo.
  } TTimerState;

  // The current state of the state machine.
  static volatile TTimerState state = COUNTDOWN;

  // Used to count timer pulses to drive the timing of each SONAR sensor.
  static volatile long countdown = kTimerCountsPerSamplingPeriod;

  if (--countdown == 0) {
    // Time to start the next SONAR sensor in the list of sensors.
    state = PULSE_HIGH;
    countdown = kTimerCountsPerSamplingPeriod;
  }

  switch (state) {
    case COUNTDOWN:
      // Continue counting so that the sensors are spaced
      // apart in time so they don't interfere with each other.
      break;

    case PULSE_HIGH:
      // Time to send out the ping for the next SONAR in the list.
      if ((g_next_sensor_index_ % 4) == 0) {
        digitalWrite(kPinTrigger0, HIGH);
      } else if ((g_next_sensor_index_ % 4) == 1) {
        digitalWrite(kPinTrigger1, HIGH);
      } else if ((g_next_sensor_index_ % 4) == 2) {
        digitalWrite(kPinTrigger2, HIGH);
      } else {
        digitalWrite(kPinTrigger3, HIGH);
      }

      state = PULSE_LOW;
      break;

    case PULSE_LOW:
      // Time to stop the ping output and begin listening for the
      // echo for the next SONAR in the list.
      if ((g_next_sensor_index_ % 4) == 0) {
        digitalWrite(kPinTrigger0, LOW);
      } else if ((g_next_sensor_index_ % 4) == 1) {
        digitalWrite(kPinTrigger1, LOW);
      } else if ((g_next_sensor_index_ % 4) == 2) {
        digitalWrite(kPinTrigger2, LOW);
      } else {
        digitalWrite(kPinTrigger3, LOW);
      }

      g_next_sensor_index_++;  // Control the next sensor in the list the next
                               // time around.
      state = COUNTDOWN;       // Begin counting down again.
      break;
  }
}

TSonar::TSonar() : TModule(TModule::kSonar) {
  for (size_t i = 0; i < kNumberSonars; i++) {
    // Initialize the array of readings used to computer the average distance
    // per sensor.
    for (size_t j = 0; j < kNumberReadingsToAverage; j++) {
      g_values_mm_history_[i][j] = 0;
    }
  }
}

TSonar& TSonar::singleton() {
  // A singleton pattern is used to support the TModule design pattern.
  if (!g_singleton_) {
    g_singleton_ = new TSonar();
  }

  return *g_singleton_;
}

uint8_t TSonar::g_next_sensor_index_ =
    0;  // Which sensor in the list is being controlled now.

TSonar* TSonar::g_singleton_ =
    nullptr;  // Used to ensure that only one instance of this class is ever
              // constructed.

int TSonar::g_values_mm_[TSonar::kNumberSonars] = {
    -1, -1, -1, -1};  // The last reading per sensor.

int TSonar::g_values_mm_history_
    [TSonar::kNumberSonars]
    [TSonar::kNumberReadingsToAverage];  // List of readings per sensur used to
                                         // computer an average per sensor.

float TSonar::g_average_value_m_[TSonar::kNumberSonars] = {
    0, 0, 0, 0};  // The computed average distance per sensor.

const float TSonar::g_time_to_mm_scaler_ =
    (10.0 / 2.0) / 29.1;  // For converting time to distance.