Prototyping part of the electronic system with Arduino

Computer, PSU and Arduino breadboardWhen I started this project I knew I couldn’t attack it all at once. This was for reasons of time, money and tool access so I have to pick away at it whenever I can. Currently that means working on the low-level Arduino controller and its integration with ROS, using a number of sensors that I had lying around or I bought for the project.

This post will describe the parts I used and go over the Arduino coding in reasonable detail. I won’t be including the full program but will be limited to the subscriber and publisher code that I’m using.

Components

The Arduino is communicating with ROS via the rosserial_arduino node, available on the ROS wiki. It has great documentation and I’ll skip the basic setup steps.

12V fuse block

Power distribution

The primary job is to provide power for all of the subsystems. As I mentioned previously, YardBot is controlled by a Mini ITX computer running Ubuntu and ROS. This box takes 12V DC input and distributes it internally to all related computer systems, like a standard tower PSU. While the robot itself will get 12V power from a 24V battery regulated down, prototyping is easier with a wall-driven source. For that I’m using a 350W computer PSU I’ve modified for electronic projects. I decided to use Traxxas RC power connectors in many places, for their high current capacity and solid connection. The main 12V output on the PSU has a connector soldered on the end that I feed to the fuse block through the AttoPilot sensor. The fuse block has a common ground to make wiring up different parts easy. Eventually YardBot will use a new fuse block for each common voltage level (24V, 12V and 5V).

AttoPilot 45A sensor

Power sensing

Between the PSU output and fuse block is the AttoPilot current and voltage sensor. This monitors the power coming from the batteries for low power situations and keeps an eye on current draw. A ROS node will check the published data to see if it remains inside the safety bounds.

As shown by the Sparkfun documentation, reading the sensor is as simple as monitoring an analog pin and scaling accordingly.

// PIN DEFINITIONS
#define CURRENT_PIN 7
#define VOLTAGE_PIN 6

// scaling properties
#define VOLTAGE_SCALE  49.44
#define CURRENT_SCALE  14.9

// message type setup
std_msgs::Float32 currentMsg;
std_msgs::Float32 voltageMsg;

// publisher definitions
ros::Publisher currentPub("/arduino/current", &currentMsg);
ros::Publisher voltagePub("/arduino/voltage", &voltageMsg);

// triggered by a timer every 1/10 second
void readSensors() {
  currentMsg.data = currentValue();
  currentPub.publish(&currentMsg);
  
  voltageMsg.data = voltageValue();
  voltagePub.publish(&voltageMsg);
}

float currentValue() {
  // read and scale the data from the current sensor
  int currentReading = analogRead(CURRENT_PIN);
  return currentReading/CURRENT_SCALE;
}

float voltageValue() {
  // read and scale the data frm the voltage sensor
  int voltageReading = analogRead(VOLTAGE_PIN);
  return voltageReading/VOLTAGE_SCALE;
}

Much of the code here is reasonably self explanatory. You can see that I define a few properties like scaling factor and pins, then assign the ROS message type and publisher topic. All of the topics used by the Arduino will be assigned the /arduino domain to keep things easy to read. I use a timer library to read and publish sensor data on a set time interval.

9DOF IMU and breadboard

IMU data

ROS uses IMU data extensively. The IMU, or Inertial Measurement Unit, uses an accelerometer, gyroscope and optionally a magnetometer to track how a vehicle travels. Used in combination with wheel encoders, the robot can have a reasonably accurate picture of where it is at any given moment. YardBot uses the LSM9DS0 IMU from Sparkfun, with full accelerometer, gyro and magnetometer capabilities on 3 axes each for 9 degrees of freedom. It connects to the Arduino via i2c at 3.3V, so that is the second PCB you can see on the breadboard above. It is a logic level shifter that allows the 5V Arduino to communicate with the 3.3V sensor. The Arduino does very minimal processing in order to avoid delays and blocks so in this case, it produces a standard IMU message and publishes it.

// for ease of use, use SparkFun's own library
#include <SFE_LSM9DS0.h>

// simple estimates based on ROS data, which will be modified with experience
#define ACCEL_COVARIANCE  0.001
#define GYRO_COVARIANCE  0.0015

// build and assign the library classes
#define LSM9DS0_XM  0x1D
#define LSM9DS0_G  0x6B
LSM9DS0 imu(MODE_I2C, LSM9DS0_G, LSM9DS0_XM);

// ROS messages and publisher
sensor_msgs::Imu imuMsg;
ros::Publisher imuPub("/imu_data", &imuMsg);

// run inside the 1/10 second sensor timer
void updateSensors() {
  // orientation -- ignored
  imuMsg.orientation_covariance[0] = -1;
  
   // accelerometer
  imu.readAccel();
  imuMsg.linear_acceleration.x = degreesToRads(imu.calcAccel(imu.ax));
  imuMsg.linear_acceleration.y = degreesToRads(imu.calcAccel(imu.ay));
  imuMsg.linear_acceleration.z = degreesToRads(imu.calcAccel(imu.az));
  imuMsg.linear_acceleration_covariance[0] = ACCEL_COVARIANCE;
  imuMsg.linear_acceleration_covariance[4] = ACCEL_COVARIANCE;
  imuMsg.linear_acceleration_covariance[8] = ACCEL_COVARIANCE;
  
  // gyro
  imu.readGyro();
  imuMsg.angular_velocity.x = gToMeters(imu.calcGyro(imu.gx));
  imuMsg.angular_velocity.y = gToMeters(imu.calcGyro(imu.gy));
  imuMsg.angular_velocity.z = gToMeters(imu.calcGyro(imu.gz));
  imuMsg.angular_velocity_covariance[0] = GYRO_COVARIANCE;
  imuMsg.angular_velocity_covariance[4] = GYRO_COVARIANCE;
  imuMsg.angular_velocity_covariance[8] = GYRO_COVARIANCE;
  
  // publish the data
  imuMsg.header.stamp = nh.now();
  imuPub.publish(&imuMsg);
}

float gToMeters(float g) {
  return g*9.81;
}

float degreesToRads(float d) {
  return d/180*PI;
}

Similar to the power sensor, I define static properties first then build the classes used by ROS and the Arduino directly. The IMU message has 3D vectors for orientation, angular acceleration and linear acceleration, with covariance data for each channel. Since at the moment I don’t have an orientation element, I’ve set the covariance of it to -1 in the array. Covariance itself is a measure of a sensor’s presumed accuracy. Right now I’m using standard ROS recommended values but those will be modified as I gain experience and see valid output.

Much of the missing methods are provided by the SparkFun library, which makes it pretty easy to get meaningful data from the sensor. You might notice that I’m not using the /arduino domain for this sensor, and that’s because standard ROS behaviour is to use /imu_data, which is assumed for other processing nodes.

DHT03 sensor and buzzer

Environmental data

I thought about what other sensors I might like to include on YardBot and one of the easy ones is a temperature and humidity sensor for environmental data. This has safety benefits too, as it allows the robot to monitor itself for overheating and excessive moisture. I’m using a watchdog node that will check the sensor data and provide feedback accordingly. The DHT03 sensor is very common and has a very good library for reading it from Adafruit. I publish both temperature and humidity channels on independent topics.

// include the library
#include <DHT.h>

// assign the single pin
#define DHTPIN  2

// create the DHT sensor object
#define DHTTYPE DHT22 
DHT dht(DHTPIN, DHTTYPE);

// set up the ROS messages and publishers
sensor_msgs::Temperature temperatureMsg;
sensor_msgs::RelativeHumidity humidityMsg;

ros::Publisher temperaturePub("/arduino/temperature", &temperatureMsg);
ros::Publisher humidityPub("/arduino/humidity", &humidityMsg);

// run by timer every 1 sec
void updateEnvironmentSensors() {
  // read and publish the different sensors available
  float temperature = dht.readTemperature(); 
  if (!isnan(temperature)) {
    temperatureMsg.header.stamp = nh.now();
    temperatureMsg.temperature = temperature;
    temperaturePub.publish(&temperatureMsg);
  }
  
  float humidity = dht.readHumidity();
  if (!isnan(humidity)) {
    humidityMsg.header.stamp = nh.now();
    humidityMsg.relative_humidity = humidity;
    humidityPub.publish(&humidityMsg);  
  }
}

Once again I use a pretty standard process from the DHT library. The timer runs every second instead of 0.1 seconds because the sensor can only be read at 4Hz, or every 0.25 second. Since environmental data isn’t as critical as something like the IMU, I just publish once per second.

Warning buzzer

This feature is a little more about “what else do I have lying around that I can add in?” but I think it does serve a practical purpose. Shown in the photo above is a 2 wire motherboard speaker used to make that POST beep on computers. Arduino has a method called tone() that oscillates a pin at high frequency to generate a sound, so I’m using it to warn about operating conditions. It’s connected to a ROS subscriber that will likely be used by the watchdog node and the message content contains the number of beeps to make.

After making the node work, I noticed that with all of the callbacks, timers and methods running together, the Arduino is not completely real-time. Even though the timer is set to run every 1/4 second, I can tell the beeps are not consistent. It’s just something I’ll have to keep in mind for future nodes, and is actually the reason I’ve offloaded most of the high cost calculations to the Mini ITX board.

#define BEEP_PIN  8

// define some timer values
boolean beepState = false;
#define BEEP_PERIOD 250
int beepTimer = -1;

// define ROS callback before making the subscriber node
void beepCallback(const std_msgs::Int8 &msg) {
  if (msg.data == -1)
    beepTimer = timer.every(BEEP_PERIOD, toggleBeep);
  else if (msg.data == 0) {
    noTone(BEEP_PIN);
    timer.stop(beepTimer);
  }
  else
    beepTimer = timer.every(BEEP_PERIOD, toggleBeep, 2*msg.data);  
}

// create the ROS subscriber objects
ros::Subscriber<std_msgs::Int8> beepSub("/arduino/beep", &beepCallback);

// method that switches on the beep
void toggleBeep() {
  beepState = !beepState;
  
  if (beepState) {
    tone(BEEP_PIN, BEEP_FREQ);
  } else {
    noTone(BEEP_PIN);
  }
}

This is the first ROS subscriber on the Arduino, and it receives a simple integer message that corresponds to the number of beeps to make. The callback itself processes the integer and either turns off the beeper directly or sets the number of toggles to make. Every time “toggleBeep” is called, it switches the state of the tone pin.

YardBot LEDs

LED status indicator

Feedback to the robot operator is very important so YardBot uses an LED to share information. The actual robot will be using a full LED strip but for now I’m starting work with a simple 5mm RGB LED. It’s controlled by the LED topic and a custom ROS message. The iPhone app I wrote for YardBot can control the LED directly but the primary driver will be from other nodes sharing their statuses. For example, it glows green when first connected, then will glow yellow for current usage and blink red when the batteries are low.

iPhone app screenshotThis is a screenshot of the native iPhone app I wrote using my custom ROSbridge library and it provides a method to send the red, green and blue colour channels, the LED mode (on, off or blink) and blink interval.

The LED code uses RGB and Colour libraries I wrote to encapsulate colour channels. It’s simpler than storing red, green and blue variables at the top level of the program. Once again we assign relevant pins, build the ROS objects as required and define the subscriber code. Inside the callback the LED mode is assigned based on the message mode and a colour is built from the message colour channels. Only the “blink” mode has anything of real interest, as I start a timer based on the message interval then toggle the colour with the method toggleLED.

#include <RGB.h>
#include <Colour.h>

// define pins
#define RED_RGB_PIN  7
#define GREEN_RGB_PIN  6
#define BLUE_RGB_PIN  5

// timer data
int ledTimer = -1;
boolean ledState = false;

// assign relevant objects
Colour red(RED);
Colour green(GREEN);
Colour currentColour(0,0,0);
RGB rgb(RED_RGB_PIN, GREEN_RGB_PIN, BLUE_RGB_PIN);

// ROS callback method
void ledCallback(const yardbot_msgs::Led &msg) {
  if (msg.mode == msg.LED_OFF) {
    rgb.off();
    timer.stop(ledTimer);
  }
  else if (msg.mode == msg.LED_ON) {
    Colour ledColour(msg.red, msg.green, msg.blue);
    rgb.on(ledColour);
  } else if (msg.mode == msg.LED_BLINK) {
    Colour currentColour(msg.red, msg.green, msg.blue);
    ledTimer = timer.every(msg.interval, toggleLED);
  }
}

// create ROS subscriber
ros::Subscriber<yardbot_msgs::Led> ledSub("/arduino/led", &ledCallback);

// toggle the LED on/off
void toggleLED() {
  ledState = !ledState;
  
  if (ledState) {
    rgb.on(currentColour);
  } else {
    rgb.off();
  }
}

With all of the components connected and the Arduino running, start the serial_node program with rosrun. Once it’s connected, you can monitor the data stream by using rostopic echo.

Even though the Arduino has many capabilities, it’s important to remember its limitations and use it as a low-level controller rather than doing expensive calculations. Hopefully this provides a solid base to build and improve on during the project.

Tagged with: , , , , ,

Leave a Reply

Your email address will not be published. Required fields are marked *

*