L0-RES 12 SERVO DRIVER FOR HEXAPOD WITH USART COMM

[xor]

The following code is an example for driving 12 50Hz servos @ 16 positions each. The code was created in response to a forum user building a robotic hexapod and is posted here for the benefit of him and others.

The USART communication is a simple single byte command structure which can be sent at virtually any baudrate and any order the user desires.

The following code is only 300 ROM compiled with mikroBasic 7.01, meaning that a free version of the compiler can be used to create the Hex Code.

Code: Select all

'*******************************************************************************
'  Program to Control 12- 50Hz Servos @ 16 Positions Each With USART Updates
'*******************************************************************************
'           Code compiled with mikroBASIC and tested on 16F887 @ 8MHz
'           By Warren Schroeder  June 5, 2008
'*******************************************************************************
'*******************************************************************************
'
'           Each USART RX Byte:
'              Servo ID        = Upper 4 bits  (0 to 11)   12 Servos
'              Servo Position  = Lower 4 bits  (0 to 15)   16 Positions
'
'   HITECH-HS422 50Hz Servo: 940us to 2140us for min and max servo rotation
'                            16 positions (940us= Position#0 + (15 x 80us))
'                            Position#7 = 1500us Center
'
'       Timer1 Setup for 1us ticks  (Prescaler=2)
'
'       CCP1 is set up for Special Event Compare Mode
'       On Compare-Match between TIMER1 and CCPR1L:CCPR1H registers
'       CCP1IF is Set (=1) and TIMER1 is RESET (=0)
'
'
'*******************************************************************************
'*******************************************************************************

Symbol Servo0  = PORTB.0
Symbol Servo1  = PORTB.1
Symbol Servo2  = PORTB.2
Symbol Servo3  = PortB.3
Symbol Servo4  = PORTB.4
Symbol Servo5  = PORTB.5
Symbol Servo6  = PORTB.6
Symbol Servo7  = PORTB.7
Symbol Servo8  = PORTD.0
Symbol Servo9  = PORTD.1
Symbol Servo10 = PORTD.2
Symbol Servo11 = PORTD.3


Dim AllOn           as Boolean                    ' all servos ON flag
Dim t0, t1, t2      as Byte                       ' work variables
Dim Frame80         as Byte                       ' 80us time frame counter
Dim ServoPos        as byte[12]                   ' 12 Servo Position Array from RX
Dim ServoWrk        as byte[12]                   ' 12 Servo Position Work Array


sub procedure interrupt
    If AllOn Then                                 ' If AllOn flag=true then
           PORTB = 255                            ' All PORTB servos ON
           PORTD = 15                             ' All PORTD servos ON
           AllOn = Not(AllOn)                     ' reset AllOn flag
           Frame80 = 0                            ' 80us frame counter
           CCPR1L = 940                           ' 940us = 0 position
           CCPR1H = Hi(940)
           For t2 = 0 to 11                       ' load work array from USART RX array
              ServoWrk[t2] = ServoPos[t2]
           Next t2
    Else
           CCPR1L = 80                            ' 80us frame delay
           CCPR1H = 0
           FSR = @ServoWrk                        ' servo work array pointer.. first servo
           If INDF = 0 Then Servo0 = 0 End If     ' turn each servo OFF on 0
           Dec(INDF)                              ' decrement servo pos counter
           INC(FSR)                               ' next servo.... etc. etc.
           If INDF = 0 Then Servo1 = 0 End If
           Dec(INDF)
           INC(FSR)
           If INDF = 0 Then Servo2 = 0 End If
           Dec(INDF)
           INC(FSR)
           If INDF = 0 Then Servo3 = 0 End If
           Dec(INDF)
           INC(FSR)
           If INDF = 0 Then Servo4 = 0 End If
           Dec(INDF)
           INC(FSR)
           If INDF = 0 Then Servo5 = 0 End If
           Dec(INDF)
           INC(FSR)
           If INDF = 0 Then Servo6 = 0 End If
           Dec(INDF)
           INC(FSR)
           If INDF = 0 Then Servo7 = 0 End If
           Dec(INDF)
           INC(FSR)
           If INDF = 0 Then Servo8 = 0 End If
           Dec(INDF)
           INC(FSR)
           If INDF = 0 Then Servo9 = 0 End If
           Dec(INDF)
           INC(FSR)
           If INDF = 0 Then Servo10 = 0 End If
           Dec(INDF)
           INC(FSR)
           If INDF = 0 Then Servo11 = 0 End If
           Dec(INDF)
           If inc(Frame80) = 16 Then             ' finished 15 80us position periods?
               CCPR1L = 18800                    ' load remaining time for 20ms interrupt
               CCPR1H = Hi(18800)
               AllOn = Not(AllOn)                ' turn all servos ON at next 20ms interrupt
           End If
    End If
    PIR1.CCP1IF = 0                              ' clear CCP1 interrupt flag
end sub


sub procedure CCP1_Setup()
    CCP1CON      = 11     ' CCP1 Compare MODE with special event trigger; resets Timer1 on match
    CCPR1L       = 65000  ' preload for 65ms delay before servo startup
    CCPR1H       = Hi(65000)
    T1CON        = 16     ' Timer1 Prescaler = 2 = 1us ticks
    PIE1.CCP1IE  = 1      ' Enable CCP1 interrupt
    PIR1.CCP1IF  = 0      ' Clear CCP1 Interrupt Flag
    INTCON       = 192    ' Global & Peripheral interrupts enabled
    T1CON.TMR1ON = 1      ' Start Timer1
end sub



main:

    ANSEL   = 0           ' disable adc's
    ANSELH  = 0           ' disable adc's
    CM1CON0 = 0           ' disable comparators
    CM2CON0 = 0           ' disable comparators
    PORTB   = 0
    PORTD   = 0
    TRISB   = 0
    TRISD   = 0
    AllOn   = True         ' All servos ON flag

    USART_INIT(19200)
    CCP1_Setup()
    
    While true
       If USART_DATA_READY = 1 Then         ' test for RX byte
          t0 = USART_READ                   ' save new RX byte
          t1 = SWAP(t0) And 15              ' extract servo ID from RX byte
          ServoPos[t1] =  t0 And 15         ' save Position Value per ID
       End If
    Wend
    
end.

[spookyrufus]

I just wanted to write you my compliments.
Even if I DON'T like basic and basic-like languages, because @ university they teach us they are evil (and because in fact they are a little bit...), I must admit that your code is really neat and simple, and seems doing (not tested yet) exactly what it's expected to do.

Great.

Andrea

[spookyrufus]

Code: Select all

           For t2 = 0 to 15                       ' load work array from USART RX array
              ServoWrk[t2] = ServoPos[t2]
           Next t2 

Is this right? Why the for loop ends when index reaches 15 and not 12?
Isn't this going to throw null pointer (or something similar) exceptions?

[Charlie]

because @ university they teach us they are evil

How So?

[trust issues]

I've had a few lecturers frown upon Basic programming also. It doesn't make a difference in PICs however. Whether you're using C, Pascal or Basic, it's generating the same hex.

[xor]

Code: Select all

           For t2 = 0 to 15                       ' load work array from USART RX array
              ServoWrk[t2] = ServoPos[t2]
           Next t2 

Is this right? Why the for loop ends when index reaches 15 and not 12?
Isn't this going to throw null pointer (or something similar) exceptions?

Yes, you are right. I originally had in mind to make this a 16 servo routine because it was capable. Here is the correct change:

Code: Select all

           For t2 = 0 to 11                       ' load work array from USART RX array
              ServoWrk[t2] = ServoPos[t2]
           Next t2

[xor]

I've had a few lecturers frown upon Basic programming also. It doesn't make a difference in PICs however. Whether you're using C, Pascal or Basic, it's generating the same hex.

Structured Basic has followed C and done quite well I think at all levels, whether PC or microcontrollers. This whole debate of C or Basic reminds me of what a friend asked me:

"When you go to see the doctor and see his diploma/certificate on the wall, do you ask him how his grades were in school first?"

When you have a great program on your PC, did you purchase it based on whether it was written using C, Basic, Delphi?? Do you know... do you care?

[spookyrufus]

You're absolutely right. In fact, using PIC uc and Mikroe products, I don't think that basic will make some difference from C or Pascal.
More generally however, C is more expressive and flexible than Basic, not because it's better designed or stuff like that, but just because Basic was thought with a higher level in mind than C. think about pointers, memory addressing, memory allocation and so on.
Or just think about writing a really complex software (such as an Operative System!) in Basic. Simply not possible.
I mean, I learned QBasic my own when I was 12, and was REALLY great.
But I had to wait till I was 19 to learn C and more advanced programming, and now I can say that I definitely prefer the formalism of C to the simplicity of Basic.

That's my opinion

[spookyrufus]

I got a doubt about translating this instruction in MikroC:

t1 = SWAP(t0)

I've thought this solution:

tmp=t0;
t1=(t1<<4) | (tmp & 0b00001111);


Will this work?

[xor]

Swap() is related to PIC assembly instruction "swapf". Instead of shifting 4 times a single instruction does the work. I don't see it as an available function in C. You can see how I handled it below. You were correct in concept, just wrong in application.

Here is the driver written in MikroC. While converting this code I found a couple mistakes in the original mB which should be corrected in this version.

Code: Select all

/*
//'*******************************************************************************
//'  Program to Control 12- 50Hz Servos @ 16 Positions Each With USART Updates
//'*******************************************************************************
//'           Code compiled with mikroC 8.2 and tested on 16F887 @ 8MHz
//'           By Warren Schroeder  June 8, 2008
//'*******************************************************************************
//'*******************************************************************************
//'
//'           Each USART RX Byte:
//'              Servo ID        = Upper 4 bits  (0 to 11)   12 Servos
//'              Servo Position  = Lower 4 bits  (0 to 15)   16 Positions
//'
//'   HITECH-HS422 50Hz Servo: 940us to 2140us for min and max servo rotation
//'                            16 positions (940us= Position#0 + (15 x 80us))
//'                            Position#7 = 1500us Center
//'
//'       Timer1 Setup for 1us ticks  (Prescaler=2)
//'
//'       CCP1 is set up for Special Event Compare Mode
//'       On Compare-Match between TIMER1 and CCPR1L:CCPR1H registers
//'       CCP1IF is Set (=1) and TIMER1 is RESET (=0)
//'
//'
//'*******************************************************************************
//'*******************************************************************************
*/

#define Servo0  PORTB.f0
#define Servo1  PORTB.f1
#define Servo2  PORTB.f2
#define Servo3  PortB.f3
#define Servo4  PORTB.f4
#define Servo5  PORTB.f5
#define Servo6  PORTB.f6
#define Servo7  PORTB.f7
#define Servo8  PORTD.f0
#define Servo9  PORTD.f1
#define Servo10 PORTD.f2
#define Servo11 PORTD.f3

unsigned int register volatile
	CCPR1	  absolute 0x0015             ;
unsigned short AllOn                     ;
unsigned short frame80                   ;
unsigned short t0                        ;
unsigned short t1                        ;
unsigned short t2                        ;
unsigned short ServoPos[12]              ;  //  12 Servo Position Array from RX
unsigned short ServoWrk[12]              ;  //  12 Servo Position Work Array


void interrupt() {
    if (AllOn) {                            //  If AllOn flag=true then
           PORTB = 255                   ;  //  All PORTB servos ON
           PORTD = 15                    ;  //  All PORTD servos ON
           AllOn = ~AllOn                ;  //  reset AllOn flag
           frame80 = 0                   ;  //  80us frame counter
           CCPR1 = 940                   ;  //  940us = 0 position
           for (t2=0;t2<12;t2++) {          //  load work array from USART RX array
              ServoWrk[t2] = ServoPos[t2];  //
           }
    }                                       //
    else {                                  //
           CCPR1 = 80                    ;  //  80us frame delay
           FSR = (unsigned short)&ServoWrk ;  //  servo work array pointer.. first servo
           if (INDF-- == 0) Servo0  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo1  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo2  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo3  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo4  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo5  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo6  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo7  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo8  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo9  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo10 = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo11 = 0  ;  //  turn each servo OFF on 0; decrement counter

           if (++frame80 == 16) {           //  finished 15 80us position periods?
               CCPR1 = 18800             ;  //  load remaining time for 20ms interrupt
               AllOn = ~AllOn            ;  //  turn ON all servos at next 20ms interrupt
           }
    }
    PIR1.CCP1IF = 0                      ;  //  clear CCP1 interrupt flag
}


void CCP1_Setup() {
    CCP1CON      = 11         ;  //  CCP1 Compare MODE with special event trigger; resets Timer1 on match
    CCPR1        = 65000      ;  //  preload for 65ms delay before servo startup
    T1CON        = 16         ;  //  Timer1 Prescaler = 2 = 1us ticks
    PIE1.CCP1IE  = 1          ;  //  Enable CCP1 interrupt
    PIR1.CCP1IF  = 0          ;  //  Clear CCP1 Interrupt Flag
    INTCON       = 192        ;  //  Global & Peripheral interrupts enabled
    T1CON.TMR1ON = 1          ;  //  Start Timer1
}



void main() {

    ANSEL   = 0               ;   //  disable adc's
    ANSELH  = 0               ;   //  disable adc's
    CM1CON0 = 0               ;   //  disable comparators
    CM2CON0 = 0               ;   //  disable comparators
    PORTB   = 0               ;
    PORTD   = 0               ;
    TRISB   = 0               ;
    TRISD   = 0               ;
    AllOn   = 1               ;   //  all servos ON flag
    

    USART_INIT(19200)         ;
    CCP1_Setup()              ;

    while(1) {
       if (USART_DATA_READY) {           // test for RX byte
          t0 = USART_READ()            ; // save new RX byte
          t1 = t0 >> 4                 ; // extract servo ID from RX byte
          ServoPos[t1] =  t0 && 15     ; // save Position Value per ID
        }
    }

}

[spookyrufus]

I've tested your code but it does not work at all
Even if I initialize the array ServoPos, servos are tying to reach a position outside bounds, and seems really jitty.
Maybe I'm missing something?

[Charlie]

Hi Spooky,

What is your hardware setup? Breadboard,or EP borad?

[spookyrufus]

EasyPic 3, Pic 16f877a, 8 Mhz quartz.
I'm testing the code onto a breadboard with a HS-422 Hitec Servo, which does not respond correctly and tries to reach position outside lower bound.

When I run the code posted by xor, Leds connected to the servo controller pins, seems turned on all the time.
When I was using my (quite) working code, I remember seeing all leds flashing at a visible flash-rate.

I think this code produces somehow (I don't understand it too much) a too high pulse out, which is not coherent with values required to drive those servos.
xor, have you tested your code in a real environment?

[xor]

The code was tested on my EasyPIC5 and I used LED's to determine whether the PWM worked or not.

I will put a scope on the outputs later to see the real waveform and let you know of any aberrant behavior.

How are you changing or setting up values? RS232 or in code? How are your servos actually connected and to what ports?

Is your servo power separated from the PIC's power supply?

[spookyrufus]

The only thing I changed is a for-loop to initialize the ServoPos array, done in the simplest way possible:

int count=0;
for(count=0;count<12;count++)
ServoPos[count]= 7;

No RSR232 yet, just put the pic onto the breadboard and connected one servo to one output pin (these are all 8 PORTB pins + 4 PORTD pins).

Servo is conected through the only control cable it has, linked directly to the output pin, and using a pull-down resistor.

My servo power is the same as pic power supply, to allow them share the same electrical reference.

I'm quite sure there's something wrong with the waveform, but I really can't say what.
I look forward for your help.

PS: maybe I'm finally going to buy a copy of MikroC!!!!!!!!!!!!!!!!!!!!!!!! I'm quite excited.