;
; Pulse Width Modulation Control routine.
;
; Inputs: Motor1, Motor2
;
;	8 bit signed values: current motor drive levels.  
;
; Outputs: Control signals on PORTB, lower nibble
;
; Uses:	R21 - R25
; 
; Decrements a free running counter that counts down from d_max to d_min
; then resets counter to d_max
;
; At the zero point of each cycle, the motor drive level is copied to the 
; holding registers and the direction and output bits are set.
;
; With each clock tick all registers are decremented.  When the holding
; registers reach zero, the output bits are cleared.
;
; The holding registers synchronize the PWM waveform with the counter so one
; can just stuff drive levels into Motor1 and 2 as needed.  They will be
; picked up on the next PWM cycle.
;
; Motor values > d_max just keep the motor on 100%
;
; Average # cycles per call: 30
;
;* Defines
.equ	MOTOR	= PORTD		; Motor Control PortD
.equ	MOT_DDR = DDRD		; Direction control portD
.set	DDRD_INIT = DDRD_INIT + (1<<d_m1_fwd) + (1<<d_m1_rev) + (1<<d_m2_fwd) + (1<<d_m2_rev)

.equ	d_m1_fwd	= 4		; Motor control bits
.equ	d_m1_rev	= 3		; Port D(2) = external interrupt
.equ	d_m2_fwd	= 1
.equ	d_m2_rev	= 0

.equ	d_min		= $00		; Min and Max counter and drive values
.equ	d_max		= $05		; These can be any values between 7F and 0

;* Register variables

.def	Count		= r21		; free running counter

.def	hold_1	= r22		; Motor 1 holding register
.def	hold_2	= r23		; Motor 2 Holding register

.def	Motor1	= r24		; Motor 1 level, signed
.def	Motor2	= r25		; Motor 2 level, signed

;* Code

pw_control:
	tst	count				; if (count == 0)
	brne	pw_down			; {
	ldi	count, d_max		;     count = d_max;
pw_1_reset:
	cbi	MOTOR, d_m1_fwd		;	// Reset Motor 1
	cbi	MOTOR, d_m1_rev		;	Clear motor control bits
	mov	hold_1, motor1		;	hold_1 = motor1
	tst	hold_1			;     if (hold_1 > 0)
	brlt	pw_1_rev			;	    set forward bit
	breq	pw_1_off			;	else if (hold_1 < 0) {
	sbi	MOTOR,  d_m1_fwd		;	    set backward bit
	rjmp	pw_1_done			;	    hold_1 = -hold_1;
pw_1_rev:					;	}
	neg	hold_1			;	
	sbi	MOTOR,  d_m1_rev		;     
	rjmp	pw_1_done
pw_1_off:
pw_1_done:					;	
						; }
pw_2_res:
	cbi	MOTOR, d_m2_fwd		;	// Reset Motor 2
	cbi	MOTOR, d_m2_rev		;	Clear motor control bits
	mov	hold_2, motor2		;	hold_2 = motor2
	tst	hold_2			;     if (hold_2 >= 0) {
	brlt	pw_2_rev			;	   
	breq	pw_2_off
	sbi	MOTOR,  d_m2_fwd		;	   Set Forward bit
	rjmp	pw_2_done			;	elseif (hold_2 < 0) {
pw_2_rev:
	neg	hold_2			;	   hold_2 = -hold_2
	sbi	MOTOR,  d_m2_rev		;        Set backward bit
pw_2_off:					;	}
pw_2_done:					; }
						; else
pw_down:					; {
	dec	count				;     count.d_val--;
pw_1_com:
	tst	hold_1			;	if (hold_1)
	breq	pw_m1_off
	dec	hold_1			;	    hold_1--;
	rjmp	pw_2_com
pw_m1_off:					;     else 
	cbi	MOTOR, d_m1_fwd		;         Clear bits; 
	cbi	MOTOR, d_m1_rev		;

pw_2_com:
	tst	hold_2			;	if (hold_2)
	breq	pw_m2_off
	dec	hold_2			;	    hold_2--;
	rjmp	pw_exit
pw_m2_off:					;     else
	cbi	MOTOR, d_m2_fwd		; 
	cbi	MOTOR, d_m2_rev		;	    Clear bits;
						; }
pw_exit:
	ret

;
; Initialization routine
;
; Set the outputs low before setting them as outputs.  That will prevent any
; glitches on the power buss
;
pwm_init:	clr	count
		clr	motor1
		clr	motor2

;		cbi	MOTOR, d_m1_fwd	; set value to zero (off)
;		cbi	MOTOR, d_m1_rev
;		cbi	MOTOR, d_m2_fwd
;		cbi	MOTOR, d_m2_rev

;		sbi	MOT_DDR, d_m1_fwd	; set port bits to output
;		sbi	MOT_DDR, d_m1_rev
;		sbi	MOT_DDR, d_m2_fwd
;		sbi	MOT_DDR, d_m2_rev

		ret