two servo walking robot using TI launchpad

       Hi, I bought two small servo motors last month. I was thinking what I can do with this two servo, since it is only two in number. Then I asked this to my friend Achu Wilson and he suggested me to try a four legged two servo walker and he shown a youtube video in which some one demonstrating it. Then I also got interested to make some thing similar to that. Then I started designing my walker using two servo and msp430 launchpad and at last it turned out even better than I thought it would.(See the video above). The servo controlling techinque used here is a little bit different compared to the usual hardware PWM, I used a circular buffer to save each servo position and o/p pin details. Only a timer compare interrupt is used for this. This is a common technique used for controlling more servo using a cheap microcontroller with limited hardware Timer-pwm modules. Using this software pwm(not a perfect pwm, but still it will work in the servo motors) techinque, I can control more servo motors like 4,5, 6 etc etc depending on the number of I/O pins. 

     Coding for this msp430 launchpad is done in asm just because I also want to refresh the msp430 assembly language programming. I used naken430asm assembler in linux for the purpose.

Photos:

design tip: (see below figure)

Just imagine a lizard or a crocodile  creeping. We could see that it will raise one leg and will apply force with another leg and this is done alternatingly with all the four legs which results in the motion. It means, we need a small up and down motion also. But if we are designing it as in above figure(top wrong), we cannot make small vertical movement, ie always the four legs will have a contact with the ground. This will prevent it's movement in the horizontal plane. 
      To overcome this problem, I need to design it as shown in the second figure(marked as right). There I made a small inclination between the two planes on which each servo is attached.
     
Circuit:
     Circuit is simple, just need to connect the servo inputs to the P1.1 and P1.2  pins of ti launchpad with MSP430G2231 microcontroller and then as usual, need to connect the power supply to the launchpad board and the servo. I used 3.7V battery to power it(an old nokia BL-5C battery).

servo motor specification: Servo motor 10g / 1.2kg / 0.09s Torque: 1.4kg/cm @ 4.8v, 1.6kg/cm @ 6v Weight: 10g Speed: 0.10/60deg @ 4.8v, 0.09/60deg @ 6v
Code:

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;Project: Two servo two directional walking robot with ti launchpad
; author: vinod s <vinodstanur@gmail.com> 
; date: Tue Jun 12
; processor: msp430g2231
; assembler: naken430asm 
; development platform: linux
; servo connections: P1.1 & P1.2
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
.include "msp430g2x31.inc"

#define TOTAL_SERVO 2

#define TOP_OF_RAM 0x27f

#define CALIBC1_1MHZ 0x10ff
#define CALDCO_1MHZ 0x10fe   

#define SERVO_INDEX TOP_OF_RAM
#define SERVO_BUFFER TOP_OF_RAM-((TOTAL_SERVO+1)*4)-3
#define TOP_OF_STACK SERVO_BUFFER - 2
#define SERVO_START_ANGLE SERVO_BUFFER
#define SERVO_START_PINS SERVO_START_ANGLE+2
;;SERVO;;
#define SERVO1_ANGLE SERVO_BUFFER+4
#define SERVO1_PIN SERVO_BUFFER+6
#define SERVO2_ANGLE SERVO_BUFFER+8
#define SERVO2_PIN SERVO_BUFFER+10

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
org 0xf800             ;flash begins 
startup:                 ;startup code which sets the stack pointer and disable WDT
    mov.w #(WDTPW|WDTHOLD), &WDTCTL ;disabling watch dog timer, otherwise it will reset on regular interval
    mov.w #(TOP_OF_STACK), SP     ;SETTING TOP OF THE STACK ON STACK POINTER
    call #main           ; (not a must :-))
    
;;;MAIN PROGRAM;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
main:
    mov.b &CALIBC1_1MHZ, &BCSCTL1
    mov.b &CALDCO_1MHZ, &DCOCTL
    mov.b #0, &P1OUT
    mov.b #255,&P1DIR
    call #timer_init
    call #servo_init
    eint
 
    infinite_loop:
        mov.w #5,r14
        ntimes1:
            call #walk_forward
            dec r14
            jnz ntimes1
        mov.w #5,r14
        call #stop_idle
        call #delay
        ntimes2:
            call #walk_reverse
            dec r14
            jnz ntimes2
        call #stop_idle
        call #delay
        jmp infinite_loop


;-------------INTERRUPT-------------------------------------;
ISR: 
    xor.w r9, r9
    mov.b &(SERVO_INDEX), r9
    mov.w (SERVO_BUFFER)(r9), &TACCR0
    mov.b (SERVO_BUFFER+2)(r9), &P1OUT
    add.b #4,r9
    cmp.b #((TOTAL_SERVO+1)*4) , r9
    jnz exit
    clr.b r9
    exit:
    mov.b r9, &(SERVO_INDEX)
    reti
;------------------------------------------------------------;

;;;;;;;;;;;;;;;;;OTHER FUNCTIONS;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

stop_idle:
    mov.w #1500, &(SERVO1_ANGLE)
    call #delay
    mov.w #1500, &(SERVO2_ANGLE)
    call #delay
    ret
    
walk_forward:
    mov.w #1000, &(SERVO1_ANGLE)
    call #delay
    mov.w #1000, &(SERVO2_ANGLE)
    call #delay
    mov.w #2000, &(SERVO1_ANGLE)
    call #delay
    mov.w #2000, &(SERVO2_ANGLE)
    call #delay
    ret

walk_reverse:
    mov.w #1000, &(SERVO2_ANGLE)
    call #delay
    mov.w #1000, &(SERVO1_ANGLE)
    call #delay
    mov.w #2000, &(SERVO2_ANGLE)
    call #delay
    mov.w #2000, &(SERVO1_ANGLE)
    call #delay
    ret
 
delay:
    push R10
    mov.w #0xffff,R10
    oo:
        dec R10
        jnz oo
    pop R10
    ret
    
timer_init:
    mov.w #20000, &TACCR0 
    mov.w #CCIE, &TACCTL0 
    mov.w #(TASSEL_2|ID_0|MC_1|TACLR), &TACTL
    ret

servo_init:
    mov.w #20000, &(SERVO_START_ANGLE)
    mov.w #1500, &(SERVO1_ANGLE)
    mov.w #1500, &(SERVO2_ANGLE)
    mov.b #0, &(SERVO_START_PINS)
    mov.b #2, &(SERVO1_PIN)
    mov.b #4, &(SERVO2_PIN)
    mov.b #0,&(SERVO_INDEX)
    ret
 
;VECTOS
org 0xfffe                  ;reset vecor
    dw startup             ;to write start address to 0xfffe on programming
org 0xfff2                  ;timer interrupt vecTOP_OF_RAM (CC)
    dw ISR  ;to write isr address to 0xfff2 on programming

The naken430asm assembler is there at mike kohn's website (http://www.mikekohn.net/micro/naken430asm_msp430_assembler.php)

hex code:

:10F80000B240805A200131406E02B0120EF8D2424E
:10F81000FF105700D242FE105600C2432100F243AF
:10F820002200B012EEF8B01202F932D23E400500CA
:10F83000B0128EF81E83FC233E400500B01278F80B
:10F84000B012E2F8B012B8F81E83FC23B01278F8B8
:10F85000B012E2F8EB3F09E959427F029249700287
:10F860007201D24972022100695279900C00012084
:10F870004943C2497F020013B240DC057402B01252
:10F88000E2F8B240DC057802B012E2F83041B24052
:10F89000E8037402B012E2F8B240E8037802B01252
:10F8A000E2F8B240D0077402B012E2F8B240D007DA
:10F8B0007802B012E2F83041B240E8037802B012A8
:10F8C000E2F8B240E8037402B012E2F8B240D007A6
:10F8D0007802B012E2F8B240D0077402B012E2F837
:10F8E00030410A123A431A83FE233A413041B24072
:10F8F000204E7201B24010006201B2401402600159
:10F900003041B240204E7002B240DC057402B24079
:10F91000DC057802B240DC057C02C2437202E2439D
:10F920007602E2427A02F2427E02C2437F02304114
:02FFF20056F8BF
:02FFFE0000F809
:00000001FF