Saturday, 12 April 2014

Heaxpod (Pt 3)

The wireless PS2 controller and 32ch servo driver board arrived, and has proven to be a minor challenge to work with.

For the last couple of days, the Torobot site has been down, possibly patching for the SSL HeartBleed vulnerability, and quite clearly having a few issues..just been there and their backend database just spilled it's contents onscreen for me.  Since I'm an ethical hacker, I'll just leave them to it. :-)

However, it has given me an updated manual for the controller/servo board and also access to the command software used to create actions.

What has been really interesting is the stuff I have found regarding sending serial data to the servo board from an Arduino Uno, receiving command data from the PS2 controller over wifi!  The servo controller uses PWM, not the degree values I have been used to with the servo library.  As it turns out, it's reasonably easy, once you know the PWM upper and lower limits for the servos.

In my case, I'm using TowerPro MG996R servos for all joints.  With some messing around (trial and error), I have a PWM range of 800 to 2500 microseconds(us).  This gives me the full 180deg sweep from 0 to 180.

I'm a bit new to this, and I got un-invited to 6th Form high school so the maths skills aren't that great, but the guts of it is:

If 0deg is 800us and 180 is 2500, then midpoint is ((2500-800)/2)+800 = 1650.
It also goes that 180 deg / 1700us (PWM range 2500-800)  = 0.105 deg per microsecond.
Therefore 1 deg = 9.52us  (1 / 0.105)

So if I want to send the servo to 40 deg, then (40deg / 0.105us) + 800 = 1180.95us

Now, they aren't the most accurate servos, and when it reaches the edges of the limits, it can overshoot.  These things are capable of 10kg/cm torque, which is enough to split the casing if the servo reckons it can go past the physical stop pin.  To counter that, I knock about 5 deg off each end of the sweep, giving me a range of 5 to 175.

Now I have a PWM range of 850 to 2450, with midpoint still at 1650..cos all I did was trim 50us off each end.

Still with me then? :-)

Right....Arduino has a really cool function named "map()" which makes it an absolute doddle to translate the values coming from the PS2 controller to PWM values.  It goes like this...

map(value to change, min value it will be, max value it will be, min value to assign, max value to assign), 0, 100, 0, 1000) will return 500...because 500 is the same to 0-1000 as 50 is to 0-100), 0, 255, 850, 2450) will return the value 1650...because 1650 is to the range 850 - 2450 as 127 is to the range 0 - 255...which is really handy because the PS2 controller joysticks produce values ranging from 0 - 255 in their full left to full right range of movement...with 127 being the sticks neutral point.

The Torobot board uses a serial data string format of "#[servo port]P[PWM value]T[Time in ms]"
To send servo on port 1 to the midpoint => "#1P1650T500" mash it all together:

Connect the PS2 wifi receiver to the Uno with some female/male jumpers.  The receiver pinouts are(left to right, looking into the socket end with the D shape smiling at you):

  1. Data -> UNO pin 12 (NOTE: Must use a 10k resistor for pullup..else the signal is all squiffy and won't work)
  2. Command -> UNO pin 11
  3. Not used
  4. Earth/ground -> Have a guess..
  5. 3.3v in -> You got it...
  6. Attention -> UNO pin 10
  7. Clock -> UNO pin 13
  8. Not Used
  9. Not Used
Connect the UNO to the Servo Controller board:
  • UNO TX -> RX0 (next to the mini USB socket, bank of 3x2 header pins ahead of "S1")
  • UNO RX -> TX0
  • (Screw block)
    • Gnd -> ..well...ground!
    • VS - Voltage supply in for the servo main power..this does not power the board itself
    • VSS -> 5v from UNO for powering the board processor.  Can power from the USB and has voltage protection so you don't zap back up the USB cable
Download and import the PS2 controller Arduino Library from Bill Porter ( shout out to him and the awesome work he's done on this.

Connect your servos to the header pins...dealers choice which ones you use.

Here's my test sketch for one robot arm (3 servos), and has been vastly trimmed just for testing.
Note that you can move mulitple servos with a single line...just add the string with "#[servo number]P[PWM value]" prior to the "T" value and the final "println"  The servo controller reacts when it gets a newline character.

[code starts here]

#include <PS2X_lib.h>  //for v1.6

PS2X ps2x; // create PS2 Controller Class

//right now, the library does NOT support hot pluggable controllers, meaning 
//you must always either restart your Arduino after you conect the controller, 
//or call config_gamepad(pins) again after connecting the controller.
int error = 0; 
byte type = 0;
byte vibrate = 0;

void setup(){

 error = ps2x.config_gamepad(13,11,10,12, true, true);   //setup pins and settings:  GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error

void loop(){
  //DualShock Controller

  ps2x.read_gamepad(false, vibrate);          //read controller and set large motor to spin at 'vibrate' speed

    Serial.print(map(ps2x.Analog(PSS_LY),0,255,900,2400));     // Read the left joystick values
    Serial.println("T500");    //Send the string off to the controller board

[End code]

You may have seen the min/max for servo 3 is around the other way?..You can swap them to actually change the servo sweep direction.  This servo is for the shin section of the leg, and by swapping the values you can get it to move "leg cramping curling up" style..or "Mr Miyagi whoopass crane" style.

I have found that if you don't disconnect the power to the servo board (the 5v and/or the USB cable), the sketch won't upload to the arduino.  I'm not sure why, but I think it gets messed up with the serial data responses from the servo board?..anyone want to chip in here?

Well...after all that, I've got remote control of a hexapod leg with a wireless PS2 controller joystick.  The photo is a couple of the legs set up in a test rig to see how well the legs move and test range and servo jitter.

Just for $hits and giggles I moved the rig so it was resting in the middle of the table, then moved the leg in a "stand up" type of movement.  Impressed with those servos..because the rig is being held in place with a 3kg dive weight..and the leg tipped it off without any bother at all.

I am encouraged! :-)

I'll upload a video of the test when I can borrow the camera (again!).

Video of the abovementioned rig in action..

See ya next post.


  1. Replies
    1. Not really sure what you are asking. The Arduino sketch is in the above blog between the "[code starts here]" and "[End code]", and you have to use it with the PS2 Controller library from also mentioned above.

  2. Hi there

    Thank you very much for your blog.
    Can you please update the arduino code in which you you used "buttons" (not stick) to control you bot.( i saw that in your video)


  3. Hi Aditya,

    All I'm doing is every loop check if the L1 or L2 button is being pressed. If yes, then change the value of an int variable by 5 (meaning increment it up or down by 5 degrees)

    When the joystick is in the neutral position, its value is 127 (having a range of 0 to 255)...and that value gets sent to the servo controller.

    So...every loop, check:

    if (ps2x.Button(PSB_L2)) {
    myOffset = myOffset + 5;
    if ((ps2x.Button(PSB_L1)){
    myOffset = myOffset - 5;

    Then update the servo position with
    joystick + offset

    Now, you should also limit the final value sent to the servo controller with the 'constrain()' function to ensure you don't overshoot the servo. It would be possible to press the L1/L2 button a bunch of times, creating an offset that would want to send a physically impossible position value, eg...(joystick = 127) + (myOffset = -283) = -156 degrees, which of course is an entirely invalid angle for a 180 deg servo.
    The 'constrain()' function will keep the final within a valid range.

    I don't generally give my code for 2 reasons..
    I'm not a programmer, and I tend to be a bit 'hackish' with it to make it work, and it's more of a face saving thing trying not to draw too much attention from proper coders :-)


    1. Hi SH

      Thank You for the quick reply.
      I did the same thing before but it wasn't working ,will try it again.

      Thank you once again.
      Cheers B-)

  4. OK...full sketch is...

    [code starts here]

    #include //for v1.6

    PS2X ps2x; // create PS2 Controller Class

    int error = 0;
    byte type = 0;
    byte vibrate = 0;

    int L2S_offset = -13;
    int L_ElbOffset = 0;
    int R_ElbOffset = 0;

    void setup(){

    error = ps2x.config_gamepad(13,11,10,12, true, true); //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error


    void loop(){
    //DualShock Controller

    ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
    if (ps2x.NewButtonState()) //will be TRUE if any button changes state (on to off, or off to on)
    L_ElbOffset = L_ElbOffset + 15; //Change the offset by 15 every time the button is pressed
    L_ElbOffset = L_ElbOffset - 15;

    // Leg 1
    Serial.print("#1P"); //Port 1 is the shoulder pivot
    Serial.print("#2P"); //Port 2 is the shoulder joint
    Serial.print("#3P"); //Port 3 is the elbow
    Serial.print(map(ps2x.Analog(PSS_LX) + L_ElbOffset,0,255,900,2400));

    // Leg 2
    Serial.print(map(ps2x.Analog(PSS_LX) + L_ElbOffset,0,255,2400,900));

    [end of code]

    I haven't implemented the 'constrain()' function in this code..I found out about it after and have yet to get back into the coding after messing with the CNC rebuild.


  5. Ooops...the blog formatting trimmed that 'include' line..
    Should read : #include "PS2X_lib.h"

  6. Hello , I'm with the problems when connecting to Arduino joystick , it would be possible you send me a picture of the link , Thank you,

  7. This comment has been removed by the author.

    1. This comment has been removed by the author.

    please send me the fritzig diagram..i am doing hexapod project now for the requirements of my studies..
    can you help me also for the gaits sir? do you have the full codes of hexapod ? please sir.. i really need a help

    ...please excuse my english..

    thank you

  9. already finished the project hexapod?