C++ Library for $50 robot Version 2

This tutorial is now deprecated as well because my WebbotLib library now allows you to use C++.


This tutorial replaces the one given earlier and has been expanded to include: UART,Software PWM, DC Motors, Logging, Sonars, Serial LCDs, Scheduling and other bits'n'pieces.


I will be posting some examples, both as source and as pre-compiled hex files, with my EZ-CD robot that shows how to use the majority of these classes.


The latest library can be downloaded here


UnZip this file to a new folder making sure that WinZip creates all the necessary sub-directories.


Once unzipped - edit the 'makefile' in the start folder. Change the following lines:-

MCU_TARGET = atmega8

F_CPU = 8000000

to reflect the target CPU and processor speed and then build the libraries by running 'make' from a DOS box in this folder.


NB If you are building for a number of different targets then you must run a 'make clean' and then edit the makefile as above, and then run 'make' again. The above settings must also match the equivalent settings in the makefile for your own code that uses the library.

The 'make' will create a library file called 'bin\cpplib.a' which may then be included in your own projects - see the makefile in the example projects for other compiler settings.


This class handles analogue to digital input pins.To create a new input pin using channel 4 (ADC4) just use:

A2D myIn(4);


You can then read an 8 bit value via:

unsigned char value = myIn.ReadByte();


or a 10 bit value via:

unsigned short value = myIn.ReadShort();


Notice that you DON'T need to initialise the A2D convertor at all. Look at the file you will see the Init method that, by default, is called with some default values. If you want to use other values then you can call this Init method yourself BEFORE you create any A2D inputs. Since this is a static method it can be done by writing:



If you need to save power and turn off the A2D function then call:


You can turn it back on later by calling A2D.Init again.



This class wraps a character array into a buffer that may be accessed in a thread safe way. IE it can be accessed by your main program, as well as by interrupts. One of its main uses is to create a buffer for accessing the UART whereby you can read/write characters at the same time as the UART reads/writes characters. Or it may be used to write info to a logging output which may go to your PC, an LCD display etc.


To create a 48 byte buffer:-

uint8_t  byteBuffer[48];       // allocate the space

BUFFER buffer(byteBuffer);   // wrap it into a safe buffer



uint8_t get();                     // Get the next byte and remove it from the buffer, or 0 if it is empty

uint8_t get(uint16_t index);  // Get the value from a given index (0=current), but don't remove it

void skip(uint16_t bytes);    // Remove the next 'bytes' elements from the buffer

void flush();                      // Clear the buffer

bool isFull();                      // return 'true' if the buffer is full

bool isEmpty();                  // return 'true' if the buffer is empty

bool put(uint8_t byte);       // try to add 'byte' to the buffer. Return false if the buffer was full, or true if it was added ok


A DC motor generally requires a PWM (speed) signal and a direction/brake pin. DC motors don't require very accurate PWM signals so this class uses PWMOut to generate the PWM signal on any output pin via software and then uses one additional output pin for controlling the direction (fwd/reverse) of the motor. Check the requirements of your DC Motor Controller or see my Motor Controller Tutorial.


So for example:-


 OUT g_direction('B', 6);         // Use B6 as the direction pin to the motor driver
     PWMOUT g_pwm('B', 5, 50);  // Use B5 as the PWM signal to the motor driver. PWM signal is every 50ms.
     DCMOTOR g_motor(&g_pwm, &g_direction);  // Join the two lines above into a single DC motor


Now we can just use the methods in Motor.hpp on g_motor. ie SetSpeed(value). This class will then sort out the setting of the direction pin and the PWM signal for you.


This is an abstract class that defines an interface for any kind of display. It only defines the one method:

void display(uint8_t c);


This means that any kind of 'Display' must be able to display a character.


Common functions for all I/O pins.


Pins are normally created via IN or OUT so that their initial state is known. See IN.hpp and OUT.hpp


uint8_t GetBit(); // Get the bit mask for this Pin

bool IsValid(); // was this defined as a valid pin


// The following are only needed by devices which use the same I/O pin to change between in and out - eg some sonar devices

void SetInput(); // Change this into an input pin

void SetOutput(); // Change this into an output pin



Defines a pin that is normally a digital input pin.


You can create a new one at the start of your code by:

   IN myPin('B',4,false); // creates an input on B4 with no pullup resistor


Methods you can call:-

   bool IsLow(); // is the pin 0v

   bool IsHigh(); // Is the pin 5v




This is a simple extension of Out.hpp that has more friendly methods On() and Off(). You can also use the methods from OUT ie Toggle() will change the state of the LED.


This class is used to log character data to any kind of display device and is often used for debugging purposes.

void put(char c);                  // Output a character to the log.

void crlf();                          // Move down to the next line

void putStr(const char *str); // Output a string

void putHexDigit(uint8_t data); // Output the number (0..15) as one hex digit

void putHex(uint8_t data);     // Output number as two hex digits

void putHex(uint16_t data);   // Output number as four hex digits

void putHex(uint32_t data);   // Output number as eight hex digits


// Output a value, as a given number of digits, in base x, signed/unsigned, with a given padding character

void putNumber(int32_t value, uint8_t numDigits, uint8_t base=10, bool isSigned=true, char padchar=' ') ;


Motor.hpp is the base abstract class for all motors - ie these methods apply to all sorts of Motors be they servos, dc motors etc.



MOTOR::MAX_SPEED represents full speed ahead

MOTOR::MIN_SPEED represents full speed backwards

The middle point between these two values (which is normally 0) represents STOP.


int SetSpeed(int speed);

This will set the new speed for the motor. This value will be clamped to be between MIN_SPEED and MAX_SPEED. The newly clamped speed will be returned to the caller. If the motor hardware incorporates a brake then setting a speed of 0 will apply the brake.


int SetSpeed(int speed, bool forwards);

A convenience method for setting a positive speed and a direction. Is the same as:


int GetSpeed();

Get the current speed of the motor. This will be between MIN_SPEED and MAX_SPEED. NB this is the current value from the last SetSpeed and does not reflect the actual physical motor speed.


void Disconnect();

Depending on the abilities of the motor driver this will attempt to put the motor into 'coasting' mode by disconnecting it from the power. If the robot is on a hill then this may cause the robot to start rolling backwards. If this is not intended then use SetSpeed(0) to apply a brake.


void Reconnect();

Reconnect the motor to the supply following a disconnect


bool IsConnected();

Is the motor currently connected?







A class for defining output pins that can be set high or low.


You can create a new one at the start of your code by:

OUT myPin('B',4,false); // creates an output on B4 and set it to low to start with


You can then change it to low or high with

myPin.SetLow(); // set it low

myPin.SetHigh(); // set it high

myPin.Set(true); // set it high

myPin.Set(false); // set it low

myPin.Toggle(); // If it was low set it high, if it was high then set it low



This class is used to generate a PWM signal on ANY outout pin using software rather than hardware timers. The granularity of the timer is multiples of 1ms. Hence it is not suitable for servos but is ideal for driving DC motors, dimming LEDs etc.


This class extends Out.hpp so that it can use any output pin.


To create an output which is high for every 1ms per 20ms on port D5 use:

PWMOUT pin('D',5, 20, 1);


The duty cycle can be varied, ie by modifying the last parameter given in the constructor, by:-

void setOnTime(uint16_t onTime);    // set the number of ms when the signal is high

uint16_t getOnTime();                   // get the number of ms when the signal is high - either from constructor or last call to setOnTime

uint16_t getCycleTime();               // get the ms per cycle as given in the constructor


So if you want an LED on port D5 to flash for 1 second every 3 seconds then just declare:

PWMOUT led('D',5,3000,1000); // Thats all you need to do!!!!!





This class allows you request a call back after a given number of milliseconds.


There is only one scheduler but it can queue up a finite maximum number of jobs - the default is 16 but can be chnaged via MAX_JOBS in Scheduler.hpp.  This is used by PWMOUT and its derived classes but may also be used by you for queuing up any kind of job.


To schedule a call back use

Scheduler::get()->schedule(SCHEDULED_JOB* job, uint16_t delay_ms);


The job that is scheduled must be derived from SCHEDULED_JOB and after the delay has elapsed then the 'schedule' method is called on the object. Look at PWMOut.hpp and PWMOut.cpp for a working example that implements PWM via the scheduler.




This is the abstract base class that defines the interface provided by all Serial (ie +5v UART driven) displays.


Note that this class extends Log.hpp so may be used as a logging destination.


uint8_t getNumRows(); // returns the number of rows in the LCD

uint8_t getNumCols(); // returns the number of columns in the LCD

void move(uint8_t column, uint8_t row); // move the cursor to a given location


The only current concrete implementations are various MatrixOrbital LCD displays. As well as providing the required methods above then they also provide other methods to: set backlight brightness, contrast, draw graphs etc.


This class is used to drive a modified servo via a general output pin.

So all of the methods of Motor are available.


To create a new servo motor pin you must first define an output pin eg.

OUT  g_servoSonarOut('B', 7);  // define B7 as an output pin

SERVO  g_servoSonar(&g_servoSonarOut, false, 1500, 500); // create a servo on B7, non-inverted, center=1.5ms, 0.5ms either side

If your motor is running in the wrong direction then change the 'false' parameter to 'true' - you don't need to change anything else.


The last two parameters specify the pulse width to center the servo (ie 1.5ms) and how much the pulse width varies on either side of that value. NB The last 3 parameters are optional and if they are missing then they will be assumed to be false,1500,500 respectively.


Note that a servo should only send out an action every 20ms. This class remembers when the speed was last set for each servo and so if you try to change the speed quicker than this then the class will make sure that all is well.  So your main loop doesn't need any delay for the servos.


This class represents a Servo being driven by a hardware (background) PWM signal from the microcontroller on ports B1 or B2.

In all other ways it is identical to Servo.hpp


This abstract class defines the interface that all sonars must provide. Sonars are assumed to be input pins but may use the IOPin functions to briefly change themselves to output for ping transmission.


The interface implemented by all sonars is:

uint16_t read();  // read the 'raw' value from the sonar. Different makes may return different values for the same distance.


uint16_t read_cm(bool useSmoothing=false); // This will return a value in 'cm' regardless of make. If useSmoothing is true then the driver will attempt to return a 'smoothed out' value in order to prevent any spurious spikes. Each actual driver may choose to ignore this parameter or it may implement it in a different way.


bool isReadyToRead(); // Since a sonar ping can echo around for a while then it is best to wait for a while before taking the next reading so as not to get echos from the previous ping. This method will return 'false' if the last ping is still echoing or if there is any other reason as to why a new 'read' command should not be performed.


At the moment the only concrete class is: Sonar\Devantech\SRF05.hpp

but, by examining its code, then it should be very easy to implement other sonars such as the Ping.



The library uses Timer2 for all timings.


TIMER::get()  returns a reference to the timer. As this is a reference then all other routines should be accessed via the '->' operator. ie TIMER::get()->MsCur();


unsigned long MsCur(); returns the number of milliseconds since power was switched on. Since an unsigned long can store a high value then it provides an effective 'time of day' clock. NB If your robot can be left switched on for a long period then you will have to take into account that this value may wrap around to 0.


bool HasElapsed(unsigned long msStart, unsigned in msWait); This will test if a given amount of time has passed. 'msStart' is a value you saved previously by calling MsCur() and 'msWait' is the number of milliseconds you want to check to see if they have passed. The method will return immediately with either 'false' if the period hasn't elapsed or 'true' if it has. This is useful with sensors such as sonars which should not be called without a reasonable delay between samplings so as to avoid ghost echoes. You can guard against this by saving the time when the sonar was last sampled. The next time it is called you can check that the minimum amount of time has elapsed since the last call and, if not, you could return the previously returned value.


void WaitMs(unsigned int ms); This will not return until the given number of milliseconds has elapsed. Note that this may be out by 1ms either way so it's not good for small numbers.


void WaitMs(unsigned int ms, unsigned long msStart); is similar to 'HasElapsed' except that it will not return until the given period has elapsed.



This class allows you to easily create a non-buffered UART. By non-buffered then I mean that when you send a character then the system will wait until the character has been transmitted before your code can continue. A more efficient Uart can be achieved via UartBuffered which allows you to pass an entire string buffer which is then sent in the background.


To define the UART use:- UART (uint32_t baud = 9600, FuncPtru08 rx_func = 0);

The first parameter specifies the baud rate (or 9600 if not specified) and the second parameter specifies the name of your function that is called when a character is received (if not specified then received chars are ignored).


So to specify a UART that is only used for logging output at 19,200 then just use:-

UART log (19200);


As a 'Display' class the UART supports:

void display(uint8_t c);


This class provides a Uart with buffered input and output and so allows your code to go at full speed whilst receiving and sending data in the background.


To create a buffered UART for output only that can buffer 64 characters then use:-

uint8_t g_uart_buffer[64];                                 // create space for 64 bytes

BUFFER g_uart_out(g_uart_buffer);                     // wrap it in a buffer

UART_BUFFERED g_uart(19200, &g_uart_out);     // connect the buffer to a 19,200 baud uart


You can then write output by using:



Equally you can define  an input buffer as well:-

uint8_t g_uart_buf_out[64];                                 // create space for 64 byte output

BUFFER g_uart_out(g_uart_buf_out);                     // wrap it in a buffer

uint8_t g_uart_buf_in[32];                                   // create space for 32 byte input

BUFFER g_uart_in(g_uart_buf_in);                         // wrap it in a buffer

UART_BUFFERED g_uart(19200, &g_uart_out,&g_uart_in);  // connect the buffers to a 19,200 baud uart

And then access the input buffer via the Buffer.hpp commands such as:-

if(  ! g_uart_in.isEmpty() ){

   uint8_t input = g_uart_in.get;  // get next char from uart

   ... do something ....




delay_cycles(volatile unsigned int cycles) is the same as the delay loop from the original $50 tutorial in that it just pauses for a given delay.


int interpolate(int value, int minVal, int maxVal, int minRtn, int maxRtn) is a useful routine. This takes a 'value' that is known to be between 'minVal' and 'maxVal' and then returns a number in the range 'minRtn' and 'maxRtn'. So if you wanted to convert a number which is in the range 0 to 100 into a matching number in the range -255 to 255 you could just do:

int answer = interpolate(value, 0, 100, -255, 255);


int clamp(int value, int minVal. int maxVal); Will limit a value to the range minVal to maxVal. This is the same as writing:

if(value < minVal) value = minVal;

if(value > maxVal) value = maxVal;


The macros 'CRITICAL_SECTION_START' and 'CRITICAL_SECTION_END' can be used to surround a piece of time critical code. Since it disables interrupts then it shouldn't be used if the surrounded code takes a long time to run as you may loose some other critical interrupts.