; H bridge 2 with cored magnet
; apply fixed pulse when sensor below min
; wait after stopping pulse before using sensor
; increase min if derivative over limit
; reverse pulse if above maximum

; enable RA6 by programming the configuration register in the programming harness
; programmer2 -c 0x300000 1100010100000000
; AN4 - hall effecct sensor
; RB3 - magnet pull
; RA6 - magnet push


#include "util2.inc"



MAGNET_UP macro
	bsf LATB, D'3'
	ENDM

MAGNET_DN macro
	bsf LATA, D'6'
	ENDM

MAGNET_OFF macro
	bcf LATB, D'3'
	bcf LATA, D'6'
	ENDM






;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; constants ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

; state
INACTIVE = H'00'
ACTIVE = H'01'
SENSOR_DELAY = H'02'

; 0x10000 - time between switching off and first accurate sensor reading
; also used as 0x10000 - minimum time between pulses
INACTIVE_DELAY = H'e000'
; 0x10000 - pulse duration for up direction
UP_DELAY = H'b000'
; 0x10000 - pulse duration for down direction
DOWN_DELAY = H'f000'



DAMP_DERIVATIVE = H'10'


MIN_FLUX = H'20'
DAMP_FLUX = H'22'
MAX_FLUX = H'38'







;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; registers ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

STATE = H'00'
INTERRUPT_FINISHED = H'01'
CURRENT_ANALOG = H'02'
PREV_ANALOG = H'03'
CURRENT_MIN_FLUX = H'04'





	ORG PROGRAM_START
	goto start

	ORG PROGRAM_INTERRUPTHI
	goto interrupt

	ORG PROGRAM_INTERRUPTLO
	goto interrupt




start:


; defaults
	SET_REGISTER STATE, ACTIVE
	setf PREV_ANALOG




; magnet off
	SET_REGISTER LATB, B'00000000'
	SET_REGISTER LATA, B'00000000'
; 0 - output 1 - input
	SET_REGISTER TRISA, B'10111111'
	SET_REGISTER TRISB, B'11110111'
; 7 - external Vref+
; 6 - external Vref-
; 4-2 - analog channel
; 1 - go/done
; 0 - enable converter
	SET_REGISTER ADCON0, B'11010001'
; 1 - digital 0 - analog
	SET_REGISTER ADCON1, B'11101111'
; 7 - right justified
; 5-3 - acquisition time
; 2-0 - conversion clock
	SET_REGISTER ADCON2, B'00000100'





; pulse timer
; 7 - on/off
; 6 - 16 bit if 0
; 5 - internal clock if 0
; 4 - edge
; 3 - skip prescaler if 1
; 2-0 - prescaler
	SET_REGISTER T0CON, B'10001111'
	SET_TIMER_REGISTER TMR0L, H'0000'






; interrupts
; 6 - A/D converter interrupt enable
; 0 - timer 1 interrupt enable
	SET_REGISTER PIE1, B'01000000'
; 6 - A/D converter flag
; 0 - timer 1 flag
	SET_REGISTER PIR1, B'00000000'
; 7 - enable priority levels on interrupts
	SET_REGISTER RCON, B'00000000'
; 7 - global interrupt enable
; 6 - peripheral interrupt enable
; 5 - timer0 interrupt enable
; 2 - timer0 flag
	SET_REGISTER INTCON, B'11100000'
	bsf ADCON0, D'1'









loop:
	goto loop



interrupt:
	clrwdt
	setf INTERRUPT_FINISHED


	btfsc INTCON, TMR0IF
; got timer overflow
		call handle_timer

	btfsc PIR1, ADIF
; got a/d converter completion
		call handle_analog


	btfss INTERRUPT_FINISHED, D'0'
	goto interrupt
		retfie







handle_timer:
; reset timer overflow
	bcf INTCON, TMR0IF
	clrf INTERRUPT_FINISHED


	SKIP_NOTEQUAL_LITERAL STATE, SENSOR_DELAY
	goto sensor_delay_timeout


	SKIP_NOTEQUAL_LITERAL STATE, ACTIVE
	goto active_timeout




;cooldown timeout
; do nothing
		SET_TIMER_REGISTER TMR0L, H'0000'
		return





sensor_delay_timeout:
		bsf ADCON0, D'1'
; invalidate prev analog for first reading
		setf PREV_ANALOG
		SET_REGISTER STATE, INACTIVE
		SET_TIMER_REGISTER TMR0L, H'0000'
		return








active_timeout:
; pulse finished
		MAGNET_OFF
		SET_REGISTER STATE, SENSOR_DELAY
		SET_TIMER_REGISTER TMR0L, INACTIVE_DELAY
		return










handle_analog:
; reset a/d converter interrupt
	bcf PIR1, ADIF
	clrf INTERRUPT_FINISHED


	SKIP_EQUAL_LITERAL STATE, INACTIVE
	return


		SKIP_LESS_LITERAL ADRESH, MAX_FLUX
		goto over_maximum


; under maximum flux
			COPY_REGISTER CURRENT_ANALOG, ADRESH
			SET_REGISTER CURRENT_MIN_FLUX, MIN_FLUX

; get prev_analog - current_analog
			SUBTRACT_REGISTER PREV_ANALOG, CURRENT_ANALOG

; skip if prev_analog >= current_analog
				btfss STATUS, C
				goto derivative1

; skip if prev_analog >= current_analog + max derivative
					SKIP_GREATER_LITERAL PREV_ANALOG, DAMP_DERIVATIVE
					goto derivative1

; adjust threshold
						SET_REGISTER CURRENT_MIN_FLUX, DAMP_FLUX



derivative1:
; skip if above threshold
			SKIP_GREATER ADRESH, CURRENT_MIN_FLUX
			goto below_threshold

; above threshold
				SET_TIMER_REGISTER TMR0L, UP_DELAY
				COPY_REGISTER PREV_ANALOG, ADRESH
				bsf ADCON0, D'1'
				return


below_threshold:
				MAGNET_UP
				SET_REGISTER STATE, ACTIVE
				bcf INTCON, TMR0IF
				return

over_maximum:
	SET_TIMER_REGISTER TMR0L, DOWN_DELAY
	MAGNET_DN
	SET_REGISTER STATE, ACTIVE
	bcf INTCON, TMR0IF
	return



END