Tracking System using 9dof Razor IMU and Servo

I'm developing a simple tracking system using the Sparkfun 9dof Razor IMU and a servo motor. I have a C++ application that reads the IMU data and calculates the change that needs to be made so that the system is pointed at the set location. Now I need to implement the Servo to read this calculation and rotate as needed.

The 9dof Razor IMU is using a simple FTDI breakout board to connect via USB. I am using an Arduino Uno R3 to control the servo.

I want to implement the servo rotation in my C++ program so the Arduino rotation signals will be sent from that. Right now, I'm looking into using Firmata to do this.

So with Firmata, I upload the standard firmata code to the Arduino and then control it elsewhere. My problem is I'm not sure where to begin with controlling it in my C++ program. All the code needs to do is determine if the change needed is negative or positive. If the value is negative it rotates in one direction. If it is positive, it rotates in the other.

Any suggestions or help would be greatly appreciated. I am completely open to using something other than Firmata.


Please add links to the components you are using (paste the url in the text). This is your sensor board, That costs 124.95 dollars ? Three sensors (accel + gyro + compass) can be bought for 20 dollars on Ebay, or 10 dollars for 10DOF board, or 8 dollars if the magnetometer is on a seperate board.

A 'normal' setup with Arduino would be: - An sensor board with I2C digital interface. - An Arduino as the main controller for the servos, sensor board, kalman filtering, PID control.

A library to control servos, A library for PID, A guide for Kalman filtering,,58048.0.html

But you have a PC somewhere in the middle......

Yes, that is my sensor board. I already have it set up and an application developed for it. That is not my concern, nor my question.

Thank you for the links but I have already gone over the Servo reference page. I also already have my algorithm set and I have no need for Kalman filtering.

I am simply looking for a way to communicate with the motor (controlled by an Arduino Uno) in my C++ application.

Thanks! :)

I see, I was confused by the PC in the middle.

The Arduino has an (virtual) serial port via the USB bus. It is used to upload sketches and to show messages for debugging and also to control the Arduino. Since the Arduino has some tasks to do (like the servos), I think you can not use firmata on top of that.

The program on the PC has to use the (virtual) serial port. Some use Python, or Java, but you have to program it in the C++ code. You can make your own communication protocol. You can make the communication readable, so you can use any terminal program to check upon the Arduino. The Arduino side of the Serial, start here,

Is this the answer you were looking for ?

I understand that a virtual serial port is created via the USB port. The same thing is done with the IMU. However, after looking at the Serial reference, I have no idea how I would go about making my own communication protocol. The examples don’t provide what I’m looking for, something to go off of.

I have spent a lot of time on Google and have read all relevant references on the Arduino websites. I am hoping for a relevant example or a detailed explanation of how I can accomplish this.

P.S. - Just a heads up, I am using OS X and my application is entirely Terminal based.


I just learned that Firmata can be combined with servos. In the Arduino IDE is an example: File / Examples / Firmata / ServoFirmata

For your own protocol, go to the Serial page On the right are some examples, click on ReadASCIIString That show how to use it, with Serial.parseInt.

This is a tutorial with Serial and Servo

This still does not help me with implementing anything in C++.

Anyone else have any ideas? Thanks!

Right now, I’m looking into using Firmata to do this.

Don’t. As has been at least hinted at, develop your own protocol. Using C++, you can write anything to the serial port. So, write something like “<n, pos>”, where n is the servo to move and pos is the new position.

Then, on the Arduino, use some code like this:

#define SOP '<'
#define EOP '>'

bool started = false;
bool ended = false;

char inData[80];
byte index;

void setup()
   // Other stuff...

void loop()
  // Read all serial data available, as fast as possible
  while(Serial.available() > 0)
    char inChar =;
    if(inChar == SOP)
       index = 0;
       inData[index] = '\0';
       started = true;
       ended = false;
    else if(inChar == EOP)
       ended = true;
      if(index < 79)
        inData[index] = inChar;
        inData[index] = '\0';

  // We are here either because all pending serial
  // data has been read OR because an end of
  // packet marker arrived. Which is it?
  if(started && ended)
    // The end of packet marker arrived. Process the packet

    // Reset for the next packet
    started = false;
    ended = false;
    index = 0;
    inData[index] = '\0';

Where it says “Process the packet”, use strtok() and atoi() to get the servo number and servo position. Then, figure out how a servo number relates to a Servo instance, and set that instance’s position.

KevinChristman: This still does not help me with implementing anything in C++.

I can find this, it has source code, Perhaps this uses a serial communication, But I don't know anything about OS X. Perhaps you should start a new topic for it, or ask it on a OS X software development forum ?