Monthly Archives: June 2013

Intro to Car Stuff

I’m gearing up for an autocross and a track day in the same week, so I figure its a great time to talk car stuff. I started wrenching on cars with my Dad as a kid and have been into it ever since. I caught the motorsport bug with my first autocross when I was in college and that really fueled the fire.

When I think about it, I’m not into fancy cars or super cars. I’m not into old cars or show cars. What I’m into is figuring how the cars i have work and how to make them better. The test bed for these improvements is then autocross, track days, time trials, and hopefully someday club racing.

In the past I’ve had a few projects, an ’04 VW R32, ’97 BMW M3, ’98 BMW 540i/6…

2004 R32

2004 VW R32

1997 BMW M3

1997 BMW M3

Probably my favorite was the 1987 BMW 325is that i built to SpecE30 specs as a dedicated track car. Simple and easy to work on, great community, and a blast to drive. Oddly, I can’t find any good pics of this car… guess I was too busy driving.

1987 BMW 325is

1987 BMW 325is

Things changed a little a few years ago when I moved to Boston in 2009. I live in the city and have to be pretty grateful for even the driveway i have. No garage, no space for projects. 🙁

I needed a car that could wear a lot of hats. My commute is up to 2 hours a day and I take lots of road trips so I wanted something comfy, nice, good stereo, etc. Since I didn’t have easy access to a garage or tools it needed to be pretty reliable, preferably with a warranty. AND it had to be fairly sporty, fun to drive, and capable of the occasional autocross and track. I decided to buy a new 2010 Volkswagen GTI and its been making me very happy ever since.

On to the Project GTI!

Phase 3: Body Kinematics, i.e., Body Translation/Rotation with the Feet Planted

The next thing I wanted to accomplish was being able to translate and rotate the body while keeping the feet planted. For now this will simple enable the robot to wiggle around and “dance”, but eventually these movements will be necessary inputs for traversing uneven terrain.

The concept for this is pretty straightforward. If i want the body to translate right while keeping the feet planted, you simply move all the feet to the left relative to the body:

newFootPositionX = initialFootPositionX - translateX
newFootPositionY = initialFootPositionY - translateY
newFootPositionZ = initialFootPositionZ - translateZ

I’m subtracting the translation because the feet are moving opposite the direction i want the body to go in. ‘translateY’ could be an input from your controller. I using the Bioloid Commander library for easy access to the Commander controller inputs.

Rotations aren’t quite as simple. The concept is to take an angular input, and provide the X,Y,Z offsets necessary for each leg to create that body rotation. A perfect job for the rotation matrix!

Rotation Matrices

Here is some good background info on rotation matrices. Its not nearly as hard as it looks. You simply apply one rotation matrix per axis of rotation. I wanted to be able to rotate on all three axis so i strung all three together.

(X,Y,Z)rotated = (X,Y,Z)initial*Rx*Ry*Rz

The angle pheta in each matrix is the input rotation angle for that axis. You do need some sort of software to crunch this up into closed form equations. I used Matlab which can do the symbolic matrix multiplication. Once I got my equations i was able to calculate my (X,Y,Z)rotated offsets and add them into the equations above to get:

newFootPositionX = initialFootPositionX - translateX - rotatedX
newFootPositionY = initialFootPositionY - translateY - rotatedY
newFootPositionZ = initialFootPositionZ - translateZ - rotatedZ

This *almost* works worked perfectly. The issue is that my corner legs were behaving funny. That i realized was because my math wasn’t taking into account their mounting angle.  Basically each corner points 45 degrees out. I need to rotate all my coordinates for these legs and there’s no better tool than… the rotation matrix! This time its easier because this is a single axis rotation so there is only one matrix, Rz.

(X,Y,Z)coxaCorrected = (X,Y,Z)newFootPosition*Rz

Note that the 45deg input is the angle for the front right leg. The right rear leg would be 135deg, the left rear would be 225deg, and the upper left would be 315deg as you go around the clock.

Here’s the code:

/**********************************************************************************************************
    fodyFK()
    Calculates necessary foot position (leg space) to acheive commanded body rotations, translations, and gait inputs
***********************************************************************************************************/
void bodyFK(){

    float sinRotX, cosRotX, sinRotY, cosRotY, sinRotZ, cosRotZ;
    int totalX, totalY, totalZ;
    int tempFootPosX[6], tempFootPosY[6], tempFootPosZ[6];
    int bodyRotOffsetX[6], bodyRotOffsetY[6], bodyRotOffsetZ[6];

    sinRotX = sin(radians(-commanderInput.bodyRotX));
    cosRotX = cos(radians(-commanderInput.bodyRotX));
    sinRotY = sin(radians(-commanderInput.bodyRotY));
    cosRotY = cos(radians(-commanderInput.bodyRotY));
    sinRotZ = sin(radians(-commanderInput.bodyRotZ));
    cosRotZ = cos(radians(-commanderInput.bodyRotZ));

    for( int legNum=0; legNum<6; legNum++){

        totalX = leg[legNum].initialFootPos.x + leg[legNum].legBasePos.x;
        totalY = leg[legNum].initialFootPos.y + leg[legNum].legBasePos.y;
        totalZ = leg[legNum].initialFootPos.z + leg[legNum].legBasePos.z;

        bodyRotOffsetX[legNum] = round(( totalY*cosRotY*sinRotZ + totalY*cosRotZ*sinRotX*sinRotY + totalX*cosRotZ*cosRotY - totalX*sinRotZ*sinRotX*sinRotY - totalZ*cosRotX*sinRotY) - totalX);
        bodyRotOffsetY[legNum] = round(  totalY*cosRotX*cosRotZ - totalX*cosRotX*sinRotZ         + totalZ*sinRotX         - totalY);
        bodyRotOffsetZ[legNum] = round(( totalY*sinRotZ*sinRotY - totalY*cosRotZ*cosRotY*sinRotX + totalX*cosRotZ*sinRotY + totalX*cosRotY*sinRotZ*sinRotX + totalZ*cosRotX*cosRotY) - totalZ);

        // Calculated foot positions to acheive xlation/rotation input. Not coxa mounting angle corrected
        tempFootPosX[legNum] = leg[legNum].initialFootPos.x + bodyRotOffsetX[legNum] - commanderInput.bodyTransX + leg[legNum].footPos.x;
        tempFootPosY[legNum] = leg[legNum].initialFootPos.y + bodyRotOffsetY[legNum] - commanderInput.bodyTransY + leg[legNum].footPos.y;
        tempFootPosZ[legNum] = leg[legNum].initialFootPos.z + bodyRotOffsetZ[legNum] - commanderInput.bodyTransZ + leg[legNum].footPos.z;
    }

    // Rotates X,Y about coxa to compensate for coxa mounting angles.
    leg[0].footPosCalc.x = round( tempFootPosY[0]*cos(radians(COXA_ANGLE))   - tempFootPosX[0]*sin(radians(COXA_ANGLE)) );
    leg[0].footPosCalc.y = round( tempFootPosY[0]*sin(radians(COXA_ANGLE))   + tempFootPosX[0]*cos(radians(COXA_ANGLE)) );
    leg[0].footPosCalc.z =        tempFootPosZ[0];
    leg[1].footPosCalc.x = round( tempFootPosY[1]*cos(radians(COXA_ANGLE*2)) - tempFootPosX[1]*sin(radians(COXA_ANGLE*2)) );
    leg[1].footPosCalc.y = round( tempFootPosY[1]*sin(radians(COXA_ANGLE*2)) + tempFootPosX[1]*cos(radians(COXA_ANGLE*2)) );
    leg[1].footPosCalc.z =        tempFootPosZ[1];
    leg[2].footPosCalc.x = round( tempFootPosY[2]*cos(radians(COXA_ANGLE*3)) - tempFootPosX[2]*sin(radians(COXA_ANGLE*3)) );
    leg[2].footPosCalc.y = round( tempFootPosY[2]*sin(radians(COXA_ANGLE*3)) + tempFootPosX[2]*cos(radians(COXA_ANGLE*3)) );
    leg[2].footPosCalc.z =        tempFootPosZ[2];
    leg[3].footPosCalc.x = round( tempFootPosY[3]*cos(radians(COXA_ANGLE*5)) - tempFootPosX[3]*sin(radians(COXA_ANGLE*5)) );
    leg[3].footPosCalc.y = round( tempFootPosY[3]*sin(radians(COXA_ANGLE*5)) + tempFootPosX[3]*cos(radians(COXA_ANGLE*5)) );
    leg[3].footPosCalc.z =        tempFootPosZ[3];
    leg[4].footPosCalc.x = round( tempFootPosY[4]*cos(radians(COXA_ANGLE*6)) - tempFootPosX[4]*sin(radians(COXA_ANGLE*6)) );
    leg[4].footPosCalc.y = round( tempFootPosY[4]*sin(radians(COXA_ANGLE*6)) + tempFootPosX[4]*cos(radians(COXA_ANGLE*6)) );
    leg[4].footPosCalc.z =        tempFootPosZ[4];
    leg[5].footPosCalc.x = round( tempFootPosY[5]*cos(radians(COXA_ANGLE*7)) - tempFootPosX[5]*sin(radians(COXA_ANGLE*7)) );
    leg[5].footPosCalc.y = round( tempFootPosY[5]*sin(radians(COXA_ANGLE*7)) + tempFootPosX[5]*cos(radians(COXA_ANGLE*7)) );
    leg[5].footPosCalc.z =        tempFootPosZ[5];

}

NOW it works!

<insert video here>

Head back to the beginning of Project B.E.T.H.

Phase 2: Leg Inverse Kinematics, i.e., Pointing a Foot in 3D Space

Inverse Kinematics (IK) is a really handy tool used when controlling moving linkages. It’s used whenever you want to move a series of linkages by locating the end of the linkages instead of having to worry about all the joint angles in between. For example, your brain does this when you want to grab a cup of coffee. You don’t think “shoulder angle to X, elbow angle to Y, wrist angle to Z”. You think “hand to coffee mug” and your brain takes care of all the angles necessary to achieve the hand goal position. IK is commonly used not only in robotics, but also in 2D and 3D animation, for example.

Like I mentioned in my intro post, I first worked on some leg IK a few  years ago. To get started, i dug up my old code. The method I used was a bit overkill and involved using rotation matrices rotating coordinate spaces defined for each joint in the leg. It basically involves setting up a bunch of matrices in a certain form and then crunching them up into closed form equations. It ended up being too processor intensive and took like 17ms to run. That’s out of my 33ms frame budget! There’s a lot more crunching ahead so that’s just too much. I decided to take a simpler approach which is possible with our simple 3DOF legs. You can figured it all out with some basic trig.

One great resource I found is Oscar Liang’s blog. His tutorial on solving 3DOF IK using trig is very clear and has better diagrams than i can draw so take a look at it here.

Going through his tutorial I was most of the way home. The final step was to add in angular offsets for my physical hardware. Because the Tibia servo is mounted on an offset bracket off the Femur, I added in that angle. And then because the tibia itself is mounted straight, i added in 90deg so that it pointing downward is centered. Finally the tibia itself has a bend to it so i added an offset for that as well.

My final function looks like:

/**************************************************************************************************************
legIK()
Calculates the coxa, femur, and tibia angles for input foot position in X,Y,Z.
***************************************************************************************************************/
void legIK(){

float CoxaFootDist, IKSW, IKA1, IKA2, tibAngle;

for( int legNum=0; legNum<6; legNum++ ){ CoxaFootDist = sqrt( sq(leg[legNum].footPosCalc.y) + sq(leg[legNum].footPosCalc.x) ); IKSW = sqrt( sq(CoxaFootDist-LENGTH_COXA) + sq(leg[legNum].footPosCalc.z) ); IKA1 = atan2( (CoxaFootDist - LENGTH_COXA) , leg[legNum].footPosCalc.z ); IKA2 = acos( (sq(LENGTH_TIBIA) - sq(LENGTH_FEMUR) - sq(IKSW) ) / (-2*IKSW*LENGTH_FEMUR) ); tibAngle = acos( (sq(IKSW) - sq(LENGTH_TIBIA) - sq(LENGTH_FEMUR)) / (-2*LENGTH_FEMUR*LENGTH_TIBIA) ); leg[legNum].jointAngles.coxa = 90 - degrees( atan2( leg[legNum].footPosCalc.y , leg[legNum].footPosCalc.x) ); leg[legNum].jointAngles.femur = 90 - degrees( IKA1 + IKA2 ); leg[legNum].jointAngles.tibia = 90 - degrees( tibAngle ); } // Applies necessary corrections to servo angles to account for hardware for( int legNum=0; legNum<3; legNum++ ){ leg[legNum].jointAngles.coxa = leg[legNum].jointAngles.coxa; leg[legNum].jointAngles.femur = leg[legNum].jointAngles.femur - 13.58; // accounts for offset servo bracket on femur leg[legNum].jointAngles.tibia = leg[legNum].jointAngles.tibia - 48.70 + 13.58 + 90; //counters offset servo bracket on femur, accounts for 90deg mounting, and bend of tibia } for( int legNum=3; legNum<6; legNum++ ){ leg[legNum].jointAngles.coxa = leg[legNum].jointAngles.coxa; leg[legNum].jointAngles.femur = -(leg[legNum].jointAngles.femur - 13.58); leg[legNum].jointAngles.tibia = -(leg[legNum].jointAngles.tibia - 48.70 + 13.58 + 90); } }[/cpp] On to Phase 3: Body Kinematics, i.e., Body Translation/Rotation with the Feet Planted!

Or head back to the beginning of Project B.E.T.H.

Phase 1: Driving Servos

Robotis Dynamixel AX-12A

The first order of business is making the servos move.

First, I found the handy Dynamixel AX-12A datasheet. A slimmed down version is here. These describe the serial interface, the instruction set, and how to create an instruction packet.

Some key points are that the servos run daisy chained on a half duplex serial bus. This means that on your UART hardware, you need to connect the RX and TX lines. This is easily done on the Arbotix Robocontroller with a jumper. It also says that the max communication speed is 1Mbps and provides details on the format of the instruction packet.

Honestly, this part isn’t especially thrilling to me, so i borrowed the ax12 library from the Arbotix bioloid library. This library includes handy low level functions like the basic data write to the bus. I downloaded the full Arbotix libraries here. I copied the ax12.cpp and ax12.h files in the arbotix\libraries\Bioloid directory into my Arduino sketch. This way i could modify them without affecting the public library.

I got pretty far using just these two files. I first initialized the servo serial interface by calling ‘ax12Init(1000000);’ in my Setup() loop. This initializes the serial port and sets it to the maximum 1Mbps baud rate. Full throttle or bust I say.

To command a servo to a position, I simply call ‘SetPosition(id, pos);’ from the library. ID is the ID of my servo and POS is the commanded position. Here’s how the AX-12 maps position:

AX-12A Position Orientation

Note that it’s a little strange how the Position is oriented on the servo. Position ranges from 0 to 1024 which corresponds to angle 0 to 300. There is a 60 degree dead space at the bottom of the servo and Positon 0 starts 30degrees off from where you’d expect to start. I designed my hex so that 0 degrees was pointing straight up and the servo could angle +/-  CW or CCW from there. The code to convert my angle system to Dynamix positioin looks like:

servoPosition  = round((abs( anglePosition  - 210) - 60 )  / 0.293);

Where ‘anglePosition’ is the angle I want using my orientation/nomenclature and ‘servoPositoin’ is the corresponding position in AX-12 speak.

But wait, there’s more!

So using the using the above SetPosition function from the ax12 library, I’m sending one command instruction per servo. This is slow and just generally ugly when there is slick ‘SYNC WRITE’ function in the AX-12 instruction set. The only problem is that there isn’t such a function in the ax12 library to use. But hey, I’m supposed to be doing this all myself right?

Page 23 of the datasheet explains the SYNC WRITE instruction and how to set it up. Basically with one instruction, you can command a whole range of servos, each to their own Goal Position. Nice and fast and neat. Here’s what my rather ugly function looks like:

/****************************************************************
    ax12SyncWriteServos()
    Writes positions to all servos at once using sync write
*****************************************************************/
void ax12SyncWriteServos(){

    int length = (2 + 1)* NUM_SERVOS + 4;   // (L + 1) * N + 4 (L: Data length for each Dynamixel actuator, N: The number of Dynamixel actuators)
    int checksum = 254 + length + AX_SYNC_WRITE + 2 + AX_GOAL_POSITION_L;
    setTXall();
    ax12write(0xFF);                // Start bits
    ax12write(0xFF);                // Start bits
    ax12write(0xFE);                // Broadcast ID for all servos
    ax12write(length);
    ax12write(AX_SYNC_WRITE);       // Instruction
    ax12write(AX_GOAL_POSITION_L);  // Parameter 1: Starting address of the location where the data is to be written
    ax12write(2);                   // Parameter 2: The length of the data to be written (L)

    //RIGHT FRONT
    checksum += RF_COXA_ID + (leg[RIGHT_FRONT].servoPos.coxa & 0xff) + (leg[RIGHT_FRONT].servoPos.coxa >> 8);
    ax12write(RF_COXA_ID);
    ax12write(leg[RIGHT_FRONT].servoPos.coxa & 0xff);
    ax12write(leg[RIGHT_FRONT].servoPos.coxa >> 8);
    checksum += RF_FEMUR_ID + (leg[RIGHT_FRONT].servoPos.femur & 0xff) + (leg[RIGHT_FRONT].servoPos.femur >> 8);
    ax12write(RF_FEMUR_ID);
    ax12write(leg[RIGHT_FRONT].servoPos.femur & 0xff);
    ax12write(leg[RIGHT_FRONT].servoPos.femur >> 8);
    checksum += RF_TIBIA_ID + (leg[RIGHT_FRONT].servoPos.tibia & 0xff) + (leg[RIGHT_FRONT].servoPos.tibia >> 8);
    ax12write(RF_TIBIA_ID);
    ax12write(leg[RIGHT_FRONT].servoPos.tibia & 0xff);
    ax12write(leg[RIGHT_FRONT].servoPos.tibia >> 8);

    //RIGHT MIDDLE
    checksum += RM_COXA_ID + (leg[RIGHT_MIDDLE].servoPos.coxa & 0xff) + (leg[RIGHT_MIDDLE].servoPos.coxa >> 8);
    ax12write(RM_COXA_ID);
    ax12write(leg[RIGHT_MIDDLE].servoPos.coxa & 0xff);
    ax12write(leg[RIGHT_MIDDLE].servoPos.coxa >> 8);
    checksum += RM_FEMUR_ID + (leg[RIGHT_MIDDLE].servoPos.femur & 0xff) + (leg[RIGHT_MIDDLE].servoPos.femur >> 8);
    ax12write(RM_FEMUR_ID);
    ax12write(leg[RIGHT_MIDDLE].servoPos.femur & 0xff);
    ax12write(leg[RIGHT_MIDDLE].servoPos.femur >> 8);
    checksum += RM_TIBIA_ID + (leg[RIGHT_MIDDLE].servoPos.tibia & 0xff) + (leg[RIGHT_MIDDLE].servoPos.tibia >> 8);
    ax12write(RM_TIBIA_ID);
    ax12write(leg[RIGHT_MIDDLE].servoPos.tibia & 0xff);
    ax12write(leg[RIGHT_MIDDLE].servoPos.tibia >> 8);

    //RIGHT REAR
    checksum += RR_COXA_ID + (leg[RIGHT_REAR].servoPos.coxa & 0xff) + (leg[RIGHT_REAR].servoPos.coxa >> 8);
    ax12write(RR_COXA_ID);
    ax12write(leg[RIGHT_REAR].servoPos.coxa & 0xff);
    ax12write(leg[RIGHT_REAR].servoPos.coxa >> 8);
    checksum += RR_FEMUR_ID + (leg[RIGHT_REAR].servoPos.femur & 0xff) + (leg[RIGHT_REAR].servoPos.femur >> 8);
    ax12write(RR_FEMUR_ID);
    ax12write(leg[RIGHT_REAR].servoPos.femur & 0xff);
    ax12write(leg[RIGHT_REAR].servoPos.femur >> 8);
    checksum += RR_TIBIA_ID + (leg[RIGHT_REAR].servoPos.tibia & 0xff) + (leg[RIGHT_REAR].servoPos.tibia >> 8);
    ax12write(RR_TIBIA_ID);
    ax12write(leg[RIGHT_REAR].servoPos.tibia & 0xff);
    ax12write(leg[RIGHT_REAR].servoPos.tibia >> 8);

    //LEFT REAR
    checksum += LR_COXA_ID + (leg[LEFT_REAR].servoPos.coxa & 0xff) + (leg[LEFT_REAR].servoPos.coxa >> 8);
    ax12write(LR_COXA_ID);
    ax12write(leg[LEFT_REAR].servoPos.coxa & 0xff);
    ax12write(leg[LEFT_REAR].servoPos.coxa >> 8);
    checksum += LR_FEMUR_ID + (leg[LEFT_REAR].servoPos.femur & 0xff) + (leg[LEFT_REAR].servoPos.femur >> 8);
    ax12write(LR_FEMUR_ID);
    ax12write(leg[LEFT_REAR].servoPos.femur & 0xff);
    ax12write(leg[LEFT_REAR].servoPos.femur >> 8);
    checksum += LR_TIBIA_ID + (leg[LEFT_REAR].servoPos.tibia & 0xff) + (leg[LEFT_REAR].servoPos.tibia >> 8);
    ax12write(LR_TIBIA_ID);
    ax12write(leg[LEFT_REAR].servoPos.tibia & 0xff);
    ax12write(leg[LEFT_REAR].servoPos.tibia >> 8);

    //LEFT MIDDLE
    checksum += LM_COXA_ID + (leg[LEFT_MIDDLE].servoPos.coxa & 0xff) + (leg[LEFT_MIDDLE].servoPos.coxa >> 8);
    ax12write(LM_COXA_ID);
    ax12write(leg[LEFT_MIDDLE].servoPos.coxa & 0xff);
    ax12write(leg[LEFT_MIDDLE].servoPos.coxa >> 8);
    checksum += LM_FEMUR_ID + (leg[LEFT_MIDDLE].servoPos.femur & 0xff) + (leg[LEFT_MIDDLE].servoPos.femur >> 8);
    ax12write(LM_FEMUR_ID);
    ax12write(leg[LEFT_MIDDLE].servoPos.femur & 0xff);
    ax12write(leg[LEFT_MIDDLE].servoPos.femur >> 8);
    checksum += LM_TIBIA_ID + (leg[LEFT_MIDDLE].servoPos.tibia & 0xff) + (leg[LEFT_MIDDLE].servoPos.tibia >> 8);
    ax12write(LM_TIBIA_ID);
    ax12write(leg[LEFT_MIDDLE].servoPos.tibia & 0xff);
    ax12write(leg[LEFT_MIDDLE].servoPos.tibia >> 8);

    //LEFT FRONT
    checksum += LF_COXA_ID + (leg[LEFT_FRONT].servoPos.coxa & 0xff) + (leg[LEFT_FRONT].servoPos.coxa >> 8);
    ax12write(LF_COXA_ID);
    ax12write(leg[LEFT_FRONT].servoPos.coxa & 0xff);
    ax12write(leg[LEFT_FRONT].servoPos.coxa >> 8);
    checksum += LF_FEMUR_ID + (leg[LEFT_FRONT].servoPos.femur & 0xff) + (leg[LEFT_FRONT].servoPos.femur >> 8);
    ax12write(LF_FEMUR_ID);
    ax12write(leg[LEFT_FRONT].servoPos.femur & 0xff);
    ax12write(leg[LEFT_FRONT].servoPos.femur >> 8);
    checksum += LF_TIBIA_ID + (leg[LEFT_FRONT].servoPos.tibia & 0xff) + (leg[LEFT_FRONT].servoPos.tibia >> 8);
    ax12write(LF_TIBIA_ID);
    ax12write(leg[LEFT_FRONT].servoPos.tibia & 0xff);
    ax12write(leg[LEFT_FRONT].servoPos.tibia >> 8);

    ax12write(0xff - (checksum % 256));
    setRX(0);
}

The comments match the descriptions in the datasheet so you can follow along. Basically, the way my servos are ID’ed and leg array is numbered, I can’t cleanly loop over them and index. Maybe someday I’ll redo that. But for now this works well enough.

The last thing I thought about is the servo update rate. After some research i settled on a 30hz update rate or frame rate. This means that the main loop of my program needs to run every 33ms and every loop should call the ax12SyncWriteServos() function to update all the servo positions. This also implies that ALL my calculations for the frame need to be calculated in 33ms! (foreshadowing…)

So at this point I can give all my servos a unique angle and they’ll move to it. Brilliant!

On to Phase 2: Leg Inverse Kinematics, i.e., Pointing a Foot in 3D Space!

Or head back to the beginning of Project B.E.T.H.

The Game Plan for B.E.T.H.

So at this point B.E.T.H. is a stock MkII PhantomX Hexapod with stock code. Pretty boring. The provided code is great for impressing your Mom, but if you really want the attention of your heart’s desire, you’re going to need your own code!

I think the best part of this is learning how it all works. You can stare at other people’s code all day long, but to really “get it” you have to figure it out and write it yourself. So here’s the game plan:

On to Phase 1: Driving the Servos!

Or head back to the beginning of Project B.E.T.H.

PhantomX Software and Electronics Setup

Once the MkII PhantomX Hexapod is built, it’s time to setup the software and electronics. I’d break it down into the following general steps:

1. Start charging your battery!

Connect the included Imax B6 charger to the wall with the power supply and barrel plug adapter. Connect the lipo battery using both the main deans power connector and the JST balance cable. This plugs into the 3S port on the charger. Scroll through the programs in the charger until you see “LIPO BALANCE CHARGE”. Hit enter, set the current to 1-2A, and the voltage to 11.1V 3S. When the settings look good (I’m not a battery expert, do your own research to get a feel for charge currents) hold the Enter button for a bit until it beeps and does a little test measuring your battery. It then tells you that it finds a 3S, and asks if that sounds correct. Hit Enter again to confirm and off it goes.

2. Arduino Setup for the Arbotix Robocontroller.

An “Arduino” is a family of easy-to-use microcontrollers based on Atmel AVRs. The Arduino IDE includes some handy software libraries for interfacing with a lot of the components in the hardware. The Arbotix Robocontroller uses an Arduino compatible Atmel AVR (ATMEGA644p), but some of its functions are not supported out of the box. So you need to add some libraries. This is outlined pretty well on their support wiki. Just follow the “Arduino Software Setup” section and you’ll be good to go. Don’t worry about any of the PyPose stuff, you don’t need any of that to get the hex up and running.

You should also download the latest MkII PhantomX Hexapod code from Trossen and put it into your Arduino sketchbook folder so its ready to go. If you’re feeling ambitious, give it a compile. If your libraries are setup right it should compile with no errors or warnings.

3. Pairing the Xbee radios.

The kit comes with two Xbee radios that must be paired in order to talk to each other. Basically this means that they need to talk on the same channel (ATID), at the same baud rate (ATBD), and have cooperating receive (ATMY) and send (ATDL) addresses. This forum thread is a great tutorial on how to pair the radios using the MaxStream X-CTU utility. The only variance from the guide is that the PhantomX kit comes with a UARTsBee instead of the Xbee Explorer, but they are functionally the same thing. And don’t worry about the Python Terminal stuff, you only need to use the X-CTU GUI utility. Once that’s all set, plug one of the Xbees into your Commander controller.

4. Programming the hex

The last step is to program the Robocontroller with the Trossen provided code. Break out your UARTsBee and check out how it will all go together here. Make sure that the little switch on the UARtsBee is set to 5V. Note that you don’t want an actual Xbee installed on the UARTsBee. The UARTsBee itself is the FTDI interface which we need for the programming. Its a multipurpose device. Also make sure that the power jumper on the Robocontroller board itself is set to ‘FTDI’ and  not ‘VIN’. This means that we’ll be powering the board with the UARTsBee and not your battery supply.  Also make sure that you DO NOT have the Xbee plugged into the Robocontroller. The Xbee radio and the UARTsBee serial interface share the same resources and can not be used at the same time.

Connect the UARTsBee to your PC with the USB cable. If all is well it will show up as a virtual COM port in your Windows Device Manager. In the Arduino IDE, select ‘Arbotix’ as your Board in the Tools Menu. Serial Port should be set to the virtual COM port of the UARTsBee.

Next unplug the UARTsBee from the USB Cable. Connect it now to the Robocontroller using the header cable making sure your wire colors are oriented correctly. Now connect the UARTsBee back to your USB cable and the Robocontroller should power up with a green light!

Now for the moment of truth. Click the ‘Upload’ button in the Arduino IDE. It should compile the code with no fuss and begin uploading it to the Robocontroller. You can see the Red LEDs blinking away on the UARTsBee as it transfers. When its all done the lights will go out and the Arduino IDE will report “Upload done.”

Now that your hex has a brain its almost go time. Disconnect the UARTsBee from the PC and from the Robocontroller. Set the Robocontroller power jumper to VIN (battery power). Plug the Xbee radio into the Robocontroller.

Power on the hexapod and power on the Commander controller. If everything went according to plan, the hex should come alive and walk, strafe, rotate, etc. Click the little black buttons above the joysticks to check out the different gaits. Victory!!!

Next: The Game Plan for B.E.T.H.’s Brain

Building B.E.T.H.

From work I saw that my MkII PhantomX hexapod kit had been delivered. I immediately raced home.

I quickly opened the box and spilled the contents onto the dining table. There was certainly a lot of stuff in there. (I wish I had a pic).

In addition to that list there is a TON of hardware. Bags and bags of screws and nuts. This is probably a boring post and I don’t have any pics, so I’ll cut to the chase. Following the assembly manual, it took a good 8 hours to build. It wasn’t hard, but it was pretty tedious. Over 350 tiny screws, each with a tiny nut. On top of that you need to locktite every nut. If you stay on track with the instructions, it goes smoothly. All the right hardware in the right places, right servos in the right locations, correct orientations, etc.

Next up was the Arbotix Commander, gotta build that too. Pretty straightforward although some of my hardware didn’t match the instructions and I had to improvise a little with a drill.

Next it was time for: PhantomX Software and Electronics Setup

Intro to Project B.E.T.H.

About five years ago I decided I wanted to build a hexapod robot. I’m really interested in legged robots because it makes you to break down and analyze what real life animals do. What is my brain doing as I walk? Why do centipedes have a ripple gait? How the heck does a mountain goat not fall off the side of the cliff?

I started by looking at Matt Denton’s hexes over at www.hexapodrobot.com. He has some great code that looks very natural. I couldn’t afford the whole hardware kit at the time, but i bought the p.Brain processor and three leg servos. With these parts and some paint stirrers i made a single leg. I got a robotics textbook (Remind me to update the title here) which explained how to use rotation matrices to calculate IK. I got through the math using Matlab and eventually was able to point my foot in 3D space! After that I did some basic trajectory planning so that i was able to make the leg step. It was very satisfying to see a leg move smoothly and naturally.  Well, by leg I mean a wooden paint stirrer.

Wooden Leg

I then got preoccupied with other things and the leg sat in a box for 5 years. A few months ago the itch returned. I went back to the old hexapodrobot.com forums and found they had dwindled down a bit. The p.Brain hardware didn’t have a huge community behind it which i knew i would need so i began researching a new platform. I wanted a platform with hardware I wouldn’t outgrow and a large community for the support I’d surely need along the way.

This led me to B.E.T.H.. B.E.T.H. (Battle Enhanced all-Terrain Hexapod, I’ll explain later) is a MarkII PhantomX Hexapod. This thing is every bit as awesome as it looks. Here’s why:

MkII PhantomX Hexapod

1. Arbotix Robocontroller – This is a great controller meant for use with the Dynamixel servos with enough speed and memory to get the job done. Arduino compatible means that its easy to code in the Arduino IDE and has a ton of community programming support.

  • Arduino compatible
  • 16MHz AVR microcontroller (ATMEGA644p).
  • 2 serial ports, 1 dedicated to driving the servo bus (with Dynamixel connectors), the other to the XBEE radio
  • 32 I/O, 8 of which can function as analog inputs
  • Hobby servo style 3-pin headers (gnd, vcc, signal) on all 8 analog inputs, and 8 of the digital IO
  • Dual 1A motor drivers, with combined motor/encoder header.

2. Dynamixel AX-12A Servos. These are “entry level” Dynamixels, but still pretty advanced robotics servos and far superior to hobby/RC servos. They communicate daisy chained along a serial bus so they’re easy to connect and command. They support commanding position, speed, torque on/off, etc and can feedback position, load, temp, voltage, and other things. Really cool!

3. Kit support. Trossen did a great job designing this hex and kit. The body looks great, allows for a ton of articulation and is physically very stout. If you buy the full kit, it includes an Arbotix Commander controller, lipo battery, charger, radios, etc, etc, etc. The Trossen online forum is also quite active and has lots of smart folks eager to help with robotics algorithms, design, etc.

So with the decision made, the trigger was pulled, and a few days later it showed up at the door.

Up next: Building B.E.T.H.

My Blog!

Welcome to my new blog. I’m not much of an online extrovert, but I figured I’d step out of my e-shell and share what I’m up to. I plan to share whatever it is i’m currently working on or obsessed with. Maybe someone will find it interesting or helpful or encouraging. Or maybe they won’t, but here’s to finding out!