Prop-SX Multi-drop -- 12 Servos x 8 Boards

JonnyMac · June 10, 2008, 08:58:30 PM

I was asked if I could create a version of the Prop-SX VSA slave program that would allow multiple Prop-SX controllers to be used. The answer is "Yes."

This program will drive 12 servos (P0..P11) and allows the board to be addressed (%000 to %111) using the P12..P14 SETUP jumpers. In order to allow multi-dropping (as with our accessories) you must connect to the board using the Serial Inverter, and build a splitter cable so that you can connect to multiple Prop-SX boards. Serial input is now on P15 (the DB-9 port is no longer used).

Note: This is not a good approach if you want a Prop-SX on one side of the yard while the second is on the other -- TTL serial has almost no noise immunity and this could be problematic.

[Edit]: Posted updated version 16 JUN 2009

' =========================================================================
'
' File...... Servo12_VSA.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 P11... Servo control
' P12 to P14... Board Address (0 to 7)
' P15.......... Serial Input (from serial inverter)
'
' The protocol is identical to the SEETRON (www.seetron.com) MiniSSC:
'
' <sync><channel><value>
'
' sync...... $FF (255)
'
' channel... 0 to 11 (addr 0), 12 to 23 (board 1), etc.
'
' value..... 55 to 245 for servos (150 = center)
' -- these values can be adjusted in the code
'
' This version runs at 57.6K

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

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

ID "Servo12"

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

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

ServosHi PIN RC ' servos / control
RX PIN RC.7 INPUT ' P15, SETUP = UP, no ULN
Addr2 PIN RC.6 INPUT ' P14 SETUP, no ULN
Addr1 PIN RC.5 INPUT ' P13 SETUP, no ULN
Addr0 PIN RC.4 INPUT ' P12 SETUP, no ULN
Servo11 PIN RC.3 OUTPUT ' P11
Servo10 PIN RC.2 OUTPUT ' P10
Servo9 PIN RC.1 OUTPUT ' P9
Servo8 PIN RC.0 OUTPUT ' P8

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

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

' -------------------------------------------------------------------------
' 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 divider for 2.480 uS interrupt

Baud57K6 CON 7

Baud1x0 CON Baud57K6 ' 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

addr VAR Byte ' board address
chLo VAR Byte ' legal channel range
chHi VAR Byte

tmpB1 VAR Byte
tmpW1 VAR Word

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)
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 400_000 ' (4) run every 2.5 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, #4, 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, #2016 & 255, Inc_Fr_Lo ' (4/6) svoFrame = 2016 (20 ms)?
CJNE svoFrLo_MSB, #2016 >> 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 11
' --------------------------
'
Test_Servo_Tix_Hi:
ASM
BANK svoDataHi ' (1)
INC svoTixHi ' (1) update divider
CJB svoTixHi, #4, Servos_Done_Hi ' (4/6) done?
CLR svoTixHi ' (1) yes, reset for next

' High group code below this point runs every 10 uS

Check_Frame_Timer_Hi:
CJNE svoFrHi_LSB, #2016 & 255, Inc_Fr_Hi ' (4/6) svoFrame = 2016 (20 ms)?
CJNE svoFrHi_MSB, #2016 >> 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

Reload_Servo_Timer_Hi:
INC svoIdxHi ' (1) point to next servo
CLRB svoIdxHi.2 ' (1) keep 0 - 3
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:
TX2 = 1 ' set TX line to idle

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

Check_Addr:
addr = ServosHi & %0111_0000 ' read SETUP jumpers
SWAP addr ' move to low nib
chLo = addr * 12 ' calc low channel
chHi = chLo + 11 ' calc high channel

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 < chLo THEN Main ' check channel range
IF chan > chHi THEN Main

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:
chan = chan // 12
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
' =========================================================================

News:

Prop-SX Multi-drop -- 12 Servos x 8 Boards

JonnyMac

June 10, 2008, 08:58:30 PM Last Edit: June 16, 2009, 10:09:06 AM by JonnyMac