' =========================================================================
'
'   File...... VEX_Demod.SXB
'   Purpose... Servo demodulator for VEX receiver
'   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... 02 MAR 2007
'
' =========================================================================


' -------------------------------------------------------------------------
' Program Description
' -------------------------------------------------------------------------
'
' Simple program to convert PPM stream from VEX RC receiver to servo
' output pulses.
'
' See: http://www.vexfan.com/viewtopic.php?t=227


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


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

DEVICE          SX28, OSCXT2, TURBO, STACKX, OPTIONX, BOR42
FREQ            50_000_000
ID              "VexDemod"


' -------------------------------------------------------------------------
' IO Pins
' -------------------------------------------------------------------------

PPM		PIN	RA.0 INPUT		' PPM in (pull-up w/10k)

SvoPort		PIN	RB   OUTPUT
Servo1		PIN	RB.0 
Servo2		PIN	RB.1 
Servo3		PIN	RB.2 
Servo4		PIN	RB.3 
Servo5		PIN	RB.4
Servo6		PIN	RB.5


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

SvoMask		CON	%11000000		' preserve RB.7..RB.6


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

pulseTmr	VAR	Word
svoPntr		VAR	Byte


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


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


' -------------------------------------------------------------------------
' Program Code
' -------------------------------------------------------------------------

Start:
  SvoPort = %00000000

Wait_For_Sync:
  pulseTmr = 0
  DO WHILE PPM = 0
    PAUSEUS 1
    INC pulseTmr
  LOOP
  IF pulseTmr < 4000 THEN Wait_For_Sync

Main:
  svoPntr = %0000_0001
  DO
    SvoPort = SvoPort | svoPntr			' start servo pulse
    DO WHILE PPM = 1				' complete framing pulse
    LOOP
    DO WHILE PPM = 0				' complete timing pulse
    LOOP
    SvoPort = SvoPort & SvoMask			' stop servo pulse
    svoPntr = svoPntr << 1			' point to next servo
  LOOP UNTIL svoPntr = %0100_0000
  GOTO Wait_For_Sync


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


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