;
; Pulse Width Modulation Control routine.
;
; Inputs: RightM, LeftM
;
;	8 bit signed values: current motor drive levels.  
;
; Outputs: Control signals on PORTD
;
; Uses:	
; 
; 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 RightM and LeftM 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
;
;Set up the hardware initialization equates.  Only need to set pins to output
;since the reset default of logic 0 is what we want for the motor drivers.
;
.set	DDRD_INIT = DDRD_INIT + (1<<Rt_fwd) + (1<<Rt_rev) + (1<<Lt_fwd) + (1<<Lt_rev)
;
; Define our output bits for the motor
;
.equ	Rt_fwd	= PD0		; Motor control bits
.equ	Rt_rev	= PD1		; Port D(2) = external interrupt
.equ	Lt_fwd	= PD3
.equ	Lt_rev	= PD5
;
; Pulse Width Modulation limits
;
.equ	d_min		= 00		; Min and Max counter and drive values
.equ	d_max		= 10		; values between 127 and 0

.equ	PWM_STOP	= 0
.equ	PWM_SLOW	= 5
.equ	PWM_MED		= 6
.equ	PWM_FAST	= 7

;* Register variables
;	PW_Count	free running PW_Counter
;	hold_1		Motor 1 holding register
;	hold_2		Motor 2 Holding register
;	RightM		Motor 1 level, signed
;	LeftM		Motor 2 level, signed

;* Code

pw_control:
	tst	PW_Count		; if (PW_Count == 0)
	brne	pw_down			; {
	ldi	I_tmp, d_max		; 
	mov	PW_Count, I_tmp		;     PW_Count = d_max;
pw_1_reset:
	cbi	MOTOR, Rt_fwd		;     // Reset Motor 1
	cbi	MOTOR, Rt_rev		;     Clear motor control bits
	mov	hold_1, RightM		;     hold_1 = RightM
	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,  Rt_fwd		;	  set backward bit
	rjmp	pw_1_done		;	  hold_1 = -hold_1;
pw_1_rev:				;     }
	neg	hold_1			;	
	sbi	MOTOR,  Rt_rev		;     
	rjmp	pw_1_done
pw_1_off:
pw_1_done:				;	
					;
pw_2_res:
	cbi	MOTOR, Lt_fwd		;	// Reset Motor 2
	cbi	MOTOR, Lt_rev		;     Clear motor control bits
	mov	hold_2, LeftM		;     hold_2 = LeftM
	tst	hold_2			;     if (hold_2 >= 0) {
	brlt	pw_2_rev		;	   
	breq	pw_2_off
	sbi	MOTOR,  Lt_fwd		;	  Set Forward bit
	rjmp	pw_2_done		;     elseif (hold_2 < 0) {
pw_2_rev:
	neg	hold_2			;	  hold_2 = -hold_2
	sbi	MOTOR,  Lt_rev		;	  Set backward bit
pw_2_off:				;     }
pw_2_done:				; }
					; else
pw_down:				; {
	dec	PW_Count		;     PW_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, Rt_fwd		;         Clear bits; 
	cbi	MOTOR, Rt_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, Lt_fwd		; 
	cbi	MOTOR, Lt_rev		;	    Clear bits;
pw_exit:				; }
	ret