SOLVED: Devantech SD21 servo contr. and desperate programmer, after one year...

Hi all!

This is almost ridiculous, but I have to ask for help.

Our robot is getting close to a point, where the Madam of the house is ready to consider it as member of a family. Well, new born child is smarter and cuter, but hey, beauty is in the eye of the beholder, right?

Anyway, here’s the problem.
SD21 takes the value for the servo position in to bytes. I have tested that it works both ways, so the mathematic is probably right.

What happens, is that when I increase the value by one on each round, servo jumps MUCH TOO MUCH when the value goes from 1529 to 1530, an daround 1540-1544 it jumps back to where it actually should be. We have to controllers, same thing. Maybe firmware problem, maybe something that I can’t see in this code.

Here is my old faithful test code, this time even shorter, but functioning.

EDIT. Corrected 255 to 256

#include <Wire.h>
#define SD21 0x61 //SD21 I2C Address, shifted by one bit from original 0xC2:sta

byte ServoHigh, ServoLow; 
int Pos_ms=1400;
int ServoNro=0;
int Luku;

void setup()

void loop()

  if (Pos_ms > 1600) 

  ServoHigh   =  byte(Pos_ms / 256);
  ServoLow    =  byte(Pos_ms - ServoHigh);

  Wire.send(0);                 //Speed Servo 0

  Wire.send(1);                 // Servo 0, Low Byte to Register 1

  Wire.send(2);                 // Servo 0, High Byte to Register 2


Electric is ok, Arduino only controls this module through I2C, controller’s power is guarantee. But all servo channels do this.

Problem appears today when we started to fine tune arm positions, step by step incremental with the servo positioning.

As usual, any advice is welcome!


Change 255 to 256

Oh sh...!!!!

Thank you, that was too obvious, and very typical mistakes I do all the time!
And my co-pilot needs to some lecturing too! He is supposed to watch after my coding!!!!

Thank you once more!


It was a good advice, but only thing that changed was the point where the error appears, it is now from 1535 to 1536 moves ahead, and from 1541 to 1542 brings back.

That's not funny... I just need to sleep over the night and try to figure it out again.


I just did a quick glance at first but now I see you are extracting the low and high bytes from Pos_ms. Why not just use the lowbyte and highbyte functions on your Pos_ms integer.


Still learning. Tried shifting 8 bit to right, and it did the same thing.

Now I have tested all points where it goes wrong, and there is a pattern. I just can not see what causes it.
It jumps every time in two pairs. Something really simple?
The first number is right, then from second to third wrong, and right again. After every set the second pair is +1 further.

511  0000 0001 1111 1111
512  0000 0010 0000 0000
513  0000 0010 0000 0001
514  0000 0010 0000 0010

767  0000 0010 1111 1111
768  0000 0011 0000 0000
770  0000 0011 0000 0010
771  0000 0011 0000 0011

1023 0000 0011 1111 1111
1024 0000 0100 0000 0000
1027 0000 0100 0000 0011
1028 0000 0100 0000 0100

1279 0000 0100 1111 1111
1280 0000 0101 0000 0000
1284 0000 0101 0000 0100
1285 0000 0101 0000 0101

1535 0000 0101 1111 1111
1536 0000 0110 0000 0000
1541 0000 0110 0000 0101
1542 0000 0110 0000 0110

1791 0000 0110 1111 1111
1792 0000 0111 0000 0000
1798 0000 0111 0000 0110
1799 0000 0111 0000 0111

2047 0000 0111 1111 1111
2048 0000 1000 0000 0000
2055 0000 1000 0000 0111
2056 0000 1000 0000 1000

2303 0000 1000 1111 1111
2304 0000 1001 0000 0000
2312 0000 1001 0000 1000
2313 0000 1001 0000 1001



That did it. No I can finally go to sleep.
My math sucks...

  ServoHigh   =  highByte(Pos_ms);
  ServoLow    =  lowByte(Pos_ms);

Thanks man!


After night everything is clear(er)…

That original code was from one year back, when I had even more problems with coding C. I haven’t altered the code, so I have had the problem from the beginning.
We are starting to rewrite some of the code again, just to see what we didn’t see in the beginning. From VB-programmer to C-programmer, it’s not a long way if you think logically, but syntax is like hell to learn.

Original code had two mistakes, at least, but in this part

 ServoHigh   =  byte(Pos_ms / 255);
 ServoLow    =  byte(Pos_ms - ServoHigh);

Should be:

 ServoHigh   =  byte(Pos_ms / 256);
 ServoLow    =  byte(Pos_ms - (ServoHigh*256));

But nobody does it like this, I just did for some reason, can’t remember what was in my thoughts a year back. Must have been terrible hurry to just do something that moves… well, no more.

Time to try find more flaws from three different codes that works with each other, I2C communication and home brewed “protocol”, PITA.


Just this solved an issue for me too, thanks.

Just this solved an issue for me too, thanks.

Weel, that is great! What exactly was your problem?
We've had so many brain farts so far, I can't even remember all stupid mistakes found and corrected, by try and learn method.


I was having issues with the high/low byte as well, turned out I found the worlds worst example of interfacing the SD21 and Arduino.
If I could remember where I found it I would post a warning. This post and quick test program had my servos working perfectly, in about 15 minutes, well except for speed but I can figure that out.

I am trying to build a graceful start and end leg positioning for a hexapod, basically an unfold from storage to start and a refold to finish.

Servo speed is a key component, as well as battery life to know when to shutdown once it is autonomous.

Have you been playing with SD21 a lot? We seem to have some problems with getting
absolute positions for servos, one day middle position is not where it was yesterday.

We have to recalibrate some servos every now and then to get them where they belong
at the start, to get safe combination for the joints of the robots arms. One thing that
is still to do, is the change of the source voltage.

If you experience any similar symptoms, it would great if you tell us about that.
I don’t know how widely this board is used, but it’s quite hard to believe that the problem
lies in it.


I have not noticed that so far, but I will pay attention. For power I am using a 7.4 Volt 2500 Lipo, with a 15c discharge rate.

But you can have 2 different issues the servos themselves, poorly made or cheap servos do not always get to the same point every time.

If it is power, you should be able to isolate it by raising your bot, removing all but 1 leg, actually remove the servos from the board. Run a few cycles and see where you end up. If all is good keep adding legs back, until things start going off, at that point isolate just that leg.

If it all works ok in isolation then you have a power issue.

It is a pain in the but though.

We tested it with single servo only, and the position didn't change with less power consumption. We have probably 5 kind of different servos at this moment used, quite cheap models all.

When our big picture is ready, we start to check the problem, now our hands are full with combining all four arduinos to work with each other.

I promise to tell if any progress happens with SD21.


Just a small up date, I have a little routine that pops all 6 legs of my hexapod to neutral positions, works fine in it’s own little sketch.

Move it over to a larger sketch to be used as servo initialization it it does not always function correctly, similar to your issue with servos not returning to the exact same point. I have a little more debugging to do, but I will keep this updated.

Very valuable information! Thanks!



Well after blowing up a couple of servos due to a bad battery, I have built some simple code to mimic a walk with 3 legs on my Hexapod (new Servos are on the way). The walk is a simple 3 legged sweep forward then back, in a 20 count loop. All was I looking for was SD21 function and to see if I had any major position issues. At 6 volts positioning was not always good, at 5 Volts much better.

I did notice on my servos they got a little louder as the loop progressed, this is most likely a result of servo quality. It seems to be only a couple of servos so no big issue there.

The Pod is sitting on a paint can so no weight on the servos at the moment.

In the process I discovered a few things that you may l already know, but just in case.

The cheap servos are very voltage fussy, I found if the voltage was not steady and within the middle of the range, the positioning was quite poor. To clean up my voltage I installed a BEC, Battery Eliminator Circuit. In the R/C flight world it allows you to eliminate the battery for the receiver by supplying a steady 5 volts from your primary battery, and is ideal for small robots using this board.

My current BEC is only 10 Amps, but I have a 20 Amp on on the way. This allows me to provide a steady 5 Volts to the Servos, the 20 Amp is adjustable for output voltage. The down side is you need a fan, these generate a fair bit of heat under load and are designed to have airflow over them. I have a little 5 volt computer fan I run off servo connector 21, it is about an inch square and generates enough airflow to keep things cool.

I found by using the BEC, I both protect my servos and seem to get better positioning, the power supplied to the servos side is only running the servos. Currently I use a 5 volt run from my bread board to power the logic side. It is just a little simpler for debugging. But it may also assist in keeping things cleaner. When the bigger BEC comes I am going to running them both off the main battery and see if there is any difference.

I also found better positioning by slowing down the servo movement, I pass a speed, currently 12 (randomly selected). Although it is a bit slow the servos seem to come back the same position. At least they have not knocked over my endpoint sticks a small straw stood up :).
I need to play with that value.

Overall I am finding the SD21 does the job, but information is hard to come by as you noted it does not seem very popular.

My final positioning tests will happen about week from now, I have a small FSR and have come up with a rig I can position under a leg. In theory if the leg returns to the same position each time the FSR should have about the same reading. I figure about 100 iterations at a variety of speeds should test the servo positioning pretty well and give me a set of benchmarks.

I use a Castle Creations BEC you can find some info here I happen to be familiar with them, there are likely similar solutions in the robotics world.

I also switched to a different I2C library, primarily for better error control but it has some, at least to me nice features.

Our project is not that heavy duty, we are trying to maximize the 12V 2500mAH battery life by cutting the power from motors and servos when they are not needed. Silent and bright, LED's are blinking and NRF24L01 is powered all the time.

If ever I start project with unlimited power supply, that BEC is on the list.


A little tidbit from Devantech if you are running version 7 of the firmware you can save your servo start positions

you write 0x5A to register 126 (dec)/7E (hex)

I use a different Ic2 library but I included an example here of my servo save function. I have a little function that checks for the SD21 and returns the address, firmware version and battery voltage. The voltage is a little useless as I use a BEC but it does confirm the BEC is functioning correctly.

Code notes:

The Address of the SD 21 is set globally
The delay is there to give the communication time to complete,
it is a debugging and testing setting only.

// Firmware Version 7 allows saving servo info

void Save_servo()
 // we write 0x5A to register 126 Decimal (7E hex)
 // fixed value
  err_c = I2c.write(SD21,0x7E);  // set the register pointer 
  if (err_c != 0)
      Serial.println("  Save_Servo - Error setting Servo pointer");

 err_c = I2c.write(SD21,0x7E,0x5A);
  if (err_c != 0)
      Serial.println("   Save_Servo - Error Saving Servo setting");
 delay (10);

i am doing a project...i am using arduino and devantech sd21 to control a hexapod spider... can someone show me a sample code.. that works ? or can give me any usefull advice of how to.