Prop-SX - 16 Servos with VSA

JonnyMac · February 27, 2008, 08:59:01 AM

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
' =========================================================================

News:

Prop-SX - 16 Servos with VSA

JonnyMac

February 27, 2008, 08:59:01 AM Last Edit: June 16, 2009, 10:08:44 AM by JonnyMac