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


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

    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              *
    '*        .2MMMW2aBMM@Si.rZ2,:;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.