November 24, 2024, 12:43:50 AM

News:

You can now use Vixen to program your Prop-1 and Prop-2 controllers!  Get started quickly and easily, without having to learn PBASIC.  Details in the Library forum.


Prop-SX - 16 Servos with VSA

Started by JonnyMac, February 27, 2008, 08:59:01 AM

Previous topic - Next topic

JonnyMac

February 27, 2008, 08:59:01 AM Last Edit: June 16, 2009, 10:08:44 AM by JonnyMac
Okay, this should about do it for the VSA-compatible programs for VSA; this one gives you 16 servos.  If you look at the code it's very modular; what's happening, in fact, is that it's running two "virtual" servo controllers that have eight servos each. 

VSA project setup: Channels: 0 to 15

Channels 0 to 15:
  * MiniSSC Servo
  * +Value = 245
  * -Value = 55
  * Default = 150


[Edit]: Posted updated version 16 JUN 2009


' =========================================================================
'
'   File...... Servo16.SXB
'   Purpose...
'   Author.... Jon Williams, EFX-TEK
'              Copyright (c) 2008-2009 EFX-TEK
'              Some Rights Reserved
'              -- see http://creativecommons.org/licenses/by/3.0/
'   E-mail.... jwilliams@efx-tek.com
'   Started...
'   Updated... 16 JUN 2009
'
' =========================================================================


' -------------------------------------------------------------------------
' Program Description
' -------------------------------------------------------------------------
'
' Servo/PWM controller designed for use with VSA or other real-time
' serial control programs.
'
'   P0 to P15... Servo control
'
' The protocol is identical to the SEETRON (www.seetron.com) MiniSSC:
'
'   <sync><channel><value>
'
'   sync...... $FF (255)
'
'   channel... 0 to 15
'
'   value..... 55 to 245 for servos (150 = center)
'              -- these values can be adjusted in the code
'
' Default baud rate is 38.4K but can be changed down to as low as 2400.


' -------------------------------------------------------------------------
' Conditional Compilation Symbols
' -------------------------------------------------------------------------


' -------------------------------------------------------------------------
' Device Settings
' -------------------------------------------------------------------------

ID              "Servo16"

DEVICE          SX28, OSCHS2, TURBO, STACKX, OPTIONX, BOR42
FREQ            50_000_000


' -------------------------------------------------------------------------
' I/O Pins
' -------------------------------------------------------------------------

ServosHi        PIN     RC   OUTPUT             ' servos - high group
Servo15        PIN     RC.7                    ' use P15
Servo14        PIN     RC.6                    ' use P14
Servo13        PIN     RC.5                    ' use P13
Servo12        PIN     RC.4                    ' use P12
Servo11        PIN     RC.3                    ' use P11
Servo10        PIN     RC.2                    ' use P10
Servo9         PIN     RC.1                    ' use P9
Servo8         PIN     RC.0                    ' use P8

ServosLo        PIN     RB   OUTPUT             ' servos - low group
Servo7         PIN     RB.7                    ' use P7
Servo6         PIN     RB.6                    ' use P6
Servo5         PIN     RB.5                    ' use P5
Servo4         PIN     RB.4                    ' use P4
Servo3         PIN     RB.3                    ' use P3
Servo2         PIN     RB.2                    ' use P2
Servo1         PIN     RB.1                    ' use P1
Servo0         PIN     RB.0                    ' use P0

TX              PIN     RA.3 OUTPUT             ' to PC
RX              PIN     RA.2 INPUT              ' from PC
SCL             PIN     RA.1 INPUT              ' EE clock line (I2C)
SDA             PIN     RA.0 INPUT              ' EE data line (I2C)

' Note: TX, SCL, and SDA lines are not used by this program


' -------------------------------------------------------------------------
' Constants
' -------------------------------------------------------------------------

IsOn            CON     1
IsOff           CON     0

Yes             CON     1
No              CON     0

LO_LIMIT        CON     55                      ' to prevent servo burn-up
HI_LIMIT        CON     245


' Bit dividers for 3.255 uS interrupt
' -- actual code runs at 3.33 uS (within tolerance for bit sampling)

Baud2400        CON     128
Baud4800        CON     64
Baud9600        CON     32
Baud19K2        CON     16
Baud38K4        CON     8

Baud1x0         CON     Baud38K4                ' 1 bit period (ISR counts)
Baud1x5         CON     Baud1x0 * 3 / 2         ' 1.5 bit periods


' -------------------------------------------------------------------------
' Variables
' -------------------------------------------------------------------------

flags           VAR     Byte                    ' (keep global)
isrFlag        VAR     flags.0
rxReady        VAR     flags.1                 ' rx byte waiting

sync            VAR     Byte
chan            VAR     Byte
value           VAR     Byte

rxSerial        VAR     Byte (16)
rxBuf          VAR     rxSerial(0)             ' 8-byte buffer
rxCount        VAR     rxSerial(8)             ' rx bit count
rxDivide       VAR     rxSerial(9)             ' bit divisor timer
rxByte         VAR     rxSerial(10)            ' recevied byte
rxHead         VAR     rxSerial(11)            ' buffer head (write to)
rxTail         VAR     rxSerial(12)            ' buffer tail (read from)
rxBufCnt       VAR     rxSerial(13)            ' # bytes in buffer

svoDataLo       VAR     Byte (16)               ' servo data, 0 - 7
posLo          VAR     svoDataLo(0)            ' position table
pos0           VAR     svoDataLo(0)
pos1           VAR     svoDataLo(1)
pos2           VAR     svoDataLo(2)
pos3           VAR     svoDataLo(3)
pos4           VAR     svoDataLo(4)
pos5           VAR     svoDataLo(5)
pos6           VAR     svoDataLo(6)
pos7           VAR     svoDataLo(7)
svoTixLo       VAR     svoDataLo(8)            ' isr divider
svoFrLo_LSB    VAR     svoDataLo(9)            ' frame timer
svoFrLo_MSB    VAR     svoDataLo(10)
svoIdxLo       VAR     svoDataLo(11)           ' active servo pointer
svoTmrLo       VAR     svoDataLo(12)           ' pulse timer
svoPinLo       VAR     svoDataLo(13)           ' active servo pin

svoDataHi       VAR     Byte (16)               ' servo data, 8 - 15
posHi          VAR     svoDataHi(0)            ' position table
pos8           VAR     svoDataHi(0)
pos9           VAR     svoDataHi(1)
pos10          VAR     svoDataHi(2)
pos11          VAR     svoDataHi(3)
pos12          VAR     svoDataHi(4)
pos13          VAR     svoDataHi(5)
pos14          VAR     svoDataHi(6)
pos15          VAR     svoDataHi(7)
svoTixHi       VAR     svoDataHi(8)            ' isr divider
svoFrHi_LSB    VAR     svoDataHi(9)            ' frame timer
svoFrHi_MSB    VAR     svoDataHi(10)
svoIdxHi       VAR     svoDataHi(11)           ' active servo pointer
svoTmrHi       VAR     svoDataHi(12)           ' pulse timer
svoPinHi       VAR     svoDataHi(13)           ' active servo pin


' =========================================================================
  INTERRUPT NOPRESERVE 300_000                  ' (4)   run every 3.333 uS
' =========================================================================

Mark_ISR:
  \ SETB  isrFlag                               ' (1)


' -------
' RX UART
' -------
'
' UART code by C. Gracey, A. Williams, et al
' -- buffer and flow control mods by Jon Williams
'
Receive:
  ASM
    MOV   FSR, #rxSerial                        ' (2)
    JB    rxBufCnt.3, RX_Done                   ' (2/4) skip if buffer is full
    MOVB  C, RX                                 ' (4)   sample serial input
    TEST  rxCount                               ' (1)   receiving now?
    JNZ   RX_Bit                                ' (2/4) yes, get next bit
    MOV   W, #9                                 ' (1)   no, prep for next byte
    SC                                          ' (1/2)
     MOV  rxCount, W                            ' (1)   if start, load  bit count
    MOV   rxDivide, #Baud1x5                    ' (2)   prep for 1.5 bit periods

RX_Bit:
    DJNZ  rxDivide, RX_Done                     ' (2/4) complete bit cycle?
    MOV   rxDivide, #Baud1x0                    ' (2)   yes, reload bit timer
    DEC   rxCount                               ' (1)   update bit count
    SZ                                          ' (1/2)
     RR   rxByte                                ' (1)   position for next bit
    SZ                                          ' (1/2)
     JMP  RX_Done                               ' (3)

RX_Buffer:
    MOV   W, #rxBuf                             ' (1)   point to buffer head
    ADD   W, rxHead                             ' (1)
    MOV   FSR, W                                ' (1)
    MOV   IND, rxByte                           ' (2)   move rxByte to head
    INC   rxHead                                ' (1)   update head
    CLRB  rxHead.3                              ' (1)   keep 0..7
    INC   rxBufCnt                              ' (1)   update buffer count
    SETB  rxReady                               ' (1)   set ready flag

RX_Done:
  ENDASM


' -------------------------
' Servo Processing - 0 to 7
' -------------------------
'
Test_Servo_Tix_Lo:
  ASM
    BANK  svoDataLo                             ' (1)
    INC   svoTixLo                              ' (1)   update divider
    CJB   svoTixLo, #3, Servos_Done_Lo          ' (4/6) done?
    CLR   svoTixLo                              ' (1)   yes, reset for next

' Low group code below this point runs every 10 uS

Check_Frame_Timer_Lo:
    CJNE  svoFrLo_LSB, #2000 & 255, Inc_Fr_Lo   ' (4/6) svoFrame = 2000 (20 ms)?
    CJNE  svoFrLo_MSB, #2000 >>  8, Inc_Fr_Lo   ' (4/6)
    CLR   svoFrLo_LSB                           ' (1)   yes, reset
    CLR   svoFrLo_MSB                           ' (1)
    MOV   svoPinLo, #%0000_0001                 ' (2)   start servo sequence
    CLR   svoIdxLo                              ' (1)   point to servo 0
    MOV   FSR, #posLo                           ' (2)
    MOV   svoTmrLo, IND                         ' (2)
    JMP   Refesh_Servo_Outs_Lo                  ' (3)

Inc_Fr_Lo:
    INC   svoFrLo_LSB                           ' (1)   INC svoFrLo
    ADDB  svoFrLo_MSB, Z                        ' (2)

Check_Servo_Timer_Lo:
    TEST  svoPinLo                              ' (1)   any servos on?
    SNZ                                         ' (1)
     JMP  Servos_Done_Lo                        ' (1)   no, exit
    DEC   svoTmrLo                              ' (1)   yes, update timer
    SZ                                          ' (1)   still running?
     JMP  Servos_Done_Lo                        ' (1)   yes, exit

Reload_Servo_Timer_Lo:
    INC   svoIdxLo                              ' (1)   point to next servo
    CLRB  svoIdxLo.3                            ' (1)   keep 0 - 7
    MOV   W, #posLo                             ' (1)   get pulse timing
    ADD   W, svoIdxLo                           ' (1)
    MOV   FSR, W                                ' (1)
    MOV   W, IND                                ' (1)
    MOV   svoTmrLo, W                           ' (1)   move to timer

Select_Next_Servo_Lo:
    CLC                                         ' (1)
    RL    svoPinLo                              ' (1)

Refesh_Servo_Outs_Lo:
    MOV   ServosLo, svoPinLo                    ' (2)   update outputs

Servos_Done_Lo:
  ENDASM


' --------------------------
' Servo Processing - 8 to 15
' --------------------------
'
Test_Servo_Tix_Hi:
  ASM
    BANK  svoDataHi                             ' (1)
    INC   svoTixHi                              ' (1)   update divider
    CJB   svoTixHi, #3, Servos_Done_Hi          ' (4/6) done?
    CLR   svoTixHi                              ' (1)   yes, reset for next

' High group code beHiw this point runs every 10 uS

Check_Frame_Timer_Hi:
    CJNE  svoFrHi_LSB, #2000 & 255, Inc_Fr_Hi   ' (4/6) svoFrame = 2000 (20 ms)?
    CJNE  svoFrHi_MSB, #2000 >>  8, Inc_Fr_Hi   ' (4/6)
    CLR   svoFrHi_LSB                           ' (1)   yes, reset
    CLR   svoFrHi_MSB                           ' (1)
    MOV   svoPinHi, #%0000_0001                 ' (2)   start servo sequence
    CLR   svoIdxHi                              ' (1)   point to servo 8
    MOV   FSR, #posHi                           ' (2)
    MOV   svoTmrHi, IND                         ' (2)
    JMP   Refesh_Servo_Outs_Hi                  ' (3)

Inc_Fr_Hi:
    INC   svoFrHi_LSB                           ' (1)   INC svoFrHi
    ADDB  svoFrHi_MSB, Z                        ' (2)

Check_Servo_Timer_Hi:
    TEST  svoPinHi                              ' (1)   any servos on?
    SNZ                                         ' (1)
     JMP  Servos_Done_Hi                        ' (1)   no, exit
    DEC   svoTmrHi                              ' (1)   yes, update timer
    SZ                                          ' (1)   still running?
     JMP  Servos_Done_Hi                        ' (1)   yes, exit

ReHiad_Servo_Timer_Hi:
    INC   svoIdxHi                              ' (1)   point to next servo
    CLRB  svoIdxHi.3                            ' (1)   keep 0 - 7
    MOV   W, #posHi                             ' (1)   get pulse timing
    ADD   W, svoIdxHi                           ' (1)
    MOV   FSR, W                                ' (1)
    MOV   W, IND                                ' (1)
    MOV   svoTmrHi, W                           ' (1)   move to timer

Select_Next_Servo_Hi:
    CLC                                         ' (1)
    RL    svoPinHi                              ' (1)

Refesh_Servo_Outs_Hi:
    MOV   ServosHi, svoPinHi                    ' (2)   update outputs

Servos_Done_Hi:
  ENDASM


ISR_Exit:
  RETURNINT                                     ' (4)


' -------------------------------------------------------------------------
' Subroutine / Function Declarations
' -------------------------------------------------------------------------

RX_BYTE         FUNC    1, 0, 0                 ' receive a byte


' =========================================================================
  PROGRAM Start
' =========================================================================

Start:
  TX = 1                                        ' set TX line to idle

  ' center servos
  PUT posLo, 150, 150, 150, 150, 150, 150, 150, 150
  PUT posHi, 150, 150, 150, 150, 150, 150, 150, 150


Main:
  sync = RX_BYTE
  IF sync <> $FF THEN Main

Get_Channel:
  chan = RX_BYTE
  IF chan = $FF THEN Get_Channel                ' correct for re-sync
  IF chan > 15 THEN Main                        ' check channel range

Get_Value:
  value = RX_BYTE
  IF value = $FF THEN Get_Channel               ' correct for re-sync

  value = value MIN LO_LIMIT                    ' force to legal limits
  value = value MAX HI_LIMIT

Process_Value:
  IF chan < 8 THEN                              ' low group?
    posLo(chan) = value
  ELSE
    chan.3 = 0                                  ' adjust for high group
    posHi(chan) = value
  ENDIF

  GOTO Main


' -------------------------------------------------------------------------
' Subroutine / Function Code
' -------------------------------------------------------------------------

' Use: aByte = RX_BYTE
' -- returns "aByte" from 8-byte circular buffer
' -- will wait if buffer is presently empty
' -- rxBufCnt holds byte count of receive buffer (0 to 8)

FUNC RX_BYTE
  ASM
    JNB   rxReady, @$                           ' wait if empty
    MOV   FSR, #rxSerial                        ' point to serial vars
    MOV   W, #rxBuf
    ADD   W, rxTail                             ' point to rxBuf(rxTail
    MOV   FSR, W
    MOV   __PARAM1, IND                         ' get byte at tail
    INC   rxTail                                ' update tail
    CLRB  rxTail.3                              ' keep 0..7
    DEC   rxBufCnt                              ' update buffer count
    SNZ                                         ' exit if not zero
     CLRB rxReady                               ' else clear ready flag
    MOV   FSR, #__DEFAULT
  ENDASM
  ENDFUNC


' =========================================================================
' User Data
' =========================================================================
Jon McPhalen
EFX-TEK Hollywood Office