; BitESC
; (c) 2004 Johan Van den Brande

.include "2343def.inc"

.equ TIMEBASE_TICK = 15000
.equ SAFETY = 25
.equ PWM_MAX = 63
.equ ESC_POS_MAX = 15

.equ esc_bit = 0
.equ down_bit = 3
.equ up_bit = 4

.equ PORTB_INIT = (1<<down_bit) | (1<<up_bit)
.equ DDRB_INIT = 1<<esc_bit

.def tmp 		= R16
.def esc_value	= R17
.def esc_pos 	= R18
.def pwm_value	= R19
.def safety		= R20

.def timebase_l	= R24
.def timebase_h = R25

.macro loadesc
	ldi	ZL,LOW(2*esc_values)
	ldi	ZH,HIGH(2*esc_values)
	clr	tmp
	add	ZL,esc_pos
	add ZH,tmp
	lpm
	mov	esc_value, R0
.endmacro

.cseg
.org $0000
	rjmp	main

main:
	ldi		tmp,LOW(RAMEND)
	out		SPL,tmp

	ldi		esc_pos, 0
	ldi		safety, SAFETY
	ldi		timebase_l, LOW(TIMEBASE_TICK)
	ldi		timebase_h,	HIGH(TIMEBASE_TICK)
	ldi		tmp, PORTB_INIT
	out		PORTB, tmp
	ldi		tmp, DDRB_INIT
	out		DDRB, tmp
	
	loadesc

loop:
	; pwm
	ldi		tmp, PORTB_INIT | (1<<esc_bit)
	cp		pwm_value, esc_value
	brlt	skip
	cbr		tmp, 1<<esc_bit
skip:
	out		PORTB, tmp
	inc		pwm_value
	andi	pwm_value, PWM_MAX
	
	; check timebase
	sbiw	timebase_l, 1
	brne	loop

	ldi		timebase_l, LOW(TIMEBASE_TICK)
	ldi		timebase_h,	HIGH(TIMEBASE_TICK)

	; read input states
	sbic	PINB, down_bit
	rjmp	skip_down
	; reset safety
	ldi		safety, SAFETY
	; motor --
	cpi		esc_pos, 0
	breq	skip_down
	dec		esc_pos
skip_down:

	sbic	PINB, up_bit
	rjmp	skip_up
	; reset safety
	ldi		safety, SAFETY
	; motor ++
	cpi		esc_pos, ESC_POS_MAX
	brge	skip_up
	inc		esc_pos
skip_up:
	
	cpi 	safety, 0
	breq	safety_powerdown
	dec 	safety
	rjmp	safety_proceed
safety_powerdown:
	cpi		esc_pos, 0
	breq	safety_proceed
	dec		esc_pos
safety_proceed:

	loadesc

	rjmp	loop

esc_values:
.db	0,	2
.db	4,	6
.db	8,	10
.db	13,	16
.db	19,	23
.db	27,	32
.db	38,	46
.db	57,	63