Caterpillar robot

This is my first locomotive robot I will be building. I so far have most of the body done, as of now it has 2 hobby servos but will have three when finished. 

As I said this is my first locomotive robot so i will be needing help with the coding. if you do wish to help, i would like the movement to be similar to a wave and as smooth as possible, and dont worry about the third servo just yet, if you are wishing to help just concentrate on the two already. thanks in advance. this will obviously be updated when needed and full recognition will be given to anyone who helps. I will post my code once i think it is done and you can do with it what you will, but bear in mind (as you will soon find out) my coding is very longwinded so if when reading through it you have any sugestions on making i shorter dont hesitate to contact me.

Update 1: 11/7/11

As you can see in the video, I have the worm moving(ish). The code to this is here: http://db.tt/keL445D. The next step is to add in the third servo and get the movement a little smoother.

Update 2: 11/7/11 (8:30pm)

I have added in the third servo motor of which can now be seen in the main picture. Now all thats left to do is code it all :)

Update 3: 12/7/11

It is finished. The speed was the smoothing problem and by most standards it is still a bit basic but i have lots of ideas for non-standard robots so sue me. New link to the final code is here http://db.tt/YlTODQJ. New video is up. see you lot soon :)