'
'   File......
'   Purpose...
'   Author.... Duncan McRee, Sorrento Technologies
'              Copyright (c) 2008 Sorrento Technologies
'              All Rights Reserved
'   E-mail.... dmcree@san.rr.com
'   Started... 12/8/2008
'   Updated... 
'
' =========================================================================


' -------------------------------------------------------------------------
' Program Description
' -------------------------------------------------------------------------
'  Model Railroad Digital Command Control Decoder for controlling 4 switches with servos 
'  The servos are ordinary R?C plane type. It will work well as is for Futaba and HiTec and
'  related brands.  Others from 
'  Inputs:  DCC signal from track - listen for my address
'           Push buttons on fascia for manual control
'           3 jumpers for programming
'                 PRG1 - new address - stores address of last decoder address recieved while jumper is on
'                 PRG2 - reverse servo endpoints - set jumper and then toggle the servo to be reversed with the buttons
'                 PRG3 - reverse frog polarity - set jumper and then toggle the switch to have its polarity reversed 
'  Outputs: 4 Servo PWM signal .9 ms - 1.5 ms at a 20ms frame rate
'           4 SPDT relay - Switched track power for points (may be left off if not needed)
'           4 TTL logic signals for panel lights or for using in control systems
'           A serial LCD line that is meant for debugging and reprogramming and not for everyday use
'           A status LED that flahses to give indications of program status
'	LED flashes
'           Startup - 1 flash
'           Address sucessfully decoded - 1 flash
'           New address accepted - 3 flashes
'           reverse points - 2 flashes
'
'  EEPROM - save address and servo states.
'     turnouts will remember their last state on power down
' Routes = any address above 512-999 is interpreted as a route.  Instead of changing the switches
'  according to the command all 4 switches are moved to the positions in the route
'  note - more than one decoder can have the same route address, in this way more than 4 switches can
'  switched at teh same time by programming the route over several decoders
'  Program Route - set all the switches as you want them in the route, set teh program address jumper and 
'  enter an accessory address in the range 512-999 on the throttle.
'  Command a Route - with the jumper removed, enter the address of th route - all the switches will move back
'  to the remembered positions.
'  There is room for 256 routes.

'  Routes can be erased by resetting the memory for the decoder by powering up while the reversing jumper is set.
'
'  DCC details -  Accesories in DCC are addressed in groups of 4:
'        Group 1 - 1,2,3,4
'        Group 2 - 5,6,7,8
'        Group 3 - 9, 10, 11, 12
'        Group 4 - 13, 14, 15, 16 ...
'       i.e. (group#-1)*4 + 1
'  That means the servo1 can have the DCC addresses 1, 5, 9, 13 ...
'  and servo2 similarly can have DCC addresses      2, 6, 10, 14 ...

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


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

ID              "4SvAcDc1"

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


' -------------------------------------------------------------------------
' I/O Pins
' -------------------------------------------------------------------------
UnusedRA        PIN	RA    INPUT PULLUP 
UnusedRB        PIN	RB    INPUT PULLUP 
UnusedRC        PIN	RC    INPUT PULLUP 

'  LCD for debugging - not meant to be connected permanently
LcdTx		PIN	RA.0  OUTPUT

' conditioned DCC signal - shifted to 0-5V TTL levels
DccIn		PIN	RA.1  INPUT PULLUP

'  Jumper low to program
ProgSet		PIN	RA.2  INPUT PULLUP      ' pull low to program

' Status LED for verifying operation
ledStatus	PIN	RA.3  OUTPUT		' To indicate status to user

'  PWM Servo outputs
SwServo1	PIN	RB.0  OUTPUT		' Points movement
SwServo2	PIN	RB.1  OUTPUT		' Points movement
SwServo3	PIN	RB.2  OUTPUT		' Points movement
SwServo4	PIN	RB.3  OUTPUT		' Points movement

' Manual overrides
Button1		PIN	RB.4  INPUT PULLUP	' hi, pulse low to change state of servo1
Button2		PIN	RB.5  INPUT PULLUP	' hi, pulse low to change state of 2
Button3		PIN	RB.6  INPUT PULLUP	' hi, pulse low to change state of 3
Button4		PIN	RB.7  INPUT PULLUP	' hi, pulse low to change state of 4

' Outputs for controlling signals or a relay for power control of frog points
Sw1Out		PIN	RC.4  OUTPUT		' State of switch 1 for external controls (relay/signal)
Sw2Out		PIN	RC.5  OUTPUT
Sw3Out		PIN	RC.6  OUTPUT		
Sw4Out		PIN	RC.7  OUTPUT

' EEPROM memory controls
SDA             PIN     RC.0  INPUT  NOPULLUP   ' EE data line (I2C)
SCL             PIN     RC.1  OUTPUT NOPULLUP 	' EE clock line (I2C)

RevServo	PIN	RC.2  INPUT PULLUP      ' To reverse servo endpoints up pull low while switching servo w/ button
RangeJmp	PIN	RC.3  INPUT PULLUP	' Sets the range the servo traverses from normal to full



' -------------------------------------------------------------------------
' Constants
' -------------------------------------------------------------------------
VersionNo	CON	$25
'  DCC timing - pulse width modulation code  --------
'   xxxxxxxx________     One
'   <58us  >    		half-bit time is 58 microseconds
'   xxxxxxxxxxxxxx__________  Zero half-bit time is >= 100 microseconds
'   < 100us      >
'   < 87us   >|      Split the difference - less than this is one and greater is 0
TickUs		CON	10_4	' ISR tick time is 10.4 microseconds
RxTimer		CON	7	' number of ISR cyces for 87 useconds -1 for lateness

WaitingEdge	CON     0	'  Constants for rxStatus
TimingPulse     CON	1
Waiting0	CON	3

' EEPROM -------------------------
SlaveWr         CON     $A0
SlaveRd         CON     $A1
Ack             CON     0
Nak             CON     1
'EEPROM Memory map
' EEPROM memory base addresses - hi byte - each "page" is 256 bytes
'  but we only use 16 bytes of it
dccPage		CON	0
VirginAddr	CON	0
'  Route Table consists of 3 byte triplets
'  first byte of table has the number of routes available
RouteTableHi	CON	1	' hi byte of route table
RouteTableLo	CON     0	' first is byte containing size of table
EE_LAST		CON	$1FFF

' servo timers -------------------
TimerHi		CON	10			' 10 * 192 * .0104ms = 19.97 ms
TimerLo		CON     192			' Servo frame rate
ServoCenter	CON	144			' 144 * .0104 ms = 1.50 ms
ServoMax	CON	86			' .9 ms
ServoMin	CON	202			' 2.1 ms
SwRight		CON	164			' good starting values for switches
SwLeft		CON	124

'  true or false -----------------
SumErr		CON	0
NoErr		CON	1

M_Run		CON	1
M_Program	CON	0

ShortRange	CON	1
FullRange	CON	0

Pressed		CON	0
NotPressed	CON	1

Armed		CON	1
NotArmed	CON	0

Closed		CON	1
Thrown		CON	0
		

' -------------------------------------------------------------------------
' Variables
' -------------------------------------------------------------------------
idx1            VAR     Byte                    ' loop control
nBuf		VAR	Byte			' number in DCC recieve buffer
rxCount		VAR	Byte
rxDivide	VAR	Byte
rxByte		VAR	Byte
rxStatus	VAR	Byte
tmpB1           VAR     Byte                    ' subroutine work vars
tmpB2           VAR     Byte
Byte1		VAR	tmpB2
tmpB3           VAR     Byte
Byte2		VAR	tmpB3			' to make coding easier to read
tmpW1		VAR	Word
flags		VAR	BYTE		
ackNak          VAR     flags.0                 ' for I2C routines
rxBit		VAR	flags.1
receiving	VAR	flags.2
startbit	VAR	flags.3
dccReady	VAR	flags.4
doRestore	VAR	flags.5
servosOn	VAR     flags.6		' used to suppress ervo opreation until system ready
buttons		VAR	Byte
  but1Armed	VAR	buttons.0	' button ready - i.e went back to high state
  but2Armed	VAR	buttons.1
  but3Armed	VAR	buttons.2
  but4Armed	VAR	buttons.3
numOnes		VAR	Byte
servoState	VAR	Byte
  reverse1	VAR	servoState.0
  reverse2	VAR	servoState.1
  reverse3	VAR	servoState.2
  reverse4	VAR	servoState.3
 S1State	VAR	servoState.4	
 S2State	VAR	servoState.5	
 S3State	VAR	servoState.6	
 S4State	VAR	servoState.7
memHi		VAR	Byte
memLo		VAR	Byte
memVal		VAR	Byte

dccConfig	VAR	Byte (16)
 virgin		VAR	dccConfig (0)
 version	VAR     dccConfig (1)
 dccAddr1	VAR	dccConfig (3)	' encoded address in 3-byte mode
 dccAddr2	VAR	dccConfig (4)
 servoThrown	VAR	dccConfig (5)	'  values of servo timer closed and thrown	
 servoClosed	VAR	dccConfig (6)
 servo1Pulse	VAR	dccConfig (7)		
 servo2Pulse	VAR	dccConfig (8)		
 servo3Pulse	VAR	dccConfig (9)		
 servo4Pulse	VAR	dccConfig (10)		

rxBuf		VAR	Byte (16)
charBufHi	VAR	rxBuf(0)	' 3 - byte message positions
charBufLo	VAR	rxBuf(1)
checkSum	VAR	rxBuf(2)

servoData	VAR	Byte (16)
Timer20L	VAR	servoData (0)
Timer20H	VAR	servoData (1)
sTimer1		VAR	servoData (2)
sPos1		VAR	servoData (3)
sTimer2		VAR	servoData (4)
sPos2		VAR	servoData (5)
sTimer3		VAR	servoData (6)
sPos3		VAR	servoData (7)
sTimer4		VAR	servoData (8)
sPos4		VAR	servoData (9)
but1Timer	VAR	servoData(10)
but2Timer	VAR	servoData(11)
but3Timer	VAR	servoData(12)
but4Timer	VAR	servoData(13)
moveTimer	VAR	servoData(14)
'dwnTimer	VAR	servoData(15)




' =========================================================================
  INTERRUPT 96_000			' 10.4 useconds
' =========================================================================

ISR_Start:
   GOTO Interrupt_Handler


' =========================================================================
' Subroutine / Function Declarations
' =========================================================================

DELAY           SUB     1, 2                       ' delay in milliseconds

PUT_EE          SUB     3                       ' write byte(s) to EEPROM
GET_EE          FUNC    1, 2                 	' read byte from EEPROM
GET_EE_INC	FUNC	1			' read byte at tmpW1 and INC tmpW1
READ_EE		SUB	0

' I2C routines used by subs/funcs above
I2C_START       SUB     0                       ' generate I2C Start
I2C_STOP        SUB     0                       ' generate I2C Stop
I2C_OUT         SUB     1, 1                    ' write byte to SDA
I2C_IN          FUNC    1, 1                    ' read byte from SDA

SAVE_SYS	SUB	0
RESTORE_SYS	SUB	0

CHECK_BUTTONS	SUB	0
CHECK_ROUTES	SUB	0
ADD_ROUTE	SUB	0

SET_SERVOS	SUB	0
GET_SERVO	FUNC	1,2
MOVE_SERVOS	SUB	0
FLASH_LED	SUB	1		' Flash LED n times


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

Start:
   LcdTx = 1
   Timer20L = TimerLo
   Timer20H = TimerHi
   IF RevServo = Pressed THEN
       doRestore = 1
   ENDIF
   RESTORE_SYS
   SET_SERVOS
INIT_SERVOS:
    sPos1 = servo1Pulse
    sPos2 = servo2Pulse
    sPos3 = servo3Pulse
    sPos4 = servo4Pulse
   servosOn = 1
   FLASH_LED 1

Main:
  CHECK_BUTTONS
  IF dccReady = 1 THEN
     ' handle the message
' Check if the message has a checksum error 
CHECK_ERRSUM:
     idx1 = 1
     tmpB1 = rxBuf(0)
     DO WHILE idx1 < nBuf
        tmpB1 = tmpB1 XOR rxBuf(idx1)
	INC idx1
     LOOP
     IF tmpB1 <> 0 GOTO Clear
CHECK_SIZE:
     IF nBuf <> 3 GOTO Clear		' If its too short or long its not for us
     ' TODO  - add 4 byte message handling
     Byte1 = charBufHi
     Byte2 = charBufLo
CHECK_IF_DECODER:
     tmpB1 = Byte1 AND %1100_0000	'  valid messages are %10AAAAAA %1aaaCDDD
     IF tmpB1 <> %10_00_0000 GOTO CLEAR
     tmpB1 = Byte2 AND %1000_0000
     IF tmpB1 = 0 GOTO CLEAR
     ' normal operations unless programming jumper set
     IF ProgSet = M_Run THEN
	' check for service mode
        ' is it addressed to me ?
	IF dccAddr1 <> Byte1 GOTO Clear
        tmpB1 = Byte2 AND %0111_0000
	IF dccAddr2 <> tmpB1 THEN
		' check for route table address
		CHECK_ROUTES
		GOTO Clear
	ENDIF
	' which servo?
	tmpB1 = Byte2 AND %0000_0110
        tmpB1 = tmpB1 >> 1
	
	IF Byte2.0 = Thrown THEN
		BRANCH tmpB1, S1t, S2t, S3t, S4t
	ELSE
		BRANCH tmpB1, S1c, S2c, S3c, S4c
	ENDIF
        S1t:
	     s1State = Thrown
             GOTO Flash
        S2t:
	     s2State = Thrown
             GOTO Flash
        S3t:
	     s3State = Thrown
             GOTO Flash
        S4t:
	     s4State = Thrown
             GOTO Flash
        S1c:
 	     s1State = Closed
             GOTO Flash
        S2c:
 	     s2State = Closed
             GOTO Flash
        S3c:
 	     s3State = Closed
             GOTO Flash
        S4c:
 	     s4State = Closed
Flash:
        SET_SERVOS
	FLASH_LED 1
     ELSE  ' Program mode
     '  use the valid decoder message for new address
	' use as a route if above 512 ($200)
	IF Byte2.5 = 0 THEN
		ADD_ROUTE
		GOTO Verify
	ENDIF
	dccAddr1 = Byte1
        dccAddr2 = Byte2 AND %0111_0000
        SAVE_SYS
        ' flash LED to let user know its saved
        FLASH_LED 3
Verify:
        GOTO Clear
     ENDIF
     
Clear:
     dccReady = 0			' message handled
     nBuf = 0
  ENDIF

  GOTO Main


' -------------------------------------------------------------------------
' Subroutine / Function Code
' -------------------------------------------------------------------------

'  check to see if this address is in our route table
SUB CHECK_ROUTES
	tmpW1_MSB = RouteTableHi
	tmpW1_LSB = RouteTableLo
	idx1 = GET_EE_INC
	charBufLo = charBufLo AND %0111_0110	' mask out unwanted bits
	DO WHILE idx1 > 0
		' read a triplet 
		memHi = GET_EE_INC		' note GET_EE uses tmpW1 but it will have same value upon return so its cool
		memLo = GET_EE_INC
		memval = GET_EE_INC
		DEC idx1
		IF memHi <> charBufHi GOTO Next_Entry
		IF memLo <> charBufLo GOTO Next_Entry
		servoState = memval
		SET_SERVOS
		FLASH_LED 2
		RETURN 
Next_Entry:
	LOOP
	RETURN	
ENDSUB

'  add this address to the route table
SUB ADD_ROUTE
	' advance table to end, add the triplet, and update table counter
	tmpW1_MSB = RouteTableHi
	tmpW1_LSB = RouteTableLo
	charBufLo = charBufLo AND %0111_0110	' mask out unwanted bits
	idx1 = GET_EE_INC
	DO WHILE idx1 > 0
		' advance one triplet 
		memHi = GET_EE_INC		' note GET_EE_INC uses tmpW1 as address and then increments it one
		memLo = GET_EE_INC
		' if address matches then overwrite old one and we are done
		IF memHi = charBufHi THEN
			IF charbufLo = memLo THEN
				PUT_EE tmpW1, servoState
				FLASH_LED idx1
				RETURN
			ENDIF
		ENDIF
		memVal = GET_EE_INC
		DEC idx1
	LOOP
	PUT_EE tmpW1, charBufHi
	INC tmpW1
	PUT_EE tmpW1, charBufLo
	INC tmpW1
	PUT_EE tmpW1, servoState
	tmpW1_MSB = RouteTableHi
	tmpW1_LSB = RouteTableLo
	idx1 = GET_EE tmpW1
	INC idx1
	PUT_EE tmpW1, idx1
	FLASH_LED idx1
	RETURN
ENDSUB

'  Usage: svalue = GET_SERVO switch_state, reverseflag
FUNC GET_SERVO
	IF RangeJmp = ShortRange THEN
		servoThrown = SwLeft
		servoClosed = SwRight
	ELSE
		servoThrown = ServoMax
		servoClosed = ServoMin
	ENDIF
	IF __PARAM1 = Thrown THEN
		IF __PARAM2 = 0 THEN
			tmpB1 = servoThrown
		ELSE
			tmpB1 = servoClosed
		ENDIF
	ELSE
		IF __PARAM2 = 0 THEN
			tmpB1 = servoClosed
		ELSE
			tmpB1 = servoThrown
		ENDIF
	ENDIF
	RETURN tmpB1
ENDFUNC
' ------------------------------------------
'
' Checks to switches - a pulse toggles state
SUB CHECK_BUTTONS
	IF but1Timer > 5 THEN
		IF but1Armed = Armed THEN
			IF ProgSet = M_Program THEN
				' put servo in middle of range for adding servo horn
				servo1Pulse = ServoCenter
				GOTO Re_Arm
			ENDIF
			IF RevServo = M_Program THEN 		'swap thrown servo end point values
				reverse1 = ~reverse1
				SAVE_SYS
				FLASH_LED 2
			ELSE
				IF s1State = Closed THEN
					s1State = Thrown
				ELSE
					s1State = Closed
				ENDIF
			ENDIF
 			SET_SERVOS
			but1Armed = NotArmed
		ENDIF
	ENDIF
	IF but2Timer > 5 THEN
		IF but2Armed = Armed THEN
			IF ProgSet = M_Program THEN
				' put servo in middle of range for adding servo horn
				servo2Pulse = ServoCenter
				GOTO Re_Arm
			ENDIF
			IF RevServo = M_Program THEN 		'swap thrown servo end point values
				reverse2 = ~reverse2
				SAVE_SYS
				FLASH_LED 2
			ELSE
				IF s2State = Closed THEN
					S2State = Thrown
				ELSE
					s2State = Closed

				ENDIF
			ENDIF
			SET_SERVOS
			but2Armed = NotArmed
		ENDIF
	ENDIF
	IF but3Timer > 5 THEN
		IF but3Armed = Armed THEN
			IF ProgSet = M_Program THEN
				' put servo in middle of range for adding servo horn
				servo3Pulse = ServoCenter
				GOTO Re_Arm
			ENDIF
			IF RevServo = M_Program THEN 		'swap thrown servo end point values
				reverse3 = ~reverse3
				SAVE_SYS
				FLASH_LED 2
			ELSE
				IF s3State = Closed THEN
					s3State = Thrown
				ELSE
					s3State = Closed
				ENDIF
			ENDIF
			SET_SERVOS
			but3Armed = NotArmed
		ENDIF
	ENDIF
	IF but4Timer > 5 THEN
		IF but4Armed = Armed THEN
			IF ProgSet = M_Program THEN
				' put servo in middle of range for adding servo horn
				servo4Pulse = ServoCenter
				GOTO Re_Arm
			ENDIF
			IF RevServo = M_Program THEN 		'swap thrown servo end point values
				reverse4 = ~reverse4
				SAVE_SYS
				FLASH_LED 2
			ELSE
			IF s4State = Closed THEN
					s4State = Thrown
				ELSE
					s4State = Closed
				ENDIF
			ENDIF
			SET_SERVOS
			but4Armed = NotArmed
		ENDIF
	ENDIF

Re_Arm:
        IF Button1 = NotPressed THEN
		but1Armed = Armed
		but1Timer = 0
	ENDIF
        IF Button2 = NotPressed THEN
		but2Armed = Armed
		but2Timer =0
	ENDIF
        IF Button3 = NotPressed THEN
		but3Armed = Armed
		but3Timer = 0
	ENDIF
        IF Button4 = NotPressed THEN
		but4Armed = Armed
		but4Timer = 0
	ENDIF
ENDSUB

' -----------------------------------------------
'
' Sets servos according to servoState
'
SUB SET_SERVOS
	tmpB1 = s1State
	servo1Pulse = GET_SERVO tmpB1, reverse1
	tmpB1 = s2State
	servo2Pulse = GET_SERVO tmpB1, reverse2
	tmpB1 = s3State
	servo3Pulse = GET_SERVO tmpB1, reverse3
	tmpB1 = s4State
	servo4Pulse = GET_SERVO tmpB1, reverse4
	sw1Out = s1State
	sw2Out = s2State
	sw3Out = s3State
	sw4Out = s4State
	'SAVE_SYS
ENDSUB

	

'  MOVE_SERVOS  - moves the towards their target according to
'     sPosx - where servo x is now
'     servoxPulse - where servo is going to
'    reoutie compares the 2 and inc/dec the servo as needed

SUB MOVE_SERVOS
	IF sPos1 < servo1Pulse THEN
		INC sPos1
	ELSEIF sPos1 > servo1Pulse THEN
		DEC sPos1
	ENDIF
	IF sPos2 < servo2Pulse THEN
		INC sPos2
	ELSEIF sPos2 > servo2Pulse THEN
		DEC sPos2
	ENDIF
	IF sPos3 < servo3Pulse THEN
		INC sPos3
	ELSEIF sPos3 > servo3Pulse THEN
		DEC sPos3
	ENDIF
	IF sPos4 < servo4Pulse THEN
		INC sPos4
	ELSEIF sPos4 > servo4Pulse THEN
		DEC sPos4
	ENDIF
ENDSUB

'  Usage: FLASH_LED ntimes
'
SUB FLASH_LED
	tmpB1 = __PARAM1
	FOR tmpB2 = 1 TO tmpB1
		ledStatus = 1
		DELAY 50
		ledstatus = 0
		DELAY 50
	NEXT		
ENDSUB

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

' ---------------[EEPROM Routines]---------------------------------------------

SUB SAVE_SYS
        virgin = $AA
	FOR idx1 = 0 TO 15
		PUT_EE idx1, DccPage, dccConfig(idx1)
	NEXT
	PUT_EE 16, DccPage, servoState		' save reversal information
ENDSUB

SUB RESTORE_SYS
	IF doRestore = 1 THEN Reset_Values
        tmpB1 = GET_EE VirginAddr, DccPage       ' check to see if the system has ever been saved
	IF tmpB1 <> $AA GOTO No_Previous_Save
	FOR idx1 = 0 to 15
		dccConfig(idx1) = GET_EE idx1, DccPage
	NEXT
	servoState = GET_EE 16, DccPage		' load reversal flags
	RETURN
Reset_Values:
No_Previous_Save:
      ' initialize the servo values with the defaults
	version = VersionNo
	servoState = $F0
	'  Set address to 1
 	dccAddr1 = $81		'  %10_000001 %1111_0000
	dccAddr2 = $70		'  %10_AAAAAA %1aaa_CDDD  where a = 1's complement - i.e. a 1 = 0
	PUT_EE RouteTableLo, RouteTableHi, $00
        SAVE_SYS
        FLASH_LED 5
	RETURN
ENDSUB
		
' -------------------------------------------------------------------------
' Use: PUT_EE address, byteVal
' -- writes 'byteVal1' to 'address'
' -- 'address' is a word (0 to 65535)

SUB PUT_EE
  tmpW1 = __WPARAM12
  tmpB1 = __PARAM3
  I2C_START
  I2C_OUT SlaveWr                               ' send slave ID
  I2C_OUT tmpW1_MSB                             ' send address, high byte
  I2C_OUT tmpW1_LSB                             ' send address, low byte
  I2C_OUT tmpB1                                 ' send data byte
  I2C_STOP                                      ' finish

  DO                                            ' let write cycle finish
    I2C_START
    I2C_OUT SlaveWr
  LOOP UNTIL ackNak = Ack

  ENDSUB

' -------------------------------------------------------------------------
' Use: byteVal = GET_EE address
' -- reads 'byteVal' from EEPROM location 'address'
' -- 'address' is a word (0 to 65535)

FUNC GET_EE
  tmpW1 = __WPARAM12
  READ_EE
  RETURN tmpB1
  ENDFUNC

FUNC GET_EE_INC
  READ_EE
  INC tmpW1
  RETURN tmpB1
  ENDFUNC

SUB READ_EE
  I2C_START
  I2C_OUT SlaveWr                               ' send slave ID
  I2C_OUT tmpW1_MSB                             ' send address, high byte
  I2C_OUT tmpW1_LSB                             ' send address, low byte
  I2C_START
  I2C_OUT SlaveRd
  tmpB1 = I2C_IN Nak
  I2C_STOP
RETURN

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

' Use: I2C_START
' -- generates I2C start condition on SDA/SCL pins

SUB I2C_START
  I2CSTART SDA
  ENDSUB

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

' Use: I2C_STOP
' -- generates I2C stop condition on SDA/SCL pins

SUB I2C_STOP
  I2CSTOP SDA
  ENDSUB

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

' Use: I2C_OUT byteVal
' -- writes 'byteVal' to SDA pin
' -- affects global var "ackNak"
' -- EE address pointer must be preset before call

SUB I2C_OUT
  I2CSEND SDA, __PARAM1, ackNak
  ENDSUB

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

' Use: byteVal = I2C_IN AckBit
' -- reads 'byteVal' from SDA pin
' -- EE address pointer must be preset before call

FUNC I2C_IN
  ackNak = __PARAM1.0
  I2CRECV SDA, tmpB3, ackNak
  RETURN tmpB3
  ENDFUNC

' Use: DELAY in 26 ms increments
' -- delay x ticks
' -- small codewise but very inaccurate
' -- DON'T USE IF TIMING CRITICAL

SUB DELAY
  IF __PARAMCNT = 1 THEN
    tmpW1 = __PARAM1                            ' save byte value
  ELSE
    tmpW1 = __WPARAM12                          ' save word value
  ENDIF
  PAUSE tmpW1
ENDSUB


' -------------------------------------------------------------------------
ASM
ORG $0728
ENDASM
Interrupt_Handler:
Recieve:
   IF rxStatus = WaitingEdge THEN      	' are we waiting for an edge?
	IF dccIn = 1 THEN		' yes, then check for one  
	    rxDivide = RxTimer		' found one, load timer
            rxStatus = TimingPulse	' load pulse flag
	ENDIF
  ENDIF

  IF rxStatus = TimingPulse THEN
	IF rxDivide = 0 THEN		' have we timed out?
  	    IF dccIn = 1 THEN
	        rxBit = 0		' test the value
	    ELSE
                rxBit = 1
		INC numOnes
           ENDIF
BitReady:
   IF receiving = 1 THEN	' are we recieving ?
     IF rxCount <> 0 THEN	' if yes, are we in the middle of a byte?
        rxByte = rxByte << 1
        rxByte = rxByte + rxBit
	DEC rxCount
	IF Z = 1 THEN
             rxBuf(nBuf) = rxByte
             INC nBuf
             nBuf = nBuf AND $0F
        ENDIF
     ELSE  			' finished byte, check for stop bit (1) or continue (0)
        IF rxBit = 0 THEN
             rxCount = 8		' go for another byte
             rxByte = 0
        ELSE
             receiving = 0	'  we are done!
             dccReady = 1		' mark message for checking in main
        ENDIF
      ENDIF
  ELSEIF numOnes > 13 THEN    ' not receiving, check for preamble - preamble is 14 or more ones
      IF rxBit = 0 THEN         ' if preamble OK then check for start bit (0)
          rxCount = 8
          rxByte = 0
          receiving = 1
          numOnes = 0
      ENDIF
  ENDIF

           rxStatus = Waiting0		' waiting for a new space between pulses
        ENDIF
	DEC rxDivide
  ENDIF

  IF rxStatus = Waiting0 THEN  'waiting for new pulse
         IF dccIn = 0 THEN
		rxStatus = WaitingEdge
	ENDIF
  ENDIF


Servo:
ASM
    sb servosOn			' check to see if servos active
    jmp ExitServo
    BANK servoData
    sb swServo1
    jmp :Done1
    dec sTimer1
    sz
    jmp :Done1
    clrb swServo1
'ENDASM
    'sTimer1 = sPos1
'ASM
  MOV FSR,#sPos1                 ;    sTimer1 = sPos1
  MOV __PARAM2,IND              
  MOV FSR,#sTimer1              
  MOV IND,__PARAM2              
:Done1
    BANK servoData
    sb swServo2
    jmp :Done2
    dec sTimer2
    sz
    jmp :Done2
    clrb swServo2
'ENDASM
    'sTimer2 = sPos2
'ASM
  MOV FSR,#sPos2                 ;    sTimer2 = sPos2
  MOV __PARAM2,IND              
  MOV FSR,#sTimer2              
  MOV IND,__PARAM2              
:Done2
    BANK servoData
    sb swServo3
    jmp :Done3
    dec sTimer3
    sz
    jmp :Done3
    clrb swServo3
'ENDASM
'    sTimer3 = sPos3
'ASM
  MOV FSR,#sPos3                 ;    sTimer3 = sPos3
  MOV __PARAM2,IND              
  MOV FSR,#sTimer3              
  MOV IND,__PARAM2              
:Done3
    BANK servoData
    sb swServo4
    jmp :Done4
    dec sTimer4
    sz
    jmp :Done4
    clrb swServo4
'ENDASM
'    sTimer4 = sPos4
'ASM
  MOV FSR,#sPos4                 ;    sTimer4 = sPos4
  MOV __PARAM2,IND              
  MOV FSR,#sTimer4              
  MOV IND,__PARAM2              
:Done4
:Timer20
    BANK servoData
    dec Timer20L
    sz
    jmp ExitServo
    mov Timer20L, #TimerLo
    dec Timer20H
    sz
    jmp ExitServo
    mov Timer20H, #TimerHi
    setb swServo1
    setb swServo2
    setb swServo3
    setb swServo4
    sb Button1
    inc but1Timer
    sb Button2
    inc but2Timer
    sb Button3
    inc but3Timer
    sb Button4
    inc but4Timer
ENDASM
	MOVE_SERVOS
	IF RangeJmp = FullRange THEN
		MOVE_SERVOS		'Go twice as fast is range is set to full
	ENDIF
'ASM
ExitServo:
'ENDASM

ISR_Exit:

  BANK 0
  RETURNINT


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