Prop + Servo Control (Advanced)

[JonnyMac]

This program shows what experienced programmers can do with SX/B -- but you don't have to be advanced to use it (just keep your code between Main..GOTO Main).  By using an interrupt this program adds a 4-channel servo controller, so all you have to do is "set and forget" using the pos() array -- and the servo values are Prop-1 compatible. The background code takes care of servo refreshing for you.

Program updated 09 JAN 2009.

' =========================================================================
'
'   File...... Prop484.SXB
'   Purpose... Prop Control framework: 4 triggers, 8 outs, 4 servos
'   Author.... Jon Williams, EFX-TEK
'              Copyright (c) 2007-2009 EFX-TEK
'              Some Rights Reserved
'              -- see http://creativecommons.org/licenses/by/2.5/
'   E-mail.... jwilliams@efx-tek.com
'   Started...
'   Updated... 09 JAN 2009
'
' =========================================================================


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

ID              "Prop484"

DEVICE          SX28, OSCXT2, TURBO, STACKX, OPTIONX, BOR42
FREQ            20_000_000


' -------------------------------------------------------------------------
' 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                    ' global (do not move)
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


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:
  IF tix > 0 THEN                               ' delay timer running?
    DEC tix                                     '   yes, decrement
  ENDIF


' -------------
' Servo Handler
' -------------
'
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, #%0000_0001                   ' start servo sequence
    CLR   svoIdx                                ' point to servo 0
    MOV   FSR, #pos
    MOV   svoTimer, IND
    SETB  sync                                  ' sync bit on
    JMP   Refesh_Servo_Outs

Update_Frame_Timer:
    SUB   svoFrame_LSB, #1                      ' DEC svoFrame
    SUBB  svoFrame_MSB, /C

Check_Servo_Timer:
    TEST  svoPin                                ' any servos on?
    SNZ
    JMP   Servo_Exit                            ' no, exit
    DEC   svoTimer                              ' yes, update timer
    SZ                                          ' still running?
     JMP  Servo_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

Select_Next_Servo:
    CLC
    RL    svoPin
    CLRB  svoPin.4                              ' limit to four servos

Refesh_Servo_Outs:
    MOV   W, ServoCtrl                          ' copy port bits
    AND   W, #%1111_0000                        ' clear last servo
    OR    W, svoPin                             ' set active servo
    AND   W, svoMask                            ' apply disable mask
    MOV   ServoCtrl, W                          ' update outputs

Servo_Exit:
    BANK  $00
  ENDASM


  RETURNINT


' -------------------------------------------------------------------------
' Subroutine Declarations
' -------------------------------------------------------------------------

DELAY_TIX       SUB     1, 2                    ' wait in 10 us units
DELAY_MS        SUB     1, 2                    ' replaces PAUSE

WAIT_SYNC       SUB     0                       ' wait for frame

SET_OUTPUTS     SUB     1                       ' set all outputs
GET_TRIGGERS    FUNC    1, 0                    ' read all trigger pins


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

Start:
  svoMask = %0000_1111                          ' all servos active

  PUT pos, 150, 150, 150, 150                   ' center all servos


Main:

  ' your code

  GOTO Main


' -------------------------------------------------------------------------
' Subroutine Code
' -------------------------------------------------------------------------

' Use: DELAY_TIX ticks
' -- hold program in 10 uS ticks (1 to 65_535)

SUB DELAY_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
  ENDSUB

' -------------------------------------------------------------------------

' Use: DELAY_MS mSecs
' -- replaces PAUSE
' -- hold program in milliseconds (1 to 65_535)

SUB DELAY_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
  ENDSUB

' -------------------------------------------------------------------------

' Use: WAIT_SYNC
' -- wait until start of next servo frame (up to 20 mS)

SUB WAIT_SYNC
  \ CLRB sync                                   ' clear sync flag
  \ JNB  sync, $                                ' wait for next
  ENDSUB

' -------------------------------------------------------------------------

' Use: SET_OUTPUTS byteVal
' -- sets output bits to "byteVal"

SUB SET_OUTPUTS
  Outs = __PARAM1
  ENDSUB

' -------------------------------------------------------------------------

' Use: result = GET_TRIGGERS
' -- returns byte containing trigger bit levels
' -- result.0 is Trigger0; result.3 is Trigger3

FUNC GET_TRIGGERS
  tmpB1 = RC & $F0                              ' copy trigger bits
  SWAP tmpB1                                    ' move to low nib
  RETURN tmpB1
  ENDFUNC


' =========================================================================
' User Data
' =========================================================================