' =========================================================================
'
'   File...... VEX_Demod-v2.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
'
' Version 2: converts output of channels 5 and 6 to digital control; the
' upper button for these channels turns the output bit on, the lower
' button turns them off.


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

'{$DEFINE Test_Mode_OFF}


' -------------------------------------------------------------------------
' 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 
Control1	PIN	RB.4			' digital control pins
Control2	PIN	RB.5


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

SvoMask		CON	%11110000		' preserve RB.7..RB.4

IsOn		CON	1
IsOff		CON	0


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

pulseTmr	VAR	Word
svoPntr		VAR	Byte

tmpB1		VAR	Byte			' work vars
tmpB2		VAR	Byte
tmpW1		VAR	Word



' =========================================================================
  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 = %0001_0000

Ctrl_Port1:
  DO WHILE PPM = 1				' complete framing pulse
  LOOP
  pulseTmr = 0
  DO WHILE PPM = 0				' measure timing pulse
    PAUSEUS 5
    INC pulseTmr
  LOOP

  '{$IFDEF Test_Mode}
    WATCH pulseTmr
    BREAK
    GOTO Start
  '{$ENDIF}

  IF pulseTmr < 150 THEN			' short pulse?
    Control1 = IsOn
  ELSE
    IF pulseTmr > 250 THEN			' long pulse?
      Control1 = IsOff
    ENDIF
  ENDIF

Ctrl_Port2:
  DO WHILE PPM = 1
  LOOP
  pulseTmr = 0
  DO WHILE PPM = 0
    PAUSEUS 5
    INC pulseTmr
  LOOP
  IF pulseTmr < 150 THEN
    Control2 = IsOn
  ELSE
    IF pulseTmr > 250 THEN
      Control2 = IsOff
    ENDIF
  ENDIF

  GOTO Wait_For_Sync

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


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