;;======================================================================;;
;;				RC radio to Lego Power Functions converter		 		;;
;;======================================================================;;
;;									;;
;; Program:		RC radio to Lego Power Functions converter		;;
;; Code:		Jindra Fucik ( http://www.fucik.name )		;;
;; Platform:		Microchip PIC16F15313, 4 Mhz			;;
;; Date:		12.04.2019					;;
;; First release:	12.04.2019					;;
;; LastDate:		12.04.2019					;;
;;									;;
;;======================================================================;;

; This program is for a RC radio to Lego PF converter

; Processor PIC 16F15323 running at 4MHz internal oscillator.
;
; Revisions:
;
; 12/04/2019	Start of writing code.



;                        PIC16F15313
;         +---------U---------+
;        -|Vdd             Vss|-
;   Bin2 -|RA5/PWM6   PWM3/RA0|-- Ain1
;   Bin1 -|RA4/PWM5   PWM4/RA1|-- Ain2
; servo2 -|RA3/CCP2   CCP1/RA2|-- servo1
;         +-------------------+  
;

; DRV8833
;
;|IN1|IN2|OUT1|OUT2| Function
;+---+---+----+----+----------
;| 0 | 0 | Z  | Z  | disconnected (Off)
;+---+---+----+----+----------
;| 0 | 1 | L  | H  | forward
;+---+---+----+----+----------
;| 1 | 0 | H  | L  | rewerse
;+---+---+----+----+----------
;| 1 | 1 | L  | L  | brake
;+---+---+----+----+----------

; R-sense max 0.133R - in ideal case two 0R27 in paralel for each channel
;
; Timers:
; Timer1 - used by CCP for capture time
; Timer2 - used by PWM as time base (1200Hz period)
;
 
; Timer2
; 1/1200 = (pr2 + 1) * 4 * 1/4000000 * presc
; 833 = (pr2 + 1) * presc
; presc=1 832 = pr2
; presc=4 207 = pr2
; presc=16 51 = pr2

; Timer1
; 37 us per step --> Timer0 is used as source
; min servo position 900us ~ 24
; max servo position 3000us ~ 81

; Timer0
; TMR0H = 36 as target value
; mode is 8 bit
; source Fosc/4

; Fosc = 4MHz = 600 instructions min per pulse
; Fosc = 1MHz = 150 instructions min per pulse


#define		__VERNUM	0x01
#define		__VERREV	" "
#define		__VERDAY	0x12
#define		__VERMONTH	0x04
#define		__VERYEAR	0x19

    list    p=16F15313,r=hex

    errorlevel -302	; Register in operand not in bank 0.  Ensure that bank bits are correct.
    ;errorlevel -305 ; Using default destination of 1 (file).

; PIC16F15313 Configuration Bit Settings
; Assembly source line config statements

#include "p16f15313.inc"

; CONFIG1
; __config 0xFFEC
 __CONFIG _CONFIG1, _FEXTOSC_OFF & _RSTOSC_HFINT1 & _CLKOUTEN_OFF & _CSWEN_ON & _FCMEN_ON
; CONFIG2
; __config 0xF7FF
 __CONFIG _CONFIG2, _MCLRE_OFF & _PWRTE_OFF & _LPBOREN_OFF & _BOREN_ON & _BORV_LO & _ZCD_OFF & _PPS1WAY_OFF & _STVREN_ON
; CONFIG3
; __config 0xFF9F
 __CONFIG _CONFIG3, _WDTCPS_WDTCPS_31 & _WDTE_OFF & _WDTCWS_WDTCWS_7 & _WDTCCS_SC
; CONFIG4
; __config 0xFFEF
 __CONFIG _CONFIG4, _BBSIZE_BB512 & _BBEN_OFF & _SAFEN_ON & _WRTAPP_OFF & _WRTB_OFF & _WRTC_OFF & _WRTSAF_OFF & _LVP_OFF
; CONFIG5
; __config 0xFFFF
 __CONFIG _CONFIG5, _CP_OFF


#define		RAMINI0		0x020		; 80 bytes
#define		RAMINI1		0x0A0		; 80 bytes
#define		RAMINI2		0x120		; 80 bytes
;#define		RAMINI3		0x1A0		; 80 bytes
;#define		RAMINI4		0x220		; 80 bytes
;#define		RAMINI5		0x2A0		; 80 bytes
#define		RAMINT		0x070		; 16 bytes


; ----- Macros

#define		DNOP		goto	$+1

; ----- Constant values

FXTAL		equ	D'4000000'		; Internal oscillator frequency

; PMD = PERIPHERAL MODULE DISABLE
PMD0_INI	equ	b'01000001'	; CLKRMD CLKR enabled; SYSCMD SYSCLK enabled; FVRMD FVR disabled; IOCMD IOC disaabled; NVMMD NVM enabled; 
PMD1_INI	equ	b'10000000'	; TMR0MD TMR0 enabled; TMR1MD TMR1 enabled; TMR2MD TMR2 enabled; NCOMD DDS(NCO) disabled; 
PMD2_INI	equ	b'01100111'	; ZCDMD ZCD disabled; CMP1MD CMP1 disabled; ADCMD ADC disabled; CMP2MD CMP2 disabled; DAC1MD DAC1 disabled; 
PMD3_INI	equ	b'00000000'	; CCP2MD CCP2 enabled; CCP1MD CCP1 enabled; PWM4MD PWM4 disabled; PWM3MD PWM3 disabled; PWM6MD PWM6 disabled; PWM5MD PWM5 disabled; 
PMD4_INI	equ	b'01010001'	; CWG1MD CWG1 disabled; MSSP1MD MSSP1 disabled; UART1MD EUSART disabled;
PMD5_INI	equ b'00011110'	; CLC3MD CLC3 disabled; CLC4MD CLC4 disabled; CLC1MD CLC1 disabled; CLC2MD CLC2 disabled; 

; PIN manager
LATA_INI    equ 0x00	    ; all outputs to zero (RA2, RA4, RA5 is out)

TRISA_INI   equ b'00001100' ; RA0, RA1 pwmA, RA2 servoA in,  RA3 servoB in, RA4, RA5 pwmB

ANSELA_INI  equ b'00000000' ; all digital

WPUA_INI    equ	0x0C	    ; enable WPU for inputs

ODCONA_INI  equ	0x00	    ; disable open drain outputs

RA0PPS_INI  equ 0x0B	    ; RA0->PWM3;
RA1PPS_INI  equ 0x0C	    ; RA5->PWM4;
RA4PPS_INI  equ 0x0D	    ; RA5->PWM5;
RA5PPS_INI  equ 0x0E	    ; RA5->PWM6;

; oscilator management
OSCCON1_INI equ 0x60	    ; NOSC HFINTOSC; NDIV 1; 
;OSCCON2 is read only
OSCCON3_INI equ 0x00	    ; CSWHOLD may proceed; 
OSCEN_INI   equ 0x00	    ; MFOEN disabled; LFOEN disabled; ADOEN disabled; EXTOEN disabled; HFOEN disabled; 
OSCFRQ_INI  equ 0x02	    ; HFFRQ 4_MHz; 
;OSCFRQ_INI  equ 0x03	    ; HFFRQ 8_MHz; 
;OSCFRQ_INI  equ 0x05	    ; HFFRQ 16_MHz; 
OSCSTAT_INI equ 0x00	    ; MFOR not ready; do not understand, it is read only
;OSCTUNE_INI equ 0x20	    ; HFTUN 32; do not understand why MCC set this?
OSCTUNE_INI equ 0x00	    ; HFTUN 0; default

; Comparator CM1 management
CM1CON0_INI equ 0x00	    ; C1HYS disabled; C1EN disabled; C1POL not inverted; C1SYNC asynchronous;
CM1CON1_INI equ 0x00	    ; C1INTN intFlag_neg; C1INTP intFlag_pos; interrupt on change, disable
CM1NCH_INI  equ 0x00	    ; NCH CIN0- (RA0)
CM1PCH_INI  equ 0x00	    ; PCH C1IN0+ (RA1)
;CM2CON0_INI equ 0x00	    ; C2HYS disabled; C2EN disabled; C2POL not inverted; C2SYNC asynchronous;

; Timer2 management			; none 60us for F-Bus
T2CLKCON_INI	equ 0x01    ; T2CS FOSC/4; 
T2HLT_INI   equ 0x00    ; T2PSYNC Not Synchronized; T2MODE Software control; T2CKPOL Rising Edge; T2CKSYNC Not Synchronized; 
T2RST_INI   equ 0x00	; T2RSEL T2INPPS pin; 
T2PR_INI    equ d'51'	; T2PR 59 (0.3B); 
;T2TMR = 0x00; TMR2 0; 
T2CON_INI   equ 0xC0	; T2CKPS 1:16; T2OUTPS 1:1; TMR2ON on; 
; Timer 2 is not used yet

; Timer1 management		; LED blink
T1GCON_INI	equ 0x00	; T1GE disabled; T1GTM disabled; T1GPOL low; T1GGO done; T1GSPM disabled; 
T1GATE_INI	equ 0x00	; GSS T1G_pin; 
T1CLK_INI	equ 0x09	; ClockSelect Timer0 out; 
;TMR1H = 0x00;    //TMR1H 0; 
;TMR1L = 0x00;    //TMR1L 0; 
T1CON_INI	equ 0x05	; CKPS 1:1; nT1SYNC do_not_synchronize; TMR1ON enabled; T1RD16 disabled; 

; Timer0 management		; F-Bus delays
T0CON1_INI	equ 0x50	; T0CS FOSC/4; T0CKPS 1:1 prescaler; T0ASYNC not_synchronised; 
T0CON0_INI	equ 0x80	; T0OUTPS 1:1; T0EN enabled; T016BIT 8-bit; 
TMR0H_INI	equ d'36'	; in 8 bit mode the TMR0H is compared same as timer2 -> 7C = 124 = 2000 cycles * postscaler = 20.000 ms
;TMR0L = 0x00; TMR0L 0; 

; EUSART 1 management
;BAUD1CON_INI	equ 0x08	; ABDOVF no_overflow; SCKP Non-Inverted; BRG16 16bit_generator; WUE disabled; ABDEN disabled; 
;RC1STA_INI	equ 0xD0	; SPEN enabled; RX9 9-bit; CREN enabled; ADDEN disabled; SREN disabled; 
;TX1STA_INI	equ 0x64	; TX9 9-bit; TX9D 0; SENDB sync_break_complete; TXEN enabled; SYNC asynchronous; BRGH hi_speed; CSRC slave; 
;SP1BRGL_INI	equ 0x1D	; SP1BRGL 29; 
;SP1BRGL_INI	equ 0x1F	; SP1BRGL 31; <- 8MHz
;SP1BRGH_INI	equ 0x00	; SP1BRGH 0;   <- 8MHz
;SP1BRGL_INI	equ 0x0F	; SP1BRGL 15;  <- 4MHz
;SP1BRGH_INI	equ 0x00	; SP1BRGH 0;   <- 4MHz
; EUSART 2 management (used for PC comm at 19200)
;PC_RC1STA_INI	equ 0x90	; SPEN enabled; RX9 8-bit; CREN enabled; ADDEN disabled; SREN disabled; 
;PC_TX1STA_INI	equ 0x24	; TX9 8-bit; TX9D 0; SENDB sync_break_complete; TXEN enabled; SYNC asynchronous; BRGH hi_speed; CSRC slave; 
;PC_SP1BRGL_INI	equ 0x67	; SP1BRGL 103;  <- 4MHz
;PC_SP1BRGH_INI	equ 0x00	; SP1BRGH 0;   <- 4MHz
;PC_SP1BRGL_INI	equ 0x33	; SP1BRGL 51;  <- 4MHz
;PC_SP1BRGH_INI	equ 0x00	; SP1BRGH 0;   <- 4MHz

CCP1CON_INI	equ 0x83		; CCPx is enabled, Capture mode: every edge of CCPx input
CCP1CAP_INI	equ 0x00		; source is CCP1PPS
CCP1PPS_INI	equ b'00000010'	; RA2->CCP1;

CCP2CON_INI	equ 0x83		; CCPx is enabled, Capture mode: every edge of CCPx input
CCP2CAP_INI	equ 0x00		; source is CCP2PPS
CCPP2PS_INI	equ b'00000011'	; RA3->CCP2;

PWMxCON_INI	equ	0x90		; PWM is enabled, PWM active low

;; EUSART 2 management (used for PC comm at 19200)
;;BAUD2CON_INI	equ 0x08	; ABDOVF no_overflow; SCKP Non-Inverted; BRG16 16bit_generator; WUE disabled; ABDEN disabled; 
;RC2STA_INI	equ 0x90	; SPEN enabled; RX9 8-bit; CREN enabled; ADDEN disabled; SREN disabled; 
;TX2STA_INI	equ 0x24	; TX9 8-bit; TX9D 0; SENDB sync_break_complete; TXEN enabled; SYNC asynchronous; BRGH hi_speed; CSRC slave; 
;;SP2BRGL_INI	equ 0x67	; SP1BRGL 103;	<- 8MHz
;;SP2BRGH_INI	equ 0x00	; SP1BRGH 0;	<- 8MHz
;SP2BRGL_INI	equ 0x33	; SP1BRGL 51;	<- 4MHz
;;SP2BRGH_INI	equ 0x00	; SP1BRGH 0;	<- 4MHz

PIE0_INI	equ 0x00	; none used
PIE1_INI	equ 0x00	; none used
PIE2_INI	equ 0x00	; none used
PIE3_INI	equ 0x00	; none used
PIE4_INI	equ 0x00	; none used
PIE5_INI	equ 0x00	; none used
PIE6_INI	equ 0x03	; CCP1 interrupt is enabled, CCP2 interrupt is enabled
PIE7_INI	equ 0x00	; none used

INTC_INI	equ 0xC0	; GIE enable, PIE enable

; --- Pin assignement

; --- Port A
; RA0 LED2 - yellow
; RA1 LED1 - green
; RA2 Rx1  - Rx data from XpressNet
; RA3 Rx2  - Rx data from PC for config
; RA4 RxTx - Rx/Tx switch
; RA5 Tx   - Tx data to XpressNet or PC

#define SERVO1	PORTA,2	; Servo 1 input
#define SERVO2	PORTA,3	; Servo 2 input

; --- EEPROM Section
;  here is no EEPROM, we have to use storage area flash (SAF)
;  SAF is defined as Last Program Memory Address - 7Fh
;  0x07FF - 0x7F = 0x780
;  Row Erase (words) = 32

#define		SAF_INI	0x00780
#define		SAF_LAST	0x007FF

; ----- Variables

; --- Internal RAM Section

; --- Top on all banks
	cblock RAMINT
SERVO1U:2	; CCP value vhen servo 1 signal go lo->hi
SERVO2U:2	; CCP value vhen servo 1 signal go lo->hi
SERVO1P		; Servo 1 pulse width
SERVO2P		; Servo 2 pulse width

FLAGS		; various flags
	endc

; --- Bank 0

	cblock RAMINI0
; none

	endc

; --- Bank 1

	cblock RAMINI1
; none
	endc

	cblock RAMINI2
; none
	endc


;----- Flags

#define	NEW_S1	FLAGS,0	; new servo 1 sample is available
#define	NEW_S2	FLAGS,1	; new servo 2 sample is available

; --------------- Program Section --------------------------------------


		org	0x000

PowerUp:
	clrf	INTCON			; Disable all interrupts
	clrf	PCLATH			; Tables on page 0
	;clrf	STATUS			; reset flags
	goto	INIT


; ----- ISR (Interrupt Service Routines)

		org	0x004

Interrupt:

	BANKSEL	PIR0			; BANK 14
IntRx:
	btfss	PIR6,CCP1IF		; interrupt from servo1
	goto	IntCCP2			; no servo1, then check next
	
	bcf		PIR6,CCP1IF		; clear flag for next int
	BANKSEL	PORTA			; BANK 0
	btfss	SERVO1			; check pulse direction
	goto	IntServo1Fall	; falling edge
IntServo1Rise:				; rising edge
	BANKSEL	CCPR1L			; BANK6
	movf	CCPR1L,w		; save CCPR1
	movwf	SERVO1U
	movf	CCPR1H,w
	movwf	SERVO1U+1
	goto	IntCCP1End		; that is all
IntServo1Fall:
	BANKSEL	CCPR1L			; BANK6
; Dest = Dest - Source
	movf	SERVO1U,W
	subwf	CCPR1L,W		; W  = CCPR1L - SERVO1U
	movwf	SERVO1U			; SERVO1U = CCPR1L - SERVO1U
	movf	SERVO1U+1,W
	subwfb	CCPR1H,W		; W  = CCPR1H - SERVO1U+1 - C
	movwf	SERVO1U+1		; SERVO1U+1 = CCPR1H - SERVO1U+1
	btfss	STATUS,Z		; if SERVO1U+1 is not zerro = pulse too long
	goto	IntCCP1End
	movf	SERVO1U,w		; use value for new PWM calculation
	movwf	SERVO1P
	bsf		NEW_S1
	;goto	IntCCP1End

IntCCP1End:
	BANKSEL	PIR0			; BANK 14
IntCCP2:
	btfss	PIR6,CCP2IF		; interrupt from servo2
	goto	IntRestore		; no servo2, then finish

	bcf		PIR6,CCP2IF		; clear flag for next int
	BANKSEL	PORTA			; BANK 0
	btfss	SERVO2			; check pulse direction
	goto	IntServo2Fall	; falling edge
IntServo2Rise:				; rising edge
	BANKSEL	CCPR2L			; BANK6
	movf	CCPR2L,w		; save CCPR1
	movwf	SERVO2U
	movf	CCPR2H,w
	movwf	SERVO2U+1
	goto	IntCCP2End		; that is all
IntServo2Fall:
	BANKSEL	CCPR2L			; BANK6
; Dest = Dest - Source
	movf	SERVO2U,W
	subwf	CCPR2L,W		; W  = CCPR2L - SERVO2U
	movwf	SERVO2U			; SERVO2U = CCPR2L - SERVO2U
	movf	SERVO2U+1,W
	subwfb	CCPR2H,W		; W  = CCPR2H - SERVO2U+1 - C
	movwf	SERVO2U+1		; SERVO2U+1 = CCPR2H - SERVO2U+1
	btfss	STATUS,Z		; if SERVO2U+1 is not zerro = pulse too long
	goto	IntCCP2End
	movf	SERVO2U,w		; use value for new PWM calculation
	movwf	SERVO2P
	bsf		NEW_S2
IntCCP2End:
IntRestore:
		retfie

; ----- Tables on first 256 bytes


;	if ($ > d'255') 
;		ERROR "  Tables exceded page 0.   If it works, why do you change it?   "
;	endif


; ----- Initialization

INIT:

	BANKSEL	PORTA		; BANK 0
	movlw	LATA_INI	; all outputs to zero (only RA2 is out)
	movwf	LATA
	;movlw	LATC_INI	; all outputs to zero (only RC5,RC4,RC3)
	;movwf	LATC

	movlw	TRISA_INI	; RA2 is output, RA3 is MCLR
	movwf	TRISA
	;movlw	TRISC_INI	; RC0=CMP0out, RC1=eusart.Rx, RC2=cfg button, RC3-5=LEDs
	;movwf	TRISC

	; PMD = PERIPHERAL MODULE DISABLE
	BANKSEL	PMD0		; BANK 15
	movlw	PMD0_INI	; CLKRMD CLKR enabled; SYSCMD SYSCLK enabled; FVRMD FVR disabled; IOCMD IOC enabled; NVMMD NVM enabled; 
	movwf	PMD0
	movlw	PMD1_INI	; TMR0MD TMR0 enabled; TMR1MD TMR1 enabled; TMR2MD TMR2 enabled; NCOMD DDS(NCO) enabled; 
	movwf	PMD1
	movlw	PMD2_INI	; ZCDMD ZCD disabled; CMP1MD CMP1 enabled; ADCMD ADC disabled; CMP2MD CMP2 disabled; DAC1MD DAC1 disabled; 
	movwf	PMD2
	movlw	PMD3_INI	; CCP2MD CCP2 disabled; CCP1MD CCP1 disabled; PWM4MD PWM4 disabled; PWM3MD PWM3 disabled; PWM6MD PWM6 disabled; PWM5MD PWM5 disabled; 
	movwf	PMD3
	movlw	PMD4_INI	; CWG1MD CWG1 disabled; MSSP1MD MSSP1 disabled; UART1MD EUSART enabled; 
	movwf	PMD4
	movlw	PMD5_INI	; CLC3MD CLC3 disabled; CLC4MD CLC4 disabled; CLC1MD CLC1 disabled; CLC2MD CLC2 disabled; 
	movwf	PMD5

; PIN manager
	BANKSEL	ANSELA		; BANK 62

	movlw	ANSELA_INI	; RA0 and RA1 to analog = comparator inuts
	movwf	ANSELA
	;movlw	ANSELC_INI	; all digital
	;movwf	ANSELC

	movlw	WPUA_INI	; disable WPU
	movwf	WPUA
	;movlw	WPUC_INI	; disable WPU
	;movwf	WPUC
	movlw	ODCONA_INI	; disable open drain outputs
	movwf	ODCONA
	;movlw	ODCONC_INI	; disable open drain outputs
	;movwf	ODCONC
	;movlw	RC0PPS_INI	; RC0->CMP1:C1OUT;
	;movwf	RC0PPS
	movlw	RA0PPS_INI	; RA0->PWM3;
	movwf	RA0PPS
	movlw	RA1PPS_INI	; RA0->PWM4;
	movwf	RA1PPS
	movlw	RA4PPS_INI	; RA0->PWM5;
	movwf	RA4PPS
	movlw	RA5PPS_INI	; RA5->PWM6;
	movwf	RA5PPS

	
	;BANKSEL	RX1DTPPS	; BANK 61
	;movlw	RX1DTPPS_INI	; RA2->EUSART1:RX1;
	;movwf	RX1DTPPS
	;movlw	RX2DTPPS_INI	; RC3->EUSART2:RX;
	;movwf	RX2DTPPS

; oscilator management
	BANKSEL	OSCCON1		; BANK 17
	movlw	OSCCON1_INI	; NOSC HFINTOSC; NDIV 4; 
	movwf	OSCCON1
		;OSCCON2 is read only
	movlw	OSCCON3_INI	; CSWHOLD may proceed; 
	movwf	OSCCON3
	movlw	OSCEN_INI	; MFOEN disabled; LFOEN disabled; ADOEN disabled; EXTOEN disabled; HFOEN disabled; 
	movwf	OSCEN
	movlw	OSCFRQ_INI	; HFFRQ 8_MHz; 
	movwf	OSCFRQ
	movlw	OSCSTAT_INI	; MFOR not ready; do not understand, it is read only
	movwf	OSCSTAT
	movlw	OSCTUNE_INI	; HFTUN 32; do not understand why MCC set this?
	movwf	OSCTUNE

; Comparator CM1 management
	BANKSEL	CM1CON0		; BANK 19
	movlw	CM1CON0_INI	; C1HYS disabled; C1EN enabled; C1POL not inverted; C1SYNC asynchronous;
	movwf	CM1CON0
	movlw	CM1CON1_INI	; C1INTN intFlag_neg; C1INTP intFlag_pos; interrupt on change, both edges
	movwf	CM1CON1
	movlw	CM1NCH_INI	; NCH CIN0- (RA0)
	movwf	CM1NCH
	movlw	CM1PCH_INI	; PCH C1IN0+ (RA1)
	movwf	CM1PCH
	;movlw	CM2CON0_INI	; C2HYS disabled; C2EN disabled; C2POL not inverted; C2SYNC asynchronous;
	;movwf	CM2CON0

; Timer2 management
	BANKSEL	TMR2		; BANK 5
	movlw	T2CLKCON_INI	; T2CS FOSC/4; 
	movwf	T2CLKCON
	movlw	T2HLT_INI	; T2PSYNC Not Synchronized; T2MODE Software control; T2CKPOL Rising Edge; T2CKSYNC Not Synchronized; 
	movwf	T2HLT
	movlw	T2RST_INI	; T2RSEL T2INPPS pin; 
	movwf	T2RST
	movlw	T2PR_INI	; T2PR 59 (0.3B); 
	movwf	T2PR
		;T2TMR = 0x00; TMR2 0; 
	movlw	T2CON_INI	; T2CKPS 1:2; T2OUTPS 1:1; TMR2ON on; 
	movwf	T2CON

; Timer1 management
	BANKSEL	TMR1L		; BANK 4
	movlw	T1GCON_INI	; T1GE disabled; T1GTM disabled; T1GPOL low; T1GGO done; T1GSPM disabled; 
	movwf	T1GCON
	movlw	T1GATE_INI	; GSS T1G_pin; 
	movwf	T1GATE
	movlw	T1CLK_INI	; ClockSelect FOSC/4; 
	movwf	T1CLK
		;TMR1H = 0x00;    //TMR1H 0; 
		;TMR1L = 0x00;    //TMR1L 0; 
	movlw	T1CON_INI	; CKPS 1:8; nT1SYNC do_not_synchronize; TMR1ON disabled; T1RD16 disabled; 
	movwf	T1CON

; Timer0 management
	BANKSEL	TMR0L		; BANK 11
	movlw	T0CON1_INI	; T0CS FOSC/4; T0CKPS 1:16; T0ASYNC not_synchronised; 
	movwf	T0CON1
	movlw	T0CON0_INI	; T0OUTPS 1:1; T0EN disaabled; T016BIT 16-bit; 
	movwf	T0CON0
	movlw	TMR0H_INI	; in 8 bit mode the TMR0H is compared same as timer2
	movwf	TMR0H
	;TMR0L = 0x00; TMR0L 0; 

; EUSART1 management
;	BANKSEL	BAUD1CON	; BANK 2
;	movlw	BAUD1CON_INI	; ABDOVF no_overflow; SCKP Non-Inverted; BRG16 16bit_generator; WUE disabled; ABDEN disabled; 
;	movwf	BAUD1CON
;	movlw	RC1STA_INI	; SPEN enabled; RX9 8-bit; CREN enabled; ADDEN disabled; SREN disabled; 
;	movwf	RC1STA
;	movlw	TX1STA_INI	; TX9 8-bit; TX9D 0; SENDB sync_break_complete; TXEN disabled; SYNC asynchronous; BRGH hi_speed; CSRC slave; 
;	movwf	TX1STA
;	movlw	SP1BRGL_INI	; SP1BRGL 29; 
;	movwf	SP1BRGL
;	movlw	SP1BRGH_INI	; SP1BRGH 0; 
;	movwf	SP1BRGH

;; EUSART2 management
;	BANKSEL	BAUD2CON	; BANK 20
;	movlw	BAUD2CON_INI	; ABDOVF no_overflow; SCKP Non-Inverted; BRG16 16bit_generator; WUE disabled; ABDEN disabled; 
;	movwf	BAUD2CON
;	movlw	RC2STA_INI	; SPEN enabled; RX9 8-bit; CREN enabled; ADDEN disabled; SREN disabled; 
;	movwf	RC2STA
;	movlw	TX2STA_INI	; TX9 8-bit; TX9D 0; SENDB sync_break_complete; TXEN disabled; SYNC asynchronous; BRGH hi_speed; CSRC slave; 
;	movwf	TX2STA
;	movlw	SP2BRGL_INI	; SP1BRGL 29; 
;	movwf	SP2BRGL
;	movlw	SP2BRGH_INI	; SP1BRGH 0; 
;	movwf	SP2BRGH

; CCP + PWM management
	BANKSEL	CCP1CON			; BANK 6
	movlw	CCP1CON_INI		; CCPx is enabled, Capture mode: every edge of CCPx input
	movwf	CCP1CON
	movlw	CCP1CAP_INI		; source is CCP1PPS
	movwf	CCP1CAP

	movlw	CCP2CON_INI		; CCPx is enabled, Capture mode: every edge of CCPx input
	movwf	CCP2CON
	movlw	CCP2CAP_INI		; source is CCP2PPS
	movwf	CCP2CAP
	movlw	PWMxCON_INI		; PWM is enabled, PWM active low
	clrf	PWM3DCL
	clrf	PWM3DCH
	movwf	PWM3CON
	clrf	PWM4DCL
	clrf	PWM4DCH
	movwf	PWM4CON
	clrf	PWM5DCL
	clrf	PWM5DCH
	movwf	PWM5CON

	BANKSEL	PWM6CON			; BANK 7
	clrf	PWM6DCL
	clrf	PWM6DCH
	movwf	PWM6CON


	BANKSEL	CCP1PPS			; BANK 61
	movlw	CCP1PPS_INI		; RA2->CCP1;
	movwf	CCP1PPS
	movlw	CCPP2PS_INI		; RA3->CCP2;
	movwf	CCP2PPS

; Interrupts
	BANKSEL	PIE0	; BANK 2
	movlw	PIE0_INI	; Enable TMR0 interrupt set TMR0IE = 1.
	movwf	PIE0
	movlw	PIE1_INI	; none used
	movwf	PIE1
	movlw	PIE2_INI	; Enabling CMP1 interrupt.
	movwf	PIE2
	movlw	PIE3_INI	; enable receive interrupt
	movwf	PIE3
	movlw	PIE4_INI	; Enabling TMR2 interrupt.
	movwf	PIE4
	movlw	PIE5_INI	; none used
	movwf	PIE5
	movlw	PIE6_INI	; none used
	movwf	PIE6
	movlw	PIE7_INI	; none used
	movwf	PIE7


	movlb	0	; BANK 0
	movlw	RAMINI0			; Clear variables bank0
	movwf	FSR0L
	clrf	FSR0H
ClearRAM:
	clrf	INDF0
	incf	FSR0L,f
	btfss	FSR0L,7			; to address 7F
	goto	ClearRAM

	BANKSEL	PIR0
	clrf	PIR6
	movlw	INTC_INI			; GIE enable, PEIE enable
	movwf	INTCON

	;BANKSEL	LATA			; bank 6
	;movlw	0x11
	;movwf	LATA			; PWM 3 = max (always 0)
	;BANKSEL	PWM6DCH			; bank 7
	;movlw	.50
	;movwf	PWM6DCH			; PWM 4 = pulses


; --- Main Loop

MainLoop:

	;goto	MainLoop
	movlb	0	; BANK 0
	btfsc	NEW_S1	; new servo 1 position received?
	call	HandleMsg1	; yes, handle it

	movlb	0	; BANK 0
	btfsc	NEW_S2	; new servo 2 position received?
	call	HandleMsg2	; yes, handle it

	goto	MainLoop

HandleMsg1:
; min servo position 900us ~ 13
; max servo position 3000us ~ 67
	bcf		NEW_S1
	movlw	.13
	subwf	SERVO1P,f		; SERVO1P = SERVO1P - 24
	btfss	STATUS,C
	return					; SERVO1P is less than 24 = too small
	movlw	.54 ; 67-13		; maximum is 81
	subwf	SERVO1P,w		; w = SERVO1P - 24
	btfsc	STATUS,C
	return					; SERVO1P is greather than 67 = too big
	movlw	HIGH(SpeedTable)
	movwf	FSR0H
	movlw	LOW(SpeedTable)
	addwf	SERVO1P,w
	movwf	FSR0L
	BANKSEL	PWM3DCH			; bank 6
	btfsc	INDF0,7			; bit 7 = reverse
	goto	HM1Reverse

	movlw	0xFF
	movwf	PWM3DCH			; PWM 3 = max (always 0)
	movf	INDF0,w
	sublw	.52			; reverse logic
	movwf	PWM4DCH			; PWM 4 = pulses
	return
HM1Reverse:
	movf	INDF0,w
	andlw	0x7F
	sublw	.52			; reverse logic
	movwf	PWM3DCH			; PWM 3 = pulses
	movlw	0xFF
	movwf	PWM4DCH			; PWM 4 = max (always 0)
	return

HandleMsg2:
; min servo position 900us ~ 13
; max servo position 3000us ~ 67
	bcf		NEW_S2
	movlw	.13
	subwf	SERVO2P,f		; SERVO2P = SERVO2P - 24
	btfss	STATUS,C
	return					; SERVO2P is less than 24 = too small
	movlw	.54 ; 67-13		; maximum is 81
	subwf	SERVO2P,w		; w = SERVO2P - 24
	btfsc	STATUS,C
	return					; SERVO2P is greather than 67 = too big
	movlw	HIGH(SpeedTable)
	movwf	FSR0H
	movlw	LOW(SpeedTable)
	addwf	SERVO2P,w
	movwf	FSR0L
	BANKSEL	PWM3DCH			; bank 6
	btfsc	INDF0,7			; bit 7 = reverse
	goto	HM2Reverse
	movlw	0xFF
	movwf	PWM5DCH			; PWM 5 = max (always 0)
	BANKSEL	PWM6DCH			; bank 7
	movf	INDF0,w
	sublw	.52			; reverse logic
	movwf	PWM6DCH			; PWM 6 = pulses
	return
HM2Reverse:
	movf	INDF0,w
	andlw	0x7F
	sublw	.52			; reverse logic
	movwf	PWM5DCH			; PWM 5 = pulses
	BANKSEL	PWM6DCH			; bank 7
	movlw	0xFF
	movwf	PWM6DCH			; PWM 6 = max (always 0)
	return


	org	SAF_INI
SpeedTable:
; 37 us per step --> Timer0 is used as source
; min servo position 500us ~ 13
; max servo position 2500us ~ 67
; usual start 1000us ~ 27
; usual middle 1500 ~ 40
; usual end 2000 ~ 54
	dt	.52			; 13
	dt	.52			; 14
	dt	.52			; 15
	dt	.52			; 16
	dt	.52			; 17
	dt	.52			; 18
	dt	.52			; 19
	dt	.52			; 20
	dt	.52			; 21
	dt	.52			; 22
	dt	.52			; 23
	dt	.52			; 24
	dt	.52			; 25
	dt	.45			; 26
	dt	.45			; 27
	dt	.39			; 28
	dt	.39			; 29
	dt	.33			; 30
	dt	.33			; 31
	dt	.26			; 32
	dt	.26			; 33
	dt	.20			; 34
	dt	.20			; 35
	dt	.14			; 36
	dt	.14			; 37
	dt	.00			; 38
	dt	.00			; 39
	dt	.00			; 40
	dt	.00			; 41
	dt	.00			; 42
	dt	.14+.128	; 43
	dt	.14+.128	; 44
	dt	.20+.128	; 45
	dt	.20+.128	; 46
	dt	.26+.128	; 47
	dt	.26+.128	; 48
	dt	.33+.128	; 49
	dt	.33+.128	; 50
	dt	.39+.128	; 51
	dt	.39+.128	; 52
	dt	.45+.128	; 53
	dt	.45+.128	; 54
	dt	.52+.128	; 55
	dt	.52+.128	; 56
	dt	.52+.128	; 57
	dt	.52+.128	; 58
	dt	.52+.128	; 59
	dt	.52+.128	; 60
	dt	.52+.128	; 61
	dt	.52+.128	; 62
	dt	.52+.128	; 63
	dt	.52+.128	; 64
	dt	.52+.128	; 65
	dt	.52+.128	; 66
	dt	.52+.128	; 67
	dt	.52+.128	; 68

	end