Simple Servo VP in SX/B
JonnyMac
Posts: 8,942
In the prop industry servos are frequently used for animatronics and special FX. Since we're (EFX-TEK) about to release a Prop-SX controller, I thought it would be fun to write a little framework program that allows our customers to control servos as easily as their digital outputs. What I did, in effect, was make a little servo controller VP. The code is pretty easy, and entirely in SX/B. By using the VP approach one can simply write a position value to a servo and the interrupt code will take care of the refreshing.
The only restriction is that you can't use things like PAUSE, SERIN, etc. -- those functions that are timing-dependant. Since timing is a big part of prop control this framework includes subroutines that use an interrupt-driven timer for delays.
Post Edited (JonnyMac) : 2/7/2007 8:29:51 PM GMT
The only restriction is that you can't use things like PAUSE, SERIN, etc. -- those functions that are timing-dependant. Since timing is a big part of prop control this framework includes subroutines that use an interrupt-driven timer for delays.
' =========================================================================
'
' File...... Prop484-EZ.SXB
' Purpose... Prop Control framework: 4 triggers, 8 outs, 4 servos
' Author.... Jon Williams, EFX-TEK
' Copyright (c) 2007 EFX-TEK
' Some Rights Reserved
' -- see http://creativecommons.org/licenses/by/2.5/
' E-mail.... jwilliams@efx-tek.com
' Started...
' Updated... 06 FEB 2007
'
' =========================================================================
' -------------------------------------------------------------------------
' Program Description
' -------------------------------------------------------------------------
'
' This program provides a framework for simple prop control that may use
' multiple trigger inputs, digital outputs, as well as servos.
'
' An ISR (Interrupt Servo Routine) is used to process the servos so that
' the user needs only set the current position (in the pos(x) array) -- no
' updating or delays are required (as with the Prop-1 or Prop-2).
'
' Note: SERIN, SEROUT, PAUSE, PAUSEUS cannot be used with this framework.
' Use the WAIT_x subroutines for program delays.
' -------------------------------------------------------------------------
' Conditional Compilation Symbols
' -------------------------------------------------------------------------
' -------------------------------------------------------------------------
' Device Settings
' -------------------------------------------------------------------------
DEVICE SX28, OSCXT2, TURBO, STACKX, OPTIONX, BOR42
FREQ 20_000_000
ID "P448-EZ"
' -------------------------------------------------------------------------
' IO Pins
' -------------------------------------------------------------------------
Trigger3 PIN RC.7 ' inputs
Trigger2 PIN RC.6
Trigter1 PIN RC.5
Trigger0 PIN RC.4
ServoCtrl PIN RC
Servo3 PIN RC.3 OUTPUT ' servo outputs
Servo2 PIN RC.2 OUTPUT
Servo1 PIN RC.1 OUTPUT
Servo0 PIN RC.0 OUTPUT
Outs PIN RB OUTPUT ' digital outputs
Out7 PIN RB.7
Out6 PIN RB.6
Out5 PIN RB.5
Out4 PIN RB.4
Out3 PIN RB.3
Out2 PIN RB.2
Out1 PIN RB.1
Out0 PIN RB.0
' -------------------------------------------------------------------------
' Constants
' -------------------------------------------------------------------------
IsOn CON 1
IsOff CON 0
' -------------------------------------------------------------------------
' Variables
' -------------------------------------------------------------------------
flags VAR Byte
sync VAR flags.0 ' marks start of frame
idx VAR Byte
tix VAR Word ' (ISR) timer
svoPos VAR Byte (4) ' servo positions
svoFrame VAR Word ' (ISR) servo frame timer
svoIdx VAR Byte ' (ISR) active servo index
svoPin VAR Byte ' (ISR) mask for active servo
svoTimer VAR Byte ' (ISR) servo pulse timer
tmpB1 VAR Byte ' for subs/funcs
tmpB2 VAR Byte
tmpW1 VAR Word
' -------------------------------------------------------------------------
INTERRUPT 100_000 ' run every 10 uS
' -------------------------------------------------------------------------
' The ISR code is setup to run every 10 uS so that the pos(x) values match
' the BS1 PULSOUT parameter for servo control (100 to 200); this simplifies
' the code and uses less variable space. Only one servo pulse is on at a
' given time.
'
' Note that after Servo3 is done the ISR waits until the start of the next
' 20 mS frame to start Servo0.
Update_Timer:
IF tix > 0 THEN ' update delay timer if on
DEC tix
ENDIF
Check_Frame_Timer:
IF svoFrame = 0 THEN ' time for new servo frame?
svoFrame = 2000 ' reload with 20 ms
svoPin = %0001 ' start servo cycle
svoIdx = 0 ' set position pointer
svoTimer = svoPos(0) ' load position timing
sync = IsOn ' mark start of frame
GOTO Refesh_Servo_Outs
ELSE
DEC svoFrame ' no, update running timer
sync = IsOff ' clear sync marker
ENDIF
Check_Servo_Timer:
IF svoPin = 0 THEN ISR_Exit ' abort if none running
DEC svoTimer
IF svoTimer > 0 THEN ISR_Exit ' abort if servo still running
Reload_Servo_Timer:
INC svoIdx ' point to next servo
svoIdx.2 = 0 ' keep 0 - 3
svoTimer = svoPos(svoIdx) ' load position timing
Select_Next_Servo:
svoPin = svoPin << 1
svoPin = svoPin & $0F ' limit to four servos
Refesh_Servo_Outs:
ServoCtrl = ServoCtrl & $F0 ' clear last servo
ServoCtrl = ServoCtrl | svoPin ' set active servo
ISR_Exit:
RETURNINT
' =========================================================================
PROGRAM Start
' =========================================================================
' -------------------------------------------------------------------------
' Subroutine Declarations
' -------------------------------------------------------------------------
WAIT_TIX SUB 1, 2 ' wait in 10 us units
WAIT_MS SUB 1, 2 ' wait in 1 ms units
WAIT_SYNC SUB 0 ' wait for frame
' -------------------------------------------------------------------------
' Program Code
' -------------------------------------------------------------------------
Start:
FOR idx = 0 TO 3
svoPos(idx) = 150 ' start at center
NEXT
Main:
FOR idx = 100 TO 200
svoPos(0) = idx
WAIT_MS 60 ' slow sweep
NEXT
GOTO Main
' -------------------------------------------------------------------------
' Subroutine Code
' -------------------------------------------------------------------------
' Use: WAIT_MS ticks
' -- hold program in 10 uS ticks (1 to 65535)
WAIT_TIX:
IF __PARAMCNT = 1 THEN
tix = __PARAM1 ' get byte value
ELSE
tix = __WPARAM12 ' get word value
ENDIF
DO WHILE tix > 0 ' hold while running
LOOP
RETURN
' -------------------------------------------------------------------------
' Use: WAIT_MS mSecs
' -- hold program in milliseconds (1 to 65535)
WAIT_MS:
IF __PARAMCNT = 1 THEN
tmpW1 = __PARAM1 ' get byte value
ELSE
tmpW1 = __WPARAM12 ' get word value
ENDIF
DO WHILE tmpW1 > 0
tix = 100
DO WHILE tix > 0
LOOP
DEC tmpW1
LOOP
RETURN
' -------------------------------------------------------------------------
' Use: WAIT_SYNC
' -- wait until start of next servo frame (up to 20 mS)
WAIT_SYNC:
DO WHILE sync = IsOff
LOOP
RETURN
' =========================================================================
' User Data
' =========================================================================
Post Edited (JonnyMac) : 2/7/2007 8:29:51 PM GMT
Comments
' ========================================================================= ' ' File...... Prop484.SXB ' Purpose... Prop Control framework: 4 triggers, 8 outs, 4 servos ' Author.... Jon Williams, EFX-TEK ' Copyright (c) 2007 EFX-TEK ' Some Rights Reserved ' -- see http://creativecommons.org/licenses/by/2.5/ ' E-mail.... jwilliams@efx-tek.com ' Started... ' Updated... 06 FEB 2007 ' ' ========================================================================= ' ------------------------------------------------------------------------- ' Program Description ' ------------------------------------------------------------------------- ' ' This program provides a framework for simple prop control that may use ' multiple trigger inputs, digital outputs, as well as servos. ' ' An ISR (Interrupt Servo Routine) is used to process the servos so that ' the user needs only set the current position (in the pos(x) array) -- no ' updating or delays are required (as with the Prop-1 or Prop-2). The ' ISR has been coded in assembly for maximum efficiency. ' ' Note: SERIN, SEROUT, PAUSE, PAUSEUS cannot be used with this framework. ' Use the WAIT_x subroutines for program delays. ' ------------------------------------------------------------------------- ' Conditional Compilation Symbols ' ------------------------------------------------------------------------- ' ------------------------------------------------------------------------- ' Device Settings ' ------------------------------------------------------------------------- DEVICE SX28, OSCXT2, TURBO, STACKX, OPTIONX, BOR42 FREQ 20_000_000 ID "Prop484" ' ------------------------------------------------------------------------- ' IO Pins ' ------------------------------------------------------------------------- Trigger3 PIN RC.7 ' inputs Trigger2 PIN RC.6 Trigter1 PIN RC.5 Trigger0 PIN RC.4 ServoCtrl PIN RC ' servo outputs Servo3 PIN RC.3 OUTPUT Servo2 PIN RC.2 OUTPUT Servo1 PIN RC.1 OUTPUT Servo0 PIN RC.0 OUTPUT Outs PIN RB OUTPUT ' digital outputs Out7 PIN RB.7 Out6 PIN RB.6 Out5 PIN RB.5 Out4 PIN RB.4 Out3 PIN RB.3 Out2 PIN RB.2 Out1 PIN RB.1 Out0 PIN RB.0 ' ------------------------------------------------------------------------- ' Constants ' ------------------------------------------------------------------------- IsOn CON 1 IsOff CON 0 ' ------------------------------------------------------------------------- ' Variables ' ------------------------------------------------------------------------- flags VAR Byte sync VAR flags.0 ' marks start of frame idx VAR Byte ' loop control tix VAR Word ' (ISR) for delay timing tmpB1 VAR Byte ' for subs/funcs tmpB2 VAR Byte tmpW1 VAR Word ' banked variables svoData VAR Byte (16) ' bank servo data (ISR) pos VAR svoData(0) ' position table pos0 VAR svoData(0) pos1 VAR svoData(1) pos2 VAR svoData(2) pos3 VAR svoData(3) svoFrame_LSB VAR svoData(9) ' frame timer (for 20 mS) svoFrame_MSB VAR svoData(10) svoIdx VAR svoData(11) ' active servo index svoPin VAR svoData(12) ' mask for active servo svoTimer VAR svoData(13) ' servo pulse timer svoMask VAR svoData(14) ' mask for disabled pins ' ------------------------------------------------------------------------- INTERRUPT NOPRESERVE 100_000 ' run every 10 uS ' ------------------------------------------------------------------------- ' The ISR code is setup to run every 10 uS so that the pos(x) values match ' the BS1 PULSOUT parameter for servo control (100 to 200); this simplifies ' the code and uses less variable space. Only one servo pulse is on at a ' given time. ' ' Note that after Servo3 is done the ISR waits until the start of the next ' 20 mS frame to start Servo0. Update_Timer: BANK 0 IF tix > 0 THEN ' delay timer running? DEC tix ' yes, decrement ENDIF Check_Frame_Timer: ASM BANK svoData CJNE svoFrame_LSB, #0, Update_Frame_Timer ' frame timer done? CJNE svoFrame_MSB, #0, Update_Frame_Timer MOV svoFrame_LSB, #2000 & 255 ' yes, svoFrame = 2000 MOV svoFrame_MSB, #2000 >> 8 MOV svoPin, #%00000001 ' start servo sequence CLR svoIdx ' point to servo 0 MOV FSR, #pos MOV svoTimer, IND BANK 0 SETB sync ' sync bit on BANK svoData JMP Refesh_Servo_Outs Update_Frame_Timer: SUB svoFrame_LSB, #1 ' DEC svoFrame SUBB svoFrame_MSB, /C BANK 0 CLRB sync ' sync bit off BANK svoData Check_Servo_Timer: TEST svoPin ' any servos on? SNZ JMP ISR_Exit ' no, exit DEC svoTimer ' yes, update timer SZ ' still running? JMP ISR_Exit ' yes, exit Reload_Servo_Timer: INC svoIdx ' point to next servo CLRB svoidx.2 ' keep 0 - 3 MOV W, #pos ' get pulse timing ADD W, svoIdx MOV FSR, W MOV svoTimer, IND ' move to timer BANK svoData Select_Next_Servo: CLC RL svoPin CLRB svoPin.4 ' limit to four servos Refesh_Servo_Outs: MOV W, ServoCtrl ' copy port bits AND W, #%11110000 ' clear last servo OR W, svoPin ' set active servo AND W, svoMask ' apply disable mask MOV ServoCtrl, W ' update outputs ISR_Exit: BANK 0 ENDASM RETURNINT ' ========================================================================= PROGRAM Start ' ========================================================================= ' ------------------------------------------------------------------------- ' Subroutine Declarations ' ------------------------------------------------------------------------- WAIT_TIX SUB 1, 2 ' wait in 10 us units WAIT_MS SUB 1, 2 ' wait in 1 ms units WAIT_SYNC SUB 0 ' wait for frame SET_OUTPUTS SUB 1 ' set all outputs GET_TRIGGERS FUNC 1, 0 ' read all trigger pins ' ------------------------------------------------------------------------- ' Program Code ' ------------------------------------------------------------------------- Start: FOR idx = 0 TO 3 pos(idx) = 150 ' center all NEXT svoMask = %00001111 ' all servos active Main: ' put your control code here GOTO Main ' ------------------------------------------------------------------------- ' Subroutine Code ' ------------------------------------------------------------------------- ' Use: WAIT_MS ticks ' -- hold program in 10 uS ticks (1 to 65535) WAIT_TIX: IF __PARAMCNT = 1 THEN tix = __PARAM1 ' get byte value ELSE tix = __WPARAM12 ' get word value ENDIF DO WHILE tix > 0 ' hold while running LOOP RETURN ' ------------------------------------------------------------------------- ' Use: WAIT_MS mSecs ' -- hold program in milliseconds (1 to 65535) WAIT_MS: IF __PARAMCNT = 1 THEN tmpW1 = __PARAM1 ' get byte value ELSE tmpW1 = __WPARAM12 ' get word value ENDIF DO WHILE tmpW1 > 0 tix = 100 DO WHILE tix > 0 LOOP DEC tmpW1 LOOP RETURN ' ------------------------------------------------------------------------- ' Use: WAIT_SYNC ' -- wait until start of next servo frame (up to 20 mS) WAIT_SYNC: DO WHILE sync = IsOff LOOP RETURN ' ------------------------------------------------------------------------- ' Use: SET_OUTPUTS byteVal ' -- sets output bits to "byteVal" SET_OUTPUTS: Outs = __PARAM1 RETURN ' ------------------------------------------------------------------------- ' Use: result = GET_TRIGGERS ' -- returns byte containing trigger bit levels ' -- result.0 is Trigger0; result.3 is Trigger3 GET_TRIGGERS: tmpB1 = RC & $F0 ' copy trigger bits SWAP tmpB1 ' move to low nib RETURN tmpB1 ' ========================================================================= ' User Data ' =========================================================================Cool code...
I can't wait until the Prop-SX board are out! Great idea putting the EEPROM on them. Any ETA ?
Bean.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheap used 4-digit LED display with driver IC·www.hc4led.com
Low power SD Data Logger www.sddatalogger.com
SX-Video Display Modules www.sxvm.com
Stuff I'm selling on ebay http://search.ebay.com/_W0QQsassZhittconsultingQQhtZ-1
"USA Today has come out with a new survey - apparently, three out of every four people make up 75% of the population." - David Letterman
Thanks so much for making this (and all the other) code available.
I think you know how much it helps people to learn. I can read the help files and books all day long, but a clear cut example shows me the light.
Chris
Thanks; coming from you that means a lot. We don't have an ETA yet, but will be showing first-run prototypes at trade shows (Haunt X and Transworld) over the next few weeks.