Let's Make Robots!

Pushing the limits with encoders

This is now a tutorial and can be found here: http://letsmakerobots.com/node/39098

I've been working on some code so that a simple Mini Driver using a lowly ATmega8A can drive two motors with encoders and 8 servos. This may not sound hard at first until you remember that the Servo library kills PWM on the ATmega8. To make matters worse, D2 and D3 are the external interrupt pins but we need them for driving the servos.

To solve all these problems I have written some simple code that uses some "virtual" timer interrupts to generate PWM, monitor the encoders and control the motors. This allows the Servo library to drive 8 servos directly from the battery using the Mini Driver.

By "virtual" timer interrupt I mean that my code uses the micros() function to call functions at reasonably precise time intervals. This method may not be as precise as a hardware generated timer interrupt but still works quite well for reading encoders and motor control.

The end result is the attached code that turns the Mini Driver into an I2C controlled "Smart" motor controller and servo driver. Just send 10 integers (20 bytes, high byte first) to give the Mini Driver:

  1. left  motor speed
  2. right motor speed
  3. servo 0 position
  4. servo 1 position
  5. servo 2 position
  6. servo 3 position
  7. servo 4 position
  8. servo 5 position
  9. servo 6 position
  10. servo 7 position

This code has been optimised for a 2 motor, 2 encoder Rover 5 chassis with encoders but by changing a few values in the encoder and motor driving section it could drive different motor / encoder combinations.

A nice little feature is that by expressing the servo position as a negative number you can reverse the sense of direction. This can be very useful when you replace a servo with one of a different brand and find it turns the opposite way to the old servo.



How the code works:

This is only very simple code and does not use any hardware driven timer interrupts. The down side to this is that the timing is less precise although good enough for our purposes. The up side is that the code is easier to adapt between different Arduino boards.

The" virtual" timer interrupts simply compare the time when the action was last performed with the micros() timer. When a minimum number of microseconds has elapsed, the new time is saved and the function is called again. This method also works very well with the millis() timer if your measuring longer periods of time.

The code has 5 main sections:

  1. PWM generator: This is only needed for the ATmega8 as the Servo library disables PWM on pins D9 & D10.
  2. Encoder monitor: External interrupts are not used as we have 4 encoder inputs. This frees D2 & D3 for driving servos.
  3. Motor Control: This code uses the encoder information to adjust the PWM to the motors.
  4. I2C Command: This code accepts motor speed and servo positions from the I2C bus. Corrupt data is flushed.
  5. Servo Control: data from the I2C command section is used to position servos.



PWM generator: This piece of code counts from 0 to 255 repeatedly, incrementing every 80uS. This gives us an 8 bit PWM with a frequency of about 48Hz. Every time the counter increments it is compared to the desired PWM for each motor. If the counter is less than the desired PWM then the output is high, otherwise it's low. If the desired PWM is 50 then the counter will be less than 50 for 50 counts (0-49) and the output will be high.

  //======================================================== PWM Generator ==================================================================================
  if(micros()-etime>79)                                   // timer generates 8 bit PWM at aproximately 50Hz
    etime=micros();                                       // reset timer
    static byte pwm;                                      
    pwm++;                                                // byte "pwm" constanly increments (0-255)

I had previously used direct port manipulation to control the PWM pins but this seemed to cause interferance with some of the servo outputs. Until I can find the source of the problem I have reverted back to using the digitalWrite() command. This will be easier for novices to follow but it takes longer for the processor to process.

If you are using a different processor to the ATmega8 then you may be able to eliminate the PWM generator entirely and simply add an analogWrite() command for left and right motors in the Motor Control section of the code.You will see these lines in the motor control code have been disabled.

If you wish to change the PWM frequency from the default frequency then have a look at the PWM cheat sheet here: http://playground.arduino.cc/Main/TimerPWMCheatsheet.

Note: although frequencies above 20KHz can eliminate motor noise they can also eliminate motor drivers that are not designed for high frequencies and cause the blue smoke to come out! Many small hobby motors have poor torque at these higher frequencies so I recommend using lower frequencies instead.


Encoder Monitor: This section of code is equivalen to the Interrupt Service Routines (ISR) normally used with external interrupts. the main reason I did not use external interrupts here is that many robot chassis's including the Rover 5 use quadrature encoders. This means each motor has 2 encoder outputs that are 90° out of phase.

  //======================================================== Encoder Monitor ================================================================================

  encold=encnew;                                          // previous new state now becomes old state
  encnew=PINC&B00001111;                                  // read new encoder state

  if((encold&B00000011)!=(encnew&B00000011))              // compare old state of left encoders with new state of left encoders                  
    lpulse=int((micros()-ltime)/50);                      // time between last state change and this state change
    ltime=micros();                                       // update ltime with time of most recent state change
    lcount++;                                             // increment left motor distance counter
  if((encold&B00001100)!=(encnew&B00001100))              // compare old state of right encoders with new state of right encoders 
    rpulse=int((micros()-rtime)/50);                      // time between last state change and this state change
    rtime=micros();                                       // update ltime with time of most recent state change
    rcount++;                                             // increment right motor distance counter

This code does not try to determine the direction of the motor so we could simply use only one output from each encoder but using both outputs doubles the encoder resolution and halves the response time. To do this we need to monitor 4 pins and many Arduino compatible boards only have 2 external interrupts.

I have used direct port access here so that I can read all pins at once creating a "snapshot" of the encoders at that moment in time. This is quicker and more accurate than reading each pin individually. The code starts by taking the previous snapshot and saving it in the variable encold. A new snapshot is taken and saved in the variable encnew.

Because this code is written for the Mini Driver and I want to keep the digital pins free for servos, I've attached my encoder pins to the analog inputs A0 - A3. A4 and A5 are the I2C pins so they cannot be used. I do not use the analog inputs for reading analog values anyway because the Arduino takes about 260uS to read an analog value and this would disrupt my PWM generation.

The variables encold and encnew are bit-masked so that we only look at the left or right encoders and then old is compared to new. If there is any change then the time since the last change is measured. This time is later used to determine the current speed of the motor.

The time is divided by fifty because sometime the encoder discs have flaws in them so that even when a constant speed is acheived the time period from one state change to the next can vary a bit. As unsigned long variables do not have a decimal point, when we divide by 50 the remainder is truncated thus eliminating any small fluctuations.

As the encoders are monitored quicker than the pins can change states when the motor is at full speed this method is just as accurate as using an external interrupt although more work is done by the processor since some times the pins have not changed states when they are checked.


Motor Control: In this section of the code, the period of time between encoder state changes is compared to the desired time period and the PWM of each motor is adjusted accordingly. To calculate the desired period of time we need to know what is the maximum speed of the motors.

    if(micros()-ltime>50000ul && lspeed!=0) lpwm+=4;      // jumpstart stalled motor
    if(micros()-rtime>50000ul && rspeed!=0) rpwm+=4;      // jumpstart stalled motor
    digitalWrite(lmdirpin,lspeed>0);                      // set direction of left  motor
    actual=20000/(abs(lspeed)*maxspeed/255);              // calculate desired time in uS between encoder pulses
    if(actual>lpulse && lpwm>0) lpwm--;                   // if motor is running too fast then decrease PWM
    if(actual<lpulse && lpwm<2549) lpwm++;                // if motor is running too slow then increase PWM
    digitalWrite(rmdirpin,rspeed>0);                      // set direction of right motor
    actual=20000/(abs(rspeed)*maxspeed/255);              // calculate desired time in uS between encoder pulses  
    if(actual>rpulse && rpwm>0) rpwm--;                   // if motor is running too fast then decrease PWM
    if(actual<rpulse && rpwm<2549) rpwm++;                // if motor is running too slow then increase PWM

In my code which is written for a Rover 5, I first wrote a simple piece of code that ran the motors at 100% duty cycle and counted how many state changes I got per second. With a 2S LiPo that was partially discharged (reading almost 7.2V) I got a peak speed of about 590 state changes a second. Since a 2S LiPo can be anywhere fro 6V when flat to 8.4V fully charged I set the variable maxspeed to 520 to ensure the maximum speed could be obtained even when the battery was nearly flat.

Setting this value to less than maximum ensures that the robot can travel in a straight line regardless of differences in the motors or the state of the battery. It also keeps a bit of power in reserve for climbing obstacles.  The down side is that the chassis never reaches it's maximum possible speed. You will need to change this variable if you are using a different chassis to the Rover 5. it must be an unsigned long variable otherwise the processor does not calculate the time correctly.

As the code compares time periods from the encoders to control PWM, if the chassis stops there will be no change which would prevent the motors starting again. To re-start a stalled / stopped motor the code automatically increments the PWM if the encoders do not change states within 50mS.

The desired speed is expressed with the variables lspeed and rspeed for the left and right motors. The variables can be negative numbers to indicate reverse direction. As such the direction control pin is driven high for positive numbers and low for negative numbers. The absolute value is used for calculating the speed.

When the PWM for the motors is incremented or decremented a PWM value of 0 to 2549 is generated. The reason for this was to alow the the motor speeds to be updated quicker without over correction. This value is later divided by ten to give an 8 bit PWM value. As an extra benifit, this method also averages out minor fluctuations for a smoother motor speed.

Note: two lines in the motor control code have been disabled. If you are using an Arduino board where your PWM outputs are not disabled by the Servo library then enable these lines and delete the PWM generation code.


I2C Command: This section accepts commands from the I2C bus master and drives any attached servos. The wire library treats a transmission like an external interrupt so this section of code is called automatically when a data packet is received from the I2C bus master.

The code expects a data packet of 20 bytes. If a transmission is received that is not 20 bytes in length then it is assumed the data is corrupt and the buffer is flushed.

Once a datapacket of 20 bytes is received it is assumed to consist of 10 integers transmitted high byte first. The first two integers are the desired left and right motor speeds. The other 8 integers are for the 8 servos that the Mini Driver can power directly from the battery. If you want to adapt this code for more or less servos then you will need to change this section of the code accordingly.

//========================================================== Interrupt Service Routine for I2C Data Received ================================================

void I2Ccommand(int bytes)
  if(bytes!=20 || Wire.available()!=20)                   // each data packet should consist of 10 integers (20 bytes)
  lspeed=Wire.read()*256+Wire.read();                     // read left  motor speed integer
  rspeed=Wire.read()*256+Wire.read();                     // read right motor speed integer                   
  for(byte i=0;i<8;i++)
    svpos[i]=Wire.read()*256+Wire.read();                 // read servo position integer for 8 servos
    if(svpos[i]>0) sv[i].writeMicroseconds(svpos[i]);     // if servopos is >0 then move servo to that position
    if(svpos[i]<0) sv[i].writeMicroseconds(3000+svpos[i]);// if servopos is <0 then move servo to that position but reverse sense of direction

//========================================================== Empty Buffer of Corrupt Data ===================================================================

void EmptyBuffer()
  while(Wire.available()>0)                               // continue until buffer is empty
    byte b=Wire.read();                                   // read byte from I2C buffer

Servos: All servo positions are expressed in uS. Typically 1500uS should center the servo with a standard servo having a range from 1000uS to 2000uS. In reality different brands of servos work over different ranges so you will need to experiment with your servos to see what is their range of travel.

At the very beginning of the code when global variables are being defined, there are 4x arrays used to specify the control pin, default position, minimum limit and maximum limit of each servo. Setting the minimum and maximum values correctly is important otherwise you can overdrive your servo and burn it out.

The minimum and maximum values provide a safeguard against bugs in your code. Even if the I2C master tries to drive the servos beyond their limits the servo library will limit the control signal to these values to protect the servo. Take the time to determine what is the range of your servos and they will survive much longer while using less battery power.

Previously I have been caught out when I've replaced a bad servo and found the new servo's sense of direction is reversed causing it to rotate clockwise when the old servo would rotate anti-clockwise. The code allows you to correct this by sending a negative value instead of a positive value.












Mini_Driver_Rover_5_and_Servo_control_via_I2C.zip2.62 KB
I2C_Master.zip691 bytes

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

The software poseted here had a glitch because the I2C interfered with the servo functions. The motor control was also a little slow to respond in some cases.

I will post future software updates in the tutorial here:  http://letsmakerobots.com/node/39098


Nice job OddBot! You really managed to make it all work on an old mega8 chip. Incredible. Your code allows me to concentrate on the brains of the robot (senses and thinking), since the motor code is all taken care of. Many thanks, sir!

I gave up on the old ATmega8 since the newer chips (168 and 328) have more PWM pins. They also have Pin Change Interrups, so they can read 4 encoder pins. I'm not sure how to modify the code to do that, it's been a long time since I coded so I would have to brush up my skills. Anyway, I plan to use one of my uBotino boards instead of the Mini. My board, it is hardwired to use Timer0 for motor PWM (pins 5,6 instead of pins 9,10) and pins 7,8 for direction. Since your code uses an 8 bit PWM, that should not be a problem, so I can use the analogWrite. I should also be able to use the analog pins for servos, since they are also digital pins D14-D19. Well, not the last 2 pins, since they are the I2C pins, but D14-D17 should work.

So, on my board I have:

D2, D3, D4, D17 = encoder pins
D9-D16 = servo pins (8 servos, D9-D13 with Vin, D14-D16 with Vcc)
D5-D8 = motor pins
A4, A5 = I2C pins.

That leaves the D0,D1 pins available (hardware serial pins). Cool!

Can't wait to start playing with it!

I'm humbled.  Looks like I'll have to take some lessons.

Extremely efficient, concise, and complete.


Don't be too humbled, I'm still not 100% certain how some of it works.

When I started work on it some things that worked in theory did not work in practice. A few days of trial and error mutated the code. Now it works great but I'm not sure how.

I'm using an Arduino UNO for these functions, and thought I was at the end of *IT's* abilities.... apparently I'm quite wrong.  I just need tighter code.


Truly a work of art, that code of yours.  Thanks again...


I spent the last half hour looking at your code and won't pretend I get it 100%.  This is one where I would need to play with it to really understand.  I didn't know one could simulate PWM like that with micros() and the bit field.  It makes sense one could manage the duty cycle that way, just never thought of it.  I see a couple of minis in my future.

This is the kind of thing which really pushes my embedded skills to that next level where I want to be.  Thanks for posting it. 




I will update the post soon with an explanation of how the code works for those who want to modify it for their own use. Direct port manipulation is not essential but it helps reduce the memory used and speed up the execution of the code.

Nice job OddBot. It seems you are always improving things which seems are not improvable :-)