#include 	P16F73.inc
	__config _HS_OSC  & _WDT_ON

; Clock = 20 MHz
; IR steering sensor on AN2
; IR Tilt sensor on AN1
; Ultrasonic Tilt Output on RB1
; Ultrasonic Tilt Input on RB2
; !Gate for H-bridge module on RB3
; H Bridge outputs on CCP1 and CCP2
; Blue LED on RC7, Pull Down
; Orange LED on RC6, Pull Down
; Kp: measured on AN3
; Kd: measured on AN4

;*******VARIABLES*********
	cblock	0x20
	DUTY, PDUTY, DDUTY	; Un-turn-compensated duty cycle
	TEMP, MultTemp, TableTemp	; For use in calculations and delays
	LimitTemp
	IR_AveH, IR_AveL	; High and low bytes for the IR average
	IR, H_Temp	; IR tilt converted to THETA units, temp high byte for 16 bit add
	Flags		; Status of measurement bit 0, bad echo, bit 1 bad IR tilt
	THETA, PTHETA	; The angle value to use in this calculation cycle
	DTERM, PTERM	; Differential and Proportional contributions to gain
	DTHETA, PDTHETA	; THETA - PTHETA
	Add1, Add2, AddAnswer	; Used for the clamp-add function
	AARGB0		; USED IN THE MULTIPLY ROUTINE 8 BIT MULTIPLICAND AND HIGH BYTE OF 16 BIT RESULT
	AARGB1		; LOWER BYTE OF THE 16 BIT RESULT FOR THE MULTIPY ROUTINE
	BARGB0		; 8 BIT MULTIPLIER FOR THE MULTIPLY ROUTINE
	SIGN		; ONLY USED IN THE MULTIPLY ROUTINE
	TEMPB3		; ONLY USED IN THE MULTIPLY ROUTINE
	LOOPCOUNT		; ONLY USED IN THE MULTIPLY ROUTINE
	ULimitH, ULimitL	; The hight and low values for the limit function
	SLimitH, SLimitL	; The hight and low values for the limit function
	ToLimit		; The value for the limit functions to limit
	Kp, Kd		; The control gains, stored as 2s comp, *16 of expected value
	Int_H, Int_L	; The high and low bytes of the accumulated error (scaled by 1/4)
	I_Temp			; A temp variable for integral stuff
	endc

;*******CONSTANTS*********
LEFTDUTY	equ	CCPR1L
RIGHTDUTY	equ	CCPR2L
	
#define	ECH_OUT	PORTB,1	; Trigger Output for Ultrasonic
#define	ECH_IN	PORTB,2	; Pulse Width Input for Ultrasonic
#define	NOR_OUT	PORTB,3	; Gate control of complementary PWM
#define	MOTORS_ON	PORTB,4	; When Low, Motor Drivers all OFF. 
#define	BLUE_LED	PORTC,4	; Active Low Blue LED
#define	RED_LED	PORTC,5	; Active Low Orange LED
#define	EchBadFlag	Flags,0	
#define	IRBadFlag	Flags,1
#define	NegDiffFlag	Flags,2
#define	DutyMAX	d'250'	; Currently limit PWM duty cycle output
#define	DutyMIN	d'5'	; while sorting out stability.
#define	MaxDutyDiff d'7'	; The max the duty can change in a cycle

	org	0

	;-----The IR lookup table-------

	org	0

Start	goto	Init

	org	4

Interupt	retfie	

Init	;------SET IO PORTS------
	bsf	RED_LED	; LED off
	bsf	BLUE_LED	; LED off
	bcf	MOTORS_ON	; Motor drivers off
	bsf	STATUS,RP0	; Bank1
	clrf	TRISC	; All outputs
	bcf	ECH_OUT	; Trigger for Ultrasonic
	bcf	NOR_OUT	; Complimentary H drive
	bcf	MOTORS_ON	; Shutdown Control for IR2104s
	bcf	STATUS,RP0	; Bank0
	bsf	RED_LED	; LED off
	bsf	BLUE_LED	; LED off
	bcf	MOTORS_ON	; Motor drivers off
	bcf	NOR_OUT	; Enable complimentary H bridge drive
	;------SET PWM MODULES------
	movlw	b'00000100'	; Timer2 ON, no prescale
	movwf	T2CON
	movlw	b'00001100'	; PWM mode for CCPs
	movwf	CCP1CON
	movwf	CCP2CON
	bsf	STATUS,RP0	; Bank1
	movlw	0xFF	
	movwf	PR2	; PWM Period of 255 
	bcf	STATUS,RP0	; Bank0
	;------SET ANOLOG CONVERTER------	
	bsf	STATUS,RP0	; Bank1
	movlw	b'010'	
	movwf	ADCON1	; All AN0-AN4 configured as analog pins
	bcf	STATUS,RP0	; Bank0
	movlw	b'10010001'
	movwf	ADCON0	; Setup for 32 Tosc, AN2 as input
	;------SET TMR1 FOR ECHO------
	movlw	b'00000001'	
	movwf	T1CON	; TMR1 ON at Tosc, no prescale
	;------SET TMR0 FOR Timebase------	
	bsf	STATUS,RP0	; Bank1
	movlw	b'11010111'	; TMR0, 255 prescale
	movwf	OPTION_REG	; used for ~10ms time base
	bcf	STATUS,RP0	; Bank0

	;------READ CONTROL CONSTANTS------
	movlw	b'10011001'	
	movwf	ADCON0	; Setup for 32 Tosc, AN3 as input
	movlw	0x30	
	movwf	TEMP	; Wait for 48 skips to let sampling capacitor
ADCharge_Kp	decfsz	TEMP,f	; fully charge
	goto	ADCharge_Kp

	bsf	ADCON0,GO	; Start ADC Conversion for Kp
ADLoop_Kp	btfsc	ADCON0,GO
	goto	ADLoop_Kp	; Loop until conversion done

	movf	ADRES,w
	call	ConstTable
	clrf	PCLATH	; Reset PCLATH for normal operation
	movwf	Kp	; Store Kp

	movlw	b'10100001'	
	movwf	ADCON0	; Setup for 32 Tosc, AN4 as input
	movlw	0x30	
	movwf	TEMP	; Wait for 48 skips to let sampling capacitor
ADCharge_Kd	decfsz	TEMP,f	; fully charge
	goto	ADCharge_Kd

	bsf	ADCON0,GO	; Start ADC Conversion for Kd
ADLoop_Kd	btfsc	ADCON0,GO
	goto	ADLoop_Kd	; Loop until conversion done

	movf	ADRES,w
	call	ConstTable
	clrf	PCLATH	; Reset PCLATH for normal operation
	movwf	Kd	; Store Kd


;************************************************************
; Main  Program Loop that cycles every TMR0 overflow, 
; Measures the echo length, updates PWMs accordingly
;************************************************************

Main	movlw	d'175'
	movwf	TMR0
	bcf	INTCON,2	; Clear TMR0 interupt Flag
	call	Measure_IR	; AD Conversion on the IR Sensor
	call	Calc_THETA	; Based on sensor results, calculate board angle THETA
	call	Calc_DTERM	; Computes the derivative term for motor out
	call	Calc_PTERM	; Computes proportional term for motor output
	call 	Calc_ITERM	; Calculate the integral term
	call	Calc_DUTY	; Computes final motor output
	call	Measure_Lean ; AD Conversion of Lean
	call	Update_Motors ; Drive motor outputs based on Lean

MLoop	clrwdt		; Main Loop Delay
	btfss	INTCON,2
	goto	MLoop	
	goto	Main	


;************************************************************
; Measure_IR  Samples the IR distance sensor measuring the board
; lean angle to corroborate the ultrasonic data. Returns with
; the ADC result left in ADRES
;************************************************************


Measure_IR	movlw	b'10001001'
	movwf	ADCON0	; Setup for 32 Tosc, AN1 as input
	movlw	0x30	
	movwf	TEMP	; Wait for 48 skips to let sampling capacitor
ADCharge_IR	decfsz	TEMP,f	; fully charge
	goto	ADCharge_IR
	clrf	IR_AveH	; Clear the accumulated IR average
	clrf	IR_AveL
	movlw	0x40
	movwf	TEMP	; Prepare for 64 additions

ADNew_Con
	bsf	ADCON0,GO	; Start ADC Conversion
ADLoop_Tilt	btfsc	ADCON0,GO
	goto	ADLoop_Tilt	; Loop until conversion done

	movf	ADRES,w	; IR Range sensor result in ADC
	call	IR_table	; Convert to an angle range with table
	clrf	PCLATH	; Restore PCLATH to current program location

	movwf	IR	; Store it in the IR register

	bcf	RED_LED
	btfsc	IR,7
	bsf	RED_LED

	clrf	H_Temp	
	btfsc	IR,7
	comf	H_Temp,f ; Make the high byte 0xff if negative

	movfw	IR
	addwf	IR_AveL,f	; Add the two bytes in order
	btfsc	STATUS,C
	incf	H_Temp,f
	movfw	H_Temp
	addwf	IR_AveH,f
	decfsz	TEMP,f
	goto	ADNew_Con
	
	rlf	IR_AveL,f	; Roll twice to give a proper averaging
	rlf	IR_AveH,f	; of unit magnitude
	rlf	IR_AveL,f
	rlf	IR_AveH,f
	
	movfw	IR_AveH
	movwf	IR

	return


;************************************************************
; Converts the current value in the ADRES register to a signed
; byte corresponding to the board angle, hopefully matching
; approximately the value obtained by the echo sensor. 
; Uses a look-up table that matches Sensor Voltages to equivalent
; thetas. Result IR is saturated between -128 and 127
;************************************************************


Calc_THETA
	movf	THETA,w
	movwf	PTHETA	; Save the old board angle

	movf	IR,w	
	movwf	THETA	
	return


;************************************************************
; Calc_DTERM   Code to generate the derivative term for the 
; motor feedback. DTERM is filtered to reduce effects of sensor
; noise.  Assumes THETA holds current angle, PTHETA holds 
; previous angle.
; Uses TmpH and L variables to assist with digital filtering
; DTHETA = 1/4 DTHETA + 3/4 PDTHETA
; Then, it uses derivative gain for DTERM = Kd*DTHETA
;************************************************************

Calc_DTERM	
	movf	DTHETA,w
	movwf	PDTHETA	; Save old DTHETA term for filtering
	movf	PTHETA,w
	xorlw	0xff
	addlw	0x01	; Negate, PTHETA
	movwf	TEMP
	addlw	0x80
	btfss	STATUS,Z
	goto	PTHETA_OK
	movlw	0x7f	; If we get here, we have -128, which does not negate properly
	movwf	TEMP

PTHETA_OK	movf	TEMP,w
	movwf	Add1
	movf	THETA,w
	movwf	Add2
	call	Clamp_Add
	movf	AddAnswer,w
	movwf	DTHETA	; DTHETA = (THETA - PTHETA)

	movwf	AARGB0	
	movlw	d'2'	; Equivalent to 1/8
	movwf	BARGB0
	call	SignedMult
	call	Mult16
	movfw	AARGB0
	movwf	MultTemp
	movlw	d'14'	; Equivalent to 7/8
	movwf	AARGB0
	movfw	PDTHETA
	movwf	BARGB0
	call 	SignedMult
	call	Mult16
	movfw	AARGB0
	movwf	Add1
	movfw	MultTemp
	movwf	Add2
	call	Clamp_Add
	movfw	AddAnswer	
	movwf	PDTHETA	; To be used for next cycle as PDTHETA

	movwf	AARGB0
	movfw	Kd
	movwf	BARGB0	
	call	SignedMult
	call	Mult16	; This is needed to make a single byte value useful, kd is divided by 16
	movf	AARGB0,w
	movwf	DTERM	
	return	

	

;************************************************************
; Calc_PTERM   Code to generate the proportional term for the 
; motor feedback. 
;************************************************************

Calc_PTERM	
	movf	THETA,w 
	movwf	AARGB0
	movfw	Kp
	movwf	BARGB0	
	call	SignedMult
	call	Mult16	
	movf	AARGB0,w
	movwf	PTERM
	return

;************************************************************
; Calc_ITERM calculates the integral term, and accumulates the error
; The Integral term in stored is Int_H, which is the high byte
; of the acculuted error/4. The accumulated error is limited at 
; +/-80*255
;******************************************************************

Calc_ITERM
	movfw	THETA
	movwf	I_Temp
	clrf	H_Temp
	btfss	THETA,7
	goto	PosITheta	

NegITheta
	comf	H_Temp,f	;Mark the high byte as all ones if THETA negative
	bsf		STATUS,C	;Roll the file (divide by 4)
	rrf		I_Temp,f
	bsf		STATUS,C
	rrf		I_Temp,f
	goto	ITermCont	
PosITheta
	bcf		STATUS,C	;Divide by 4
	rrf		I_Temp,f
	bcf		STATUS,C
	rrf		I_Temp,f

ITermCont
	movfw	I_Temp
	addwf	Int_L,f
	btfsc	STATUS,C
	incf	H_Temp,f
	movfw	H_Temp
	addwf	Int_H,f	;Finish adding the accumulated error

	btfsc	Int_H,7	;Check if the accumulation is negative	
	goto	Neg_Int
Pos_Int	
	movfw	Int_H	;Test that the term is not too large
	addlw	d'256'-d'80'
	btfss	STATUS,C
	goto	Int_OK
	movlw	d'80'	;Limit the term if it is too large
	movwf	Int_H
	clrf	Int_L
	goto	Int_OK

Neg_Int
	movfw	Int_H
	addlw	d'80'
	btfsc	STATUS,C
	goto	Int_OK
	movlw	d'256'-d'80'
	movwf	Int_H
	clrf	Int_L
	comf	Int_L,f
	;goto	Int_OK

Int_OK
	return
	
;************************************************************
; Calc_DUTY   Code to generate motor duty cycle with proportional
; and derivative feedback.
; DUTY = 128 + PTERM + DTERM 
;************************************************************

Calc_DUTY
	movf	DUTY,w
	movwf	PDUTY	; Save old duty
	movf	PTERM,w
	movwf	Add1
	movf	DTERM,w
	movwf	Add2
	call	Clamp_Add	; AddAnswer now has PTERM + DTERM
	movf	AddAnswer,w	; AddAnswer also in range of -128 to +127

	movwf	Add1
	movfw	Int_H
	movwf	Add2
	call	Clamp_Add	; Add the integral term
	movfw	AddAnswer

	addlw	d'128'
	movwf	DUTY

	movf	PDUTY,w
	subwf	DUTY,w
	movwf	DDUTY	; DDUTY = DUTY - PDUTY
	skpnc
	goto	PosDDUTY
	goto	NegDDUTY

PosDDUTY	movlw	d'127'	; Max Positive signed byte
	btfsc	DDUTY,7
	movwf	DDUTY	
	movfw	DDUTY
	addlw	d'256' - MaxDutyDiff
	skpc	
	goto	DoneDuty	
	movlw	MaxDutyDiff
	movwf	DDUTY
	goto	DoneDuty

NegDDUTY	movlw	d'128'	; Max Negative signed byte
	btfss	DDUTY,7
	movwf	DDUTY
	movfw	DDUTY
	addlw	MaxDutyDiff	;
	skpnc	
	goto 	DoneDuty	
	movlw	d'256' - MaxDutyDiff
	movwf	DDUTY
	goto	DoneDuty			

DoneDuty	movfw	DDUTY
	addwf	PDUTY,w
	movwf	DUTY
	return

;************************************************************
; Meausure_Lean    Performs and Analog Digital Conversion
; to get the board lean angle for steering
;************************************************************
Measure_Lean
	movlw	b'10010001'
	movwf	ADCON0	; Setup for 32 Tosc, AN2 as input

	movlw	0x30	
	movwf	TEMP	; Wait for 48 skips to let sampling capacitor
ADCharge_Lean decfsz	TEMP,f	; fully charge
	goto	ADCharge_Lean

	bsf	ADCON0,GO	; Start ADC Conversion
ADLoop	btfsc	ADCON0,GO
	goto	ADLoop	; Loop until conversion done
	movf	ADRES,w	; Check if Sensor Voltage in
	addlw 	d'255' - d'150'	; Expected Range (90-150)
	addlw 	d'60' + d'1'
	btfss	STATUS,C
	goto	BadLean	; If Not, don't apply steer correction
	
	movlw	d'120'	; Conversion when board is level
	subwf	ADRES,f	; ADRES = Tilt Signal (~ -11 to 11)
	btfss	STATUS,C
	goto	NegLean
	goto	PosLean

NegLean	movf	ADRES,w
	addwf	DUTY,w
	btfss	STATUS,C
	movlw	0x00
	movwf	LEFTDUTY
	movf	ADRES,w
	subwf	DUTY,w
	btfsc	STATUS,C
	movlw	0xFF
	movwf	RIGHTDUTY
	bsf	BLUE_LED
	return

	
PosLean	movf	ADRES,w
	addwf	DUTY,w	; 
	btfsc 	STATUS,C
	movlw	0xFF
	movwf	LEFTDUTY
	movf	ADRES,w
	subwf	DUTY,w	; Differential Steering Control	
	btfss	STATUS,C
	movlw	0x00
	movwf	RIGHTDUTY	
	bcf	BLUE_LED
	return

BadLean	
	bsf	BLUE_LED
	movfw	DUTY
	movwf	RIGHTDUTY
	movwf	LEFTDUTY
	return

;************************************************************
; Update_Motors   Outputs the desired PWM to each motor to 
; account for both balancing and steering
;************************************************************
Update_Motors
	movlw	DutyMAX
	movwf	ULimitH

	movlw	DutyMIN
	movwf	ULimitL

	movfw	RIGHTDUTY
	movwf	ToLimit

	call	ULimit

	movfw	ToLimit
	movwf	RIGHTDUTY
	
	movfw	LEFTDUTY
	movwf	ToLimit
	
	call	ULimit

	movfw	ToLimit
	movwf	LEFTDUTY

 	bsf	MOTORS_ON	; Make sure motor drivers enabled

	return

;************************************************************
; Clamp_Add  Sums the two signed integers Add1 and Add2 with
; results stored in AddAnswer. If there is an overflow and the
;  sum exceeds -128 or 127, then it is saturated at those values.  
;************************************************************

Clamp_Add		
	movf	Add1,w
	addwf	Add2,w
	movwf	AddAnswer
		
	btfss	Add1,7
	goto	FirstPos

FirstNeg	btfss	Add2,7
	goto	AddDone
BothNeg	btfsc	AddAnswer,7
	goto	AddDone
	movlw	0x80
	goto	AddDone

FirstPos	btfsc	Add2,7
	goto	AddDone
BothPos	btfss	AddAnswer,7
	goto	AddDone
	movlw	0x7f
	
AddDone	movwf	AddAnswer
	return

;************************************************************
; 8x8 bit signed fixed point multiply 8x8 -> 16
; Input: 8 bit signed fixed point multiplicand in AARGB0
;        8 bit signed fixed point multiplier in BARGB0
; Result: AARG
; Routine is copied from sample assembly code in Microchip
; Application Note AN964
;************************************************************

SignedMult	
	CLRF	AARGB1	;CLEAR PARTIAL PRODUCT
	CLRF	SIGN
	MOVF	AARGB0,W
	BTFSC	STATUS, Z
	RETLW	00H
	
	XORWF	BARGB0,W
	MOVWF	TEMPB3
	BTFSC	TEMPB3,7
	COMF	SIGN, F

	BTFSS	BARGB0, 7
	GOTO	M0808SOK
	COMF	BARGB0,F	;MAKE MULTIPLIER BARG > 0
	INCF	BARGB0,F
	COMF	AARGB0,F
	INCF	AARGB0,F

	BTFSC	BARGB0, 7
	GOTO	M0808SX
M0808SOK

SMUL0808L
	MOVLW	07H
	MOVWF	LOOPCOUNT
	MOVF	AARGB0, W
LOOPSM0808A
	RRF	BARGB0,	F
	BTFSC	STATUS,C	
	GOTO	LSM0808NA
	DECFSZ	LOOPCOUNT,F
	GOTO	LOOPSM0808A

	CLRF	AARGB0
	RETLW	00H
LOOPSM0808
	RRF	BARGB0,F
	BTFSC	STATUS,C
	ADDWF	AARGB0,F
LSM0808NA
	RLF	SIGN,F	
	RRF	AARGB0,F
	RRF	AARGB1,F
	DECFSZ	LOOPCOUNT,F
	GOTO	LOOPSM0808
	RLF	SIGN,F
	RRF	AARGB0,F
	RRF	AARGB1,F
	RETLW	00H
M0808SX
	CLRF	AARGB1
	RLF	SIGN, W
	RRF	AARGB0,F
	RRF	AARGB1,F
	RETLW	00H

;************************************
; This function multiplies the value in AARG by 16, as this 
; makes the high byte more valid as a single byte.
;*************************************

Mult16
	movlw	0x04
	movwf	TEMP

	btfsc	AARGB0,7
	goto	MultNeg
MultPos
	bcf	STATUS,C
	rlf	AARGB1,f
	rlf	AARGB0,f
	btfsc	AARGB0,7
	goto	MPosOverf
	decfsz	TEMP,f
	goto	MultPos

	goto	MultDone

MPosOverf
	movlw	d'127'
	movwf	AARGB0
	movlw	d'255'
	movwf	AARGB1
	goto	MultDone


MultNeg
	bsf	STATUS,C
	rlf	AARGB1,f
	rlf	AARGB0,f
	btfss	AARGB0,7
	goto	MNegOverf
	decfsz	TEMP,f
	goto	MultNeg

	goto	MultDone

MNegOverf
	movlw	d'128'
	movwf	AARGB0
	movlw	d'0'
	movwf	AARGB1
	goto	MultDone
MultDone
	return

;******************************************************
; This function checks the value of an unsigned byte against an
; upper and lower limit and sets the value to the limit if it 
; exceeds the limit.
; Upper Limit in ULimitH, Lower Limit in ULimitL
;*******************************************************
ULimit	
	movfw	ToLimit	; Test for zero, as the other tests miss this case
	btfsc	STATUS,Z
	goto	ULimitZ

	movfw	ULimitH	; Test if the value is too high
	sublw	0x00
	addwf	ToLimit,w
	btfss	STATUS,C
	goto 	UTestLow
	movf	ULimitH,w	; The value is too high, limit it
	movwf	ToLimit
	goto	ULimitDone
	
UTestLow	movf	ToLimit,w	; Test if the value is too low
	sublw	0x00
	addwf	ULimitL,w
	btfss	STATUS,C
	goto	ULimitDone
	movf	ULimitL,w	; Limit the value
	movwf	ToLimit
	goto	ULimitDone

ULimitZ	movfw	ULimitL
	movwf	ToLimit

ULimitDone 	return


	org 	d'507'


;************************************************************
; Lookup Table to conver the IR sensor readings into a board
; angle. Assumes that the IR voltage is in the working register 
;************************************************************

IR_table	movwf	TableTemp
	movlw	0x02
	movwf	PCLATH
	movf	TableTemp,w
	addwf	PCL,F
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127' 
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'127'
 retlw d'123'
 retlw d'113'
 retlw d'103'
 retlw d'94'
 retlw d'86'
 retlw d'77'
 retlw d'70'
 retlw d'62'
 retlw d'55'
 retlw d'48'
 retlw d'42'
 retlw d'35'
 retlw d'29'
 retlw d'24'
 retlw d'18'
 retlw d'13'
 retlw d'8'
 retlw d'3'
 retlw d'254'
 retlw d'249'
 retlw d'245'
 retlw d'241'
 retlw d'237'
 retlw d'233'
 retlw d'229'
 retlw d'225'
 retlw d'222'
 retlw d'218'
 retlw d'215'
 retlw d'212'
 retlw d'209'
 retlw d'205'
 retlw d'202'
 retlw d'200'
 retlw d'197'
 retlw d'194'
 retlw d'191'
 retlw d'189'
 retlw d'186'
 retlw d'184'
 retlw d'182'
 retlw d'179'
 retlw d'177'
 retlw d'175'
 retlw d'173'
 retlw d'171'
 retlw d'169'
 retlw d'167'
 retlw d'165'
 retlw d'163'
 retlw d'161'
 retlw d'159'
 retlw d'157'
 retlw d'156'
 retlw d'154'
 retlw d'152'
 retlw d'151'
 retlw d'149'
 retlw d'147'
 retlw d'146'
 retlw d'144'
 retlw d'143'
 retlw d'142'
 retlw d'140'
 retlw d'139'
 retlw d'138'
 retlw d'136'
 retlw d'135'
 retlw d'134'
 retlw d'132'
 retlw d'131'
 retlw d'130'
 retlw d'129'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128' 
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128'
 retlw d'128' 
 retlw d'128'


;*********************************************************
; This table is used to determine the control constants from an analog input.
; The function is an exponential, ranging from 1-255, with 16 in the centre
;*********************************************************


	org	d'1019'

ConstTable
	movwf	TableTemp
	movlw	0x04
	movwf	PCLATH
	movf	TableTemp,w
	addwf	PCL,F

 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'1'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'2'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'3'
 retlw d'4'
 retlw d'4'
 retlw d'4'
 retlw d'4'
 retlw d'4'
 retlw d'4'
 retlw d'4'
 retlw d'4'
 retlw d'4'
 retlw d'4'
 retlw d'4'
 retlw d'4'
 retlw d'5'
 retlw d'5'
 retlw d'5'
 retlw d'5'
 retlw d'5'
 retlw d'5'
 retlw d'5'
 retlw d'5'
 retlw d'5'
 retlw d'6'
 retlw d'6'
 retlw d'6'
 retlw d'6'
 retlw d'6'
 retlw d'6'
 retlw d'6'
 retlw d'6'
 retlw d'7'
 retlw d'7'
 retlw d'7'
 retlw d'7'
 retlw d'7'
 retlw d'7'
 retlw d'7'
 retlw d'8'
 retlw d'8'
 retlw d'8'
 retlw d'8'
 retlw d'8'
 retlw d'9'
 retlw d'9'
 retlw d'9'
 retlw d'9'
 retlw d'9'
 retlw d'10'
 retlw d'10'
 retlw d'10'
 retlw d'10'
 retlw d'10'
 retlw d'11'
 retlw d'11'
 retlw d'11'
 retlw d'11'
 retlw d'12'
 retlw d'12'
 retlw d'12'
 retlw d'12'
 retlw d'13'
 retlw d'13'
 retlw d'13'
 retlw d'13'
 retlw d'14'
 retlw d'14'
 retlw d'14'
 retlw d'15'
 retlw d'15'
 retlw d'15'
 retlw d'16'
 retlw d'16'
 retlw d'16'
 retlw d'17'
 retlw d'17'
 retlw d'17'
 retlw d'18'
 retlw d'18'
 retlw d'19'
 retlw d'19'
 retlw d'19'
 retlw d'20'
 retlw d'20'
 retlw d'21'
 retlw d'21'
 retlw d'22'
 retlw d'22'
 retlw d'23'
 retlw d'23'
 retlw d'24'
 retlw d'24'
 retlw d'25'
 retlw d'25'
 retlw d'26'
 retlw d'26'
 retlw d'27'
 retlw d'27'
 retlw d'28'
 retlw d'29'
 retlw d'29'
 retlw d'30'
 retlw d'31'
 retlw d'31'
 retlw d'32'
 retlw d'33'
 retlw d'33'
 retlw d'34'
 retlw d'35'
 retlw d'36'
 retlw d'36'
 retlw d'37'
 retlw d'38'
 retlw d'39'
 retlw d'40'
 retlw d'41'
 retlw d'41'
 retlw d'42'
 retlw d'43'
 retlw d'44'
 retlw d'45'
 retlw d'46'
 retlw d'47'
 retlw d'48'
 retlw d'49'
 retlw d'50'
 retlw d'52'
 retlw d'53'
 retlw d'54'
 retlw d'55'
 retlw d'56'
 retlw d'57'
 retlw d'59'
 retlw d'60'
 retlw d'61'
 retlw d'63'
 retlw d'64'
 retlw d'65'
 retlw d'67'
 retlw d'68'
 retlw d'70'
 retlw d'71'
 retlw d'73'
 retlw d'74'
 retlw d'76'
 retlw d'78'
 retlw d'79'
 retlw d'81'
 retlw d'83'
 retlw d'85'
 retlw d'87'
 retlw d'89'
 retlw d'91'
 retlw d'92'
 retlw d'95'
 retlw d'97'
 retlw d'99'
 retlw d'101'
 retlw d'103'
 retlw d'105'
 retlw d'108'
 retlw d'110'
 retlw d'112'
 retlw d'115'
 retlw d'117'
 retlw d'120'
 retlw d'123'
 retlw d'125'
 retlw d'128'
 retlw d'131'
 retlw d'134'
 retlw d'137'
 retlw d'140'
 retlw d'143'
 retlw d'146'
 retlw d'149'
 retlw d'152'
 retlw d'156'
 retlw d'159'
 retlw d'162'
 retlw d'166'
 retlw d'170'
 retlw d'173'
 retlw d'177'
 retlw d'181'
 retlw d'185'
 retlw d'189'
 retlw d'193'
 retlw d'197'
 retlw d'202'
 retlw d'206'
 retlw d'211'
 retlw d'215'
 retlw d'220'
 retlw d'225'
 retlw d'230'
 retlw d'235'
 retlw d'240'
 retlw d'245'
 retlw d'251'
 retlw d'255'
	end