MEL PICBASIC Forum - Driving up to 9 servos with 2 I/O lines

  • Driving up to 9 servos with 2 I/O lines

    by
    aberco
    Published on - 23rd December 2010 14:25  Number of Views: 10066  
    This little code example demonstrate how to drive 9 servos using only two I/O lines. The PWM signal is then demultiplexed using a simple CD4017 CMOS decade counter to drive each separate servo channels.


    <<< DOWNLOAD THE FILE HERE: NineServo.pbp >>>

    SOURCE CODE:
    Code:
    '***************************************************************************
'*  Name    : NINESERVO.pbp                                                *
'*  Author  : Antoine Bercovici                                            *
'*  Date    : 12/23/2010            ,;22ZB088ZZSS,                         *
'*  Version : 1.0               7WMM@MB00ZZ0BW0@B@MMW2                     *  
'*                            iM@0Z28ZXZ877Sa0W8XZWB@MM.                   *          
'*                            M0SSXa0Z20;.   ;Wa2aa2SZaZ                   *
'*                            80Wa2Z27X28r.;X777S2a8a;SS XX                *
'*                             M2a80S0BZZ0BW88a002rriSW@W00MMi             *
'*                          X0MMBaZaZZaSZZ8Za22XXS0MMMMMMWM   iMM          *
'*                      ;XXa0Z@MMMM@WBB@BB08a2ZBMMBr   7@M@88MM8;          *
'*                  ,Z@ZZ77X;:i;aWMM@MMMMMM@@MMM,        MM0i              *
'*                @MBaXa22S7iri;:.222aXrir20@Mi  r@MMMi  SM                *
'*              i0M@@0ZSXX2aaX7Sr,..,:r72Z00M   MMWB@M;  iM    ;a8S        *
'*             BMX:;S0W@0822aZaS7X7aZ0BWBW0M   MMWWMMS   ;MX2Ba;           *
'*             ZMMM2;,;SBW@BZXr:iXBB08BM@X8@   MMMMS  r  a@Ma              *
'*        [email protected],:;ZBB088W@   WM         M;  MM                *
'*       MMM.  ,Zi:iX8MM@Si:S0MMMMB0@M   @008     7WM8  0WM                *
'*          .@27;X2222XS0MB800r  rMMM  ,@8ZZWMMMMMMMr  BMMW                *
'*           ,8WZW@W@BX,i72Z0Z     ;  XM0ZZ8S  rBS.  XMMa                  *
'*              @M2i:rS8B@WZZ0WBi     aM@0Z0;      S0S                     *
'*              8M@B0BWB@WWS22Z0M0      ,288MM@0ZX,                        *
'*              SMMM@@BBBW022ZZ@S  Z@i    BM8X.                            *
'*               .0MMM@BWBB22aW7  ZWWMM882.                                *
'*                  :WMMM@BZ2W:  @MMW2,                                    *
'*                     i@MMBW7 ;MBi                                        *
'*                        .Xi.:                                            *
'*                                                                         *
'*  USAGE  1. This interrupt update the position of nine servos using      *
'*            only two I/O lines and an external CD4017. Resolution for    *
'*            each servo channel is 11bit (0 to 2047) and signal period is *
'*            22ms (about 45Hz).                                           *
'*         2. The interrupt is triggered by a 16bit timer (Timer1) using   *
'*            Darrel Taylor's Instant interrupt routines. Don't forget to   *
'*            include these two files in your project folder               *
'***************************************************************************

ASM
__config _INTRC_OSC_NOCLKOUT & _WDT_ON & _MCLRE_ON & _BOREN_OFF & _LVP_OFF & _CP_OFF
ENDASM

DEFINE OSC 4          
DEFINE LOADER_USED 1
clear

'*******************************************************
'I/O port declaration
'*******************************************************
                                                                 
ServoClock VAR PORTB.0
ServoReset VAR PortB.1

'*******************************************************
'Variable declaration
'*******************************************************

ServoValue VAR WORD[9] 'Resolution of each channel is 11bit (0 to 2047)
ServoTemp VAR WORD
InvertedValue VAR WORD
ServoID VAR BYTE
i VAR WORD

'*******************************************************
'Init for PIC16F628A
'*******************************************************

CMCON = %00000111 'Disable comparator module
TRISA = %00000000 'All PortA is output
TRISB = %00000000 'All PortB is output
T1CON = %00000001 'Set Timer1 ON at 1Mhz running from internal oscillator with /1 prescaler

'*******************************************************
'Init the interrupt handling system
'*******************************************************
INCLUDE "DT_INTS-14.bas" 'Base Interrupt System
INCLUDE "ReEnterPBP.bas" 'Include if using PBP interrupts

ASM
INT_LIST  macro     ; IntSource,          Label,  Type, ResetFlag?
        INT_Handler     TMR1_INT,   _ServoUpdate,   PBP,  Yes
    endm
    INT_CREATE ; Creates the interrupt processor
ENDASM

'*******************************************************
'Main Program
'*******************************************************
wsave VAR BYTE $70 SYSTEM 'alternate save location for W, for DT_INTS use

ServoID = 9
InvertedValue = 0

@ INT_ENABLE  TMR1_INT ; enable Timer 1 interrupts

MainLoop:

'Example routine that rotates all servo position counterclockwise
FOR i = 0 TO 2047
    ServoValue[0] = i
    ServoValue[1] = i
    ServoValue[2] = i
    ServoValue[3] = i
    ServoValue[4] = i
    ServoValue[5] = i
    ServoValue[6] = i
    ServoValue[7] = i
    ServoValue[8] = i
    PAUSE 10
NEXT i

GOTO MainLoop

'*******************************************************
'Timer1 Interrupt Handler
'*******************************************************
ServoUpdate:
    
IF ServoID >= 9 THEN
    HIGH ServoReset
    ServoTemp = 65535 - InvertedValue
    InvertedValue = 18423 'Initialize the remaining time to its maximum value, 9*2047
    ServoID = 0
    LOW ServoReset
ELSE
    HIGH ServoClock
    ServoTemp = 65270 - ServoValue[ServoID] 'Offset value for servo centerpoint (1.5ms)
    InvertedValue = InvertedValue - ServoValue[ServoID] 'Compute the remaining time to be added at the end
    ServoID = ServoID + 1
    LOW ServoClock
ENDIF
 
TMR1H = ServoTemp.BYTE1 'Update the timer value for next interrupt when overflow
TMR1L = ServoTemp.BYTE0 
        
@ INT_RETURN

END
    How does it work?
    In fact, it is similar to the decoding stage found in many PPM radio receiver, where the servo position is encoded as a pulse train with variable durations. The signal is reconstructed by clocking the 4017 using this signal, and the active output advances to the next pin with the predefined delay each time the counter is clocked.



    Signal generated.
    All current radios assume that a high level of 1.5 ms is control centered. The range of variation for servo position (-90° to +90°) is 1.0 to 2.0 ms. However variations does exists and it is preferable to go beyond these limits to get the full range of movement. The program given herein provides for a pulse period of 22ms (about 45Hz), with a PWM resolution of 11bit per channel (values from 0 to 2047). 1023 correspond to the center value, and the two extreme goes slightly beyond the capability of the servos (about -92° to +92°).

    Each servo position are then updated one after another, instead of all at the same time. The output Q0 is not available for use as it is used to reconstruct the final timing necessary to get the 22ms signal period. At this rate, the interruption routine is called about 450 times per seconds.



    Variations can easily be made using shift registers instead of decade counters with minor changes to the code.