banner

Monday, June 11, 2012

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

17 comments:

GAURAV JAIN said...

Can i get the circuit diagram of the development board..?? please reply soon..

Pramode C E said...

Cool!

Pramode C E said...

You can publish the project here:

http://www.43oh.com/

Vinod.S said...

thank you Sir for your sincere guidance, motivation and encouragement.......

mobi-Lars said...

Very nice!

pascal buirey said...

Hi,

Thanks, for the idea ! I just made something similar : http://www.skallblog.net/2012/07/un-petit-robot-fabrique-avec-2-servo.html
also with a launchpad, but written the code using energia. (for of arduino ide)

Vinod.S said...

nice :)

Dinesh said...

Hi! Vinod, really nice blog. I have trying to get in touch with by email and phone. No response...I have few project requirements, wish to know your availability. Please send me email on dinesh.p.pawar(at)gmail.com keep up the good work.

Hari said...

Hey, Well done.
I wanted to know which firmware did you use? CCS/IAR or any other?
Please reply


Thank you.
Shreehari

sagar yelavdekar said...

hi,sir
can you give me the idea about,How to interface msp430 launchpad using visual basic,i have design 1 form of vb & display some parameter on computer with help of ani application.

Vinod.S said...

Hello, no idea about vb..

Sai Krishna said...

HELLO SIR,
CAN I SENT ME THE CIRCUIT DIAGRAM TO MAIL ID : speak2sainow@gmail.com

Вова Мукосей said...

Hi it is the cool idea.
Please give me fool scheme in this email
cozerog14@yandex.ru
Thanks

Jeff Douglass said...

Hi. My 10yo loaded your code up on code composer studio it returned 89 errors, He managed to fix most of them but is having difficulty with "org 0xf800 " in Problem it says " this declaration has no storage class or type specifier " . He would be very happy to complete his Walker and show it to his friends, Could you possibly show a work around?

Vinod.S said...

Use naken430asm assembler. Don't use CCS.

Also U can use direct hex file posted at the bottom of the asm file.

Jeff Douglass said...

Vinod Thanks the advice. unfortunatly my 10 year old is up to his head with ccs. asm code is just way to much for him. after some long hours he is loosing it. he would like to know if we can fix this using ccs?
enjoyed the vid

Jeff Douglass said...

Vinod i admit you really have a cool project that my son wanted to build. We tryed the naken430asm assembler web site you sugested, but for some reason the download does not install. unfortunatly for him this is quickly turning into a dead end. would it be to much to ask if you could provid this in C. ?

Post a Comment

SHARE IT on FB