;=======================================================================================
;                   Speed Controller With Full Bridge Output
;=======================================================================================
;                    Bruce Abbott bhabbott@paradise.net.nz
;
; for Microchip PIC12F629 or PIC12F675
;
; Features:
;
;     - Bi-directional control of brushed DC motor 
;     - Neutral throttle detected at 1.5mS or 1.37mS
;     - LVC proportional to initial battery voltage


                PROCESSOR PIC12F675
                INCLUDE   <P12F675.inc>
                radix     dec

;#DEFINE SIMULATE	; for debugging only!
#DEFINE SLOW		; rate-limited throttle response 

#define VERSION "=BRIDGE===V0.1=="

; =====================================================================================
; Configuration is:
;   Master Clear pin is disabled (used as input)
;   Code Protection is OFF
;   Watchdog Timer is ON
;   Oscillator is Internal RC
                __config  _MCLRE_OFF & _CP_OFF & _WDT_ON & _INTRC_OSC_NOCLKOUT

; GPIO register bits 
RevLoBit        EQU       0   ; GP0, pin 7. Turns on motor reverse FET
BattBit         EQU       1   ; GP1, pin 6. Battery status input
ForLoBit        EQU       2   ; GP2, pin 5. Turns on motor forward FET
ServoBit        EQU       3   ; GP3, pin 4. Servo pulse input
RevHiBit        EQU       4   ; GP3, pin 3. Turns on high-side reverse FET
ForHiBit        EQU       5   ; GP4, pin 2. Turns on high-side forward FET

; TRIS register value
TrisBits        EQU       (1<<ServoBit)|(1<<BattBit)

; Bits for H bridge activation

ON_FORWARD      EQU (1<<ForLoBit)|(1<<ForHiBit)
OFF_FORWARD     EQU (1<<ForHiBit)
ON_STOP         EQU 0
OFF_STOP        EQU 0
ON_REVERSE      EQU (1<<RevLoBit)|(1<<RevHiBit)
OFF_REVERSE     EQU (1<<RevHiBit)


; Bits to be set with the OPTION instruction
;   No wake up
;   No weak pullups
;   Timer 0 source internal
;   Which edge is don't care
;   Prescaler to watchdog, set to give 16*17 = 272ms timeout

OptionBits      EQU       B'11011100'

; =====================================================================================
; Algorithm constants
;
ArmPulsesReqd   EQU       25   ; number of good servo pulses required to allow starting     
PWMLOOPS        EQU       42   ; number of steps in Motor PWM cycle           
PRECHARGE	EQU	-112   ; pre-charge servocount for roll over at 0.9mS

;======================================================================================
; Macro to create offsets for general purpose registers in RAM
;

_addr           SET 32    ; general purpose registers start here

BYTE            MACRO     _name
_name           EQU       _addr
_addr           SET       _addr+1
                ENDM

; =====================================================================================
; General Purpose Register Definitions.  
;

                BYTE       Flags           ; Various boolean flags            
                BYTE       PwmCount        ; Counts down to zero during PWM output  
                BYTE       PwmWidth        ; PWM loop counter
                BYTE       MotorON         ; GPIO output bits for motor ON
                BYTE       MotorOFF        ; GPIO output bits for motor OFF 
                BYTE       ServoCount      ; servo pulse count 0-255 * 8uS
                BYTE       HiCycles        ; counts PWM cycles during servo pulse
                BYTE       PrevCount       ; Previous servo pulse width
                BYTE       Index           ; Index into table of PWM widths (0-75)
                BYTE       ArmPulseCount   ; Arming pulses required (counts down)
                BYTE       PwmPeriod       ; Number of 8uS loops in one PWM cycle
		BYTE	   Volts	   ; battery voltage
		BYTE       temp		   ; general purpose variable

; Bits in Flags

BattLow         EQU       0	; low battery detected
PriorServoBit   EQU       1	; servo pulse was high
BadPulseBit     EQU       2	; servo pulse was bad 
Stop1		EQU	  3	; last arm pulse was in table 1 stop area
Stop2		EQU	  4	; last arm pulse was in table 2 stop area
UseTable1	EQU	  5	; using throttle table 1
UseTable2	EQU	  6	; using throttle table 2


; ==================================================================================
; Macro for generating short time delays
;
NO_OP           MACRO   count
NO_OP_COUNT     SET     count
                WHILE   NO_OP_COUNT>1
		goto	$+1		; 2 clocks
NO_OP_COUNT     SET     NO_OP_COUNT-2
                ENDW
		IF	NO_OP_COUNT
		nop			; 1 clock
		ENDIF
                ENDM

; ==================================================================================
; Macro to start PWM Cycle
; 
; Takes 4 cycles to execute
;
StartPWM        MACRO
		movf	 MotorON,W
		movwf    GPIO
		movf     PwmWidth,W
		movwf    PwmCount
		ENDM

; =================================================================================
; Macro to keep up with the servo pulse duration
; The code in this Macro uses 2 cycles.
;
DoServoCount    MACRO                          
                btfsc     GPIO, ServoBit
                incf      ServoCount, F
                ENDM


;==================================================================================
;                                  RESET
;==================================================================================
;
Coldstart:      ORG       0                

		goto	Start

		org	  0x8
                dt        VERSION


;=================================================================================
;                          Throttle Curve Table  
;================================================================================
; Returns with PWM width and Reverse Flag (bit 7) in W.
; Takes 5 CPU cycles to execute (including call and retlw).
;
; There are two tables. The second table is accessed by adding 75 to W.
;
; Table1:- Has equal forward and backward range, for stick with neutral at 1.5mS
; Table2:- More forward than reverse range, for stick with neutral at 1.37mS    
;
Table:	    addwf   PCL,F      ; computed goto, W = 0  to 150
; Table 1
            retlw   128+43     ; 0  reverse max at 1.2mS
            retlw   128+42     ; 1 
            retlw   128+42     ; 2
            retlw   128+41     ; 3
            retlw   128+41     ; 4
            retlw   128+40     ; 5
            retlw   128+40     ; 6
            retlw   128+39     ; 7
            retlw   128+38     ; 8
            retlw   128+37     ; 9
            retlw   128+36     ;10
            retlw   128+35     ;11
            retlw   128+34     ;12
            retlw   128+33     ;13
            retlw   128+32     ;14
            retlw   128+31     ;15
            retlw   128+30     ;16
            retlw   128+29     ;17
            retlw   128+28     ;18    reverse
            retlw   128+26     ;19 
            retlw   128+24     ;20 
            retlw   128+22     ;21     
            retlw   128+20     ;22  
            retlw   128+18     ;23 
            retlw   128+16     ;24 
            retlw   128+14     ;25 
            retlw   128+12     ;26 
            retlw   128+10     ;27 
            retlw    128+8     ;28 
            retlw    128+6     ;29    
            retlw    128+4     ;30 
            retlw    128+2     ;31 
            retlw        0     ;32 }    
            retlw        0     ;33 }
            retlw        0     ;34 }
            retlw        0     ;35 }
            retlw        0     ;36 }
            retlw        0     ;37 }  STOP at 1.5mS
            retlw        0     ;38 }
            retlw        0     ;39 }
            retlw        0     ;40 }
            retlw        0     ;41 }
            retlw        0     ;42 }
            retlw        2     ;43
            retlw        4     ;44
            retlw        6     ;45
            retlw        8     ;46
            retlw       10     ;47
            retlw       12     ;48
            retlw       14     ;49
            retlw       16     ;50
            retlw       18     ;51
            retlw       20     ;52
            retlw       22     ;53
            retlw       24     ;54
            retlw       26     ;55  forward
            retlw       27     ;56
            retlw       28     ;57
            retlw       29     ;58
            retlw       30     ;59
            retlw       31     ;60
            retlw       32     ;61
            retlw       33     ;62
            retlw       34     ;63
            retlw       35     ;64
            retlw       36     ;65
            retlw       37     ;66
            retlw       38     ;67
            retlw       39     ;68
            retlw       40     ;69
            retlw       40     ;70
            retlw       41     ;71
            retlw       41     ;72
            retlw       42     ;73
            retlw       42     ;74
            retlw       43     ;75 forward max at 1.8mS
; Table 2
            retlw    128+30    ; 0  reverse max at 1.2mS
            retlw    128+29    ; 1 
            retlw    128+28    ; 2
            retlw    128+27    ; 3
            retlw    128+26    ; 4
            retlw    128+25    ; 5
            retlw    128+24    ; 6
            retlw    128+22    ; 7
            retlw    128+20    ; 8
            retlw    128+18    ; 9   reverse
            retlw    128+16    ;10
            retlw    128+14    ;11
            retlw    128+12    ;12
            retlw    128+8     ;13
            retlw    128+6     ;14
            retlw    128+4     ;15
            retlw        0     ;16 }
            retlw        0     ;17 }
            retlw        0     ;18 }
            retlw        0     ;19 }
            retlw        0     ;20 }
            retlw        0     ;21 }  STOP at 1.37mS  
            retlw        0     ;22 } 
            retlw        0     ;23 }
            retlw        0     ;24 }
            retlw        0     ;25 }  
            retlw        0     ;26 }
            retlw        2     ;27 
            retlw        4     ;28 
            retlw        6     ;29    
            retlw        8     ;30 
            retlw       10     ;31 
            retlw       12     ;32    forward
            retlw       14     ;33
            retlw       16     ;34
            retlw       18     ;35
            retlw       20     ;36
            retlw       22     ;37
            retlw       23     ;38
            retlw       24     ;39
            retlw       25     ;40
            retlw       26     ;41
            retlw       27     ;42
            retlw       28     ;43
            retlw       29     ;44
            retlw       30     ;45
            retlw       30     ;46
            retlw       31     ;47
            retlw       31     ;48
            retlw       32     ;49
            retlw       32     ;50
            retlw       33     ;51
            retlw       33     ;52
            retlw       34     ;53
            retlw       34     ;54
            retlw       35     ;55
            retlw       35     ;56
            retlw       36     ;57
            retlw       36     ;58
            retlw       37     ;59
            retlw       37     ;60
            retlw       38     ;61
            retlw       38     ;62
            retlw       39     ;63
            retlw       39     ;64
            retlw       40     ;65
            retlw       40     ;66
            retlw       40     ;67
            retlw       41     ;68
            retlw       41     ;69
            retlw       41     ;70
            retlw       42     ;71
            retlw       42     ;72
            retlw       42     ;73
            retlw       42     ;74
            retlw       43     ;75 forward max at 1.8mS

;----------------------------------------------------------------------------------
;                   Battery Voltage Cutoff Table
;----------------------------------------------------------------------------------
CutoffTable:
		addwf	PCL,F	; BATTVOLTS   CUTOFF    BATTERY TYPE
		retlw	3	; 0   4.0V        
		retlw	3	; 1   4.5V     
		retlw	3	; 2   5.0V        
		retlw	3	; 3   5.5V        
		retlw	3	; 4   6.0V     5.5V   6V SLA     
		retlw	3	; 5   6.5V     5.5V   6V SLA
		retlw	4	; 6   7.0V     6.0V   7.2V Nicad (6 cells)
		retlw	4	; 7   7.5V     6.0V   7.2V Nicad (6 cells)
		retlw	4	; 8   8.0V     6.0V   7.2V Nicad (6 cells)
		retlw	6	; 9   8.5V     7.0V   8.4V Nicad (7 cells)
		retlw	6	; 10  9.0V     7.0V   8.4V Nicad (7 cells)
		retlw	6	; 11  9.5V     7.0V   8.4V Nicad (7 cells)
		retlw	8	; 12 10.0V     8.0V   9.6V Nicad (8 cells)
		retlw	8	; 13 10.5V     8.0V   9.6V Nicad (8 cells) 
		retlw	8	; 14 11.0V     8.0V   9.6V Nicad (8 cells)
		retlw	14	; 15 11.5V    11.0V   12V SLA   


;==================================================================================
;                         Measure Battery Voltage
;==================================================================================
;
BattVolts:	movlw	15
		movwf	Volts		; start comparison at maximum Volts
bv_next:	bsf	STATUS,RP0
		movf	Volts,w
		movwf	VRCON		; set Vref divider to Volts
		bsf	VRCON,VREN	; enable Vref
		bcf	STATUS,RP0
		NO_OP	4		; wait 5uS
		btfss	CMCON,COUT	; Battery voltage higher than VRef?
		goto	bv_done		; yes, done
		decfsz	Volts		; no, try lower Vref
		goto	bv_next
bv_done:	return
		

;==================================================================================
;                        Measure servo pulse width                
;==================================================================================
;
Measure:	movlw	PRECHARGE
		movwf	ServoCount		; start servocount at -0.9mS
                bcf	Flags,BadPulseBit
Measure1: 	clrwdt
                btfss	GPIO,ServoBit		; wait for start of pulse       
                goto	Measure1             
Measure2:  	clrwdt				;(1)>
		nop				;(1)> measuring first 0.9mS
	        incf	ServoCount,F		;(1)> 
                skpnz				;(2)> done 0.9mS ?
                goto	Measure3		;(+1)     
                btfsc	GPIO,ServoBit		;(1)> pulse finished early ?
                goto	Measure2		;(2)> 
MeasureError:	clrf	ServoCount		;     bad if pulse < 0.9mS
		goto	MeasureDone		
Measure3:	clrwdt				;(1)]
		nop				;(1)] measuring to end of pulse
		incf	ServoCount,F		;(1)]
		skpnz				;(2)] bad if pulse > 2.9mS
		goto	MeasureError		;     
		btfsc	GPIO,ServoBit		;(1)] done if end of pulse
		goto	Measure3		;(2)]
MeasureDone:	movf	ServoCount,w
		return


; ================================================================================
; PWM pulse subroutine.  On entry, the motor may be on or off. If the motor 
; is off, it will never be turned on.  However, it will be turned off when/if 
; the PwmCount register reaches zero.
;
; takes 350 CPU cycles. 
;
; Minimum ON time is 27uS 
; 2uS inside StartPWM + 18uS + 7uS inside DoPwmPulse.
;
; Minimum OFF time is 13uS
; 7uS inside DoPwmPulse + 4uS + 2uS inside StartPWM.
;
;
;      2,2        18        7           336 cycles          7    4    2,2      
;        ____________________                                           _______
;  _____/                    \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\___________/
;        ON                     OFF at (PwmCount-1) * 8uS              ON
;    (startPWM)             |<--------- DoPwmPulse -------->|       (startPWM)                                
;
;
DoPwmPulse:     movlw     PWMLOOPS              ;(1) set PWM period counter
                movwf     PwmPeriod             ;(1) 
                movf      MotorOFF,W            ;(1) get GPIO value for motor off
pwmloop:        decf      PwmCount,F            ;(1)
                skpnz                           ;(1) IF end of PWM high time
                movwf     GPIO                  ;(1) THEN turn off motor
		IFDEF	SIMULATE
		nop				
		nop
		ELSE
                btfsc     CMCON,COUT            ;(2) 
                goto      BattLoShutdown        ; quit if battery low
 		ENDIF
                decfsz    PwmPeriod,F           ;(2)
                goto      pwmloop               ;(+1)                           
                retlw     0                     ;(2) done 
                                                
;------------------------------------------------------------------------------------
;                           END OF SUBROUTINES
;------------------------------------------------------------------------------------


                ORG       256               

; Retrieve and set the factory-programmed oscillator calibration value.
Start:		call	0x3ff

 		bsf	STATUS,RP0	; Register bank 1
        	movwf   OSCCAL          ; set oscillator frequency
		movlw	b'00000010'
		movwf	ANSEL		; Analog I/O on GP1
	       	bcf	STATUS,RP0      ; register bank 0
		movlw	b'00000100'      
        	movwf	CMCON		; Comparator in- to GP1, in+ to VREF
;
; Move the prescaler from tmr0 to the watchdog, without accidental resets.
;
                clrwdt                    
                clrf      TMR0            
                movlw     OptionBits | 7  
                option                    
                clrwdt
                movlw     OptionBits
                option
                clrwdt

; Initialise GPIO register
;
                clrf    GPIO		; all outputs will be low
                movlw   TrisBits
                tris    GPIO		; set I/O directions

InitDelay:	clrf	PwmPeriod
InitDelay1:	clrf	temp
InitDelay2:	clrwdt		
		NO_OP	4		; wait 0.5s for peripherals to stabilize
		decfsz	temp
		goto	InitDelay2
		decfsz	PwmPeriod
		goto	InitDelay1

; set battery cutoff voltage

		call	BattVolts	; get battery voltage
		movf	Volts,w
		call	CutoffTable	; calculate cutoff voltage
	       	bsf	STATUS,RP0      ; register bank 0
		movwf	VRCON		; set VRef voltage for cutoff 
		bsf	VRCON,VREN	; enable VRef
	       	bcf	STATUS,RP0      ; register bank 0

                clrf      Flags		; clear memory         

Rearm:		clrwdt
		IFNDEF	SIMULATE
                btfsc   CMCON,COUT	; wait for good battery voltage
                goto    Rearm                         
		ENDIF
;
; Wait for a whole servo pulse before looking for arm pulses
;
RaWaitOn:	clrwdt 
		btfss   GPIO, ServoBit
                goto    RaWaitOn
RaWaitOff: 	btfsc   GPIO, ServoBit
                goto    RaWaitOff
;
; wait until throttle is in neutral position 
;
RaStartCount:   movlw   ArmPulsesReqd
                movwf   ArmPulseCount	; set required number of good pulses
		bcf	Flags,Stop1
		bcf	Flags,Stop2	

RaNextPulse:	call	Measure		; measure servo pulse width
		skpnz			; bad pulse ?
		goto	RaStartCount
		movwf	PrevCount	; remember servo pulse
		movwf	Index
                movlw	38                 
                subwf	Index,F             
                skpc			; minimum = 1.2mS  
                clrf	Index                      
                movlw	75                 
                subwf	Index,W		; maximum = 1.8mS (index range: 0 to 75)
                skpnc			
		subwf	Index,F	

		btfsc	Flags,UseTable2	; using throttle table 2 ?	
		goto	RaTable2	; yes, check for table 2 STOP

		clrf	PCLATH
		movf	Index,w
		call	Table
		xorlw	0		; Table1 STOP ?
		skpnz
		goto	RaStop1					

		btfsc	Flags,UseTable1	; using thottle table 1 ?
		goto	RaStartCount	; no, stick not at neutral

RaTable2:	clrf	PCLATH
		movf	Index,w
		addlw	76
		call	Table
		xorlw	0		; Table 2 STOP ?
		skpz
		goto	RaStartCount	; no, stick not at neutral

RaStop2:	bsf	Flags,Stop2	; now in Table 2 STOP region
		btfss	Flags,Stop1	; was last pulse in Table 1 STOP region ?
		goto	RaCount		; no, continue
		bcf	Flags,Stop1 
		goto	RaStartCount	; yes, restart

RaStop1:	bsf	Flags,Stop1	; now in table 1 STOP region
		btfss	Flags,Stop2	; was last pulse in Table 2 STOP region ?	
		goto	RaCount		; no, continue
		bcf	Flags,Stop2	
		goto	RaStartCount	; yes, restart
	

RaCount:	decfsz	ArmPulseCount,F	; Do we have enough arm pulses?
                goto	RaNextPulse	; Not yet.

;
; Set throttle table to use
;
		btfsc	Flags,Stop1	
		bsf	Flags,UseTable1	
		btfsc	Flags,Stop2
		bsf	Flags,UseTable2 
;
; Play a 2KHz 'beep' sound for 1/8 second, to indicate successful arming.
; (uses the motor as a speaker!)
;
PlayBeep:
                clrf      PwmCount     
beep1:          movlw	  ON_FORWARD
		movwf	  GPIO		       ; motor on 
                movlw     5
                movwf     PwmPeriod
beep2:          clrwdt                         ; for 22uS 
                decfsz    PwmPeriod,F
                goto      beep2
                movlw	  OFF_FORWARD
		movwf	  GPIO		       ; motor off
                movlw     128
                movwf     PwmPeriod                
beep3:          clrwdt                     
                decfsz    PwmPeriod,F
                goto      beep3                ; for 0.5mS 
                decfsz    PwmCount,F
                goto      beep1                ; 256 cycles

; wait for end of next servo pulse

servo_hi:       btfss     GPIO,ServoBit
                goto      servo_hi
servo_lo:       btfsc     GPIO,ServoBit
                goto      servo_lo

; ready to go!            
                clrf      MotorON              ; Motor FETs OFF  
                clrf      MotorOFF             ; Direction FETs OFF
                clrf      PwmWidth             ; PWM=0

                movlw     PRECHARGE
                movwf     ServoCount           ; precharge ServoCount 

		goto	  MainPwmEntry

; ==================================================================================
; The main PWM loop
;
; Two timing loops have to be simultaneously maintained:-
;
; 1/ Measure servo pulse width every 8uS. DoServoCount takes 2 CPU 
;    cycles, thus 6 CPU cycles are available between each instance.
;  
; 2/ StartPWM + 18 cycles + DoPwmPulse + 4 cycles etc. This works  
;    out to 372 CPU cycles, creating a PWM frequency of 2.7Khz.
;
; The watchdog timer is cleared only AFTER we have received a good servo 
; pulse. Loss of signal will cause the PIC to reset itself!
;
;
MainPwmLoop:    DoServoCount                   ;(2) <--- }
                movlw     PWMLOOPS             ;(1)      }                  
                movwf     PwmPeriod            ;(1)      }
                movf      MotorOFF,W           ;(1)      }
pwmpulseloop:   decf      PwmCount,F           ;(1)      }
                skpnz                          ;(1)      } <=== Complete PWM cycle.
                movwf     GPIO                 ;(1)      }      Also measure servo
                DoServoCount                   ;(2) <--- }      pulse width.
                decfsz    PwmPeriod,F          ;(2)      }      
                goto      pwmpulseloop         ;(+1)     }   (takes 342 CPU cycles)    
		IFDEF	SIMULATE
		nop
		nop
		ELSE
                btfsc     CMCON,COUT           ;(2)      }
                goto      BattLoShutdown       ; quit if battery low
		ENDIF
                NO_OP     2                    ;(2)
                DoServoCount                   ;(2) <---
MainPwmEntry:   StartPWM                       ;(4) ====> start 1st PWM cycle
                NO_OP     2                    ;(2)
                DoServoCount                   ;(2) <--- 
                btfsc     GPIO, ServoBit       ;(2)
                goto      ServoIsHigh          ;(+1) Is servo pulse High ?
                btfsc     Flags, PriorServoBit ;(2)
                goto      EndOfPulse           ;(+1) end of servo pulse ?
		movlw	  PRECHARGE	       ;(1)
                movwf     ServoCount           ;(1) pre-charge servocount
                DoServoCount                   ;(2) <---
                clrf      HiCycles             ;(1)
                NO_OP     3                    ;(1)
                goto      MainPwmLoop          ;(2) 
                
ServoIsHigh:    bsf       Flags, PriorServoBit ;(1) servo pulse is High!
                NO_OP	  2		       ;(2) 
                DoServoCount                   ;(2) <---
                incf      HiCycles,F           ;(1) Count PWM cycles while servo is high 
                skpnz                          ;(1)
                decf      HiCycles,F           ;(1) avoid HiCycles overflow
                NO_OP     1                    ;(1)
                goto      MainPwmLoop          ;(2) 


; ============================================================================
; End of Servo Pulse. We don't have to measure the servo pulse width now, but 
; we still have to maintain the PWM cycle:-
;
;   StartPWM + 18 clocks + DoPwmPulse + 4 clocks + StartPWM etc.
;
;
; gets here 9 clocks after start of 1st PWM

EndOfPulse:     bcf       Flags,BadPulseBit     ;(1) assume servo pulse is OK
                movlw     2                     ;(1)
                subwf     HiCycles,W            ;(1) short pulse ? (less than 3 PWM's)
                skpc                            ;(1) 
                bsf       Flags,BadPulseBit     ;(1)
                movlw     7                     ;(1)
                subwf     HiCycles,W            ;(1) long pulse ? (more than 6 PWM's)
                skpnc                           ;(1) 
                bsf       Flags,BadPulseBit     ;(1)
                call      DoPwmPulse            ;(350) <=== Complete 1st PWM cycle
                movlw     163                   ;(1)
                subwf     ServoCount,W          ;(1) servo pulse width in-range ?       
                skpnc                           ;(1) ( 0.9mS < pulse < 2.2mS) 
                bsf       Flags,BadPulseBit     ;(1) 
                StartPWM                        ;(4) ====> Start 2nd PWM cycle
                btfsc     Flags,BadPulseBit     ;(2)  ignore bad pulses
                goto      BadPulse              ;(+1)

		IFDEF  SLOW ; change-rate limited throttle

		movf	  ServoCount,w		;(1) compare throttle to last position
		subwf	  PrevCount,w		;(1)
		skpc	  			;(2) going up ?
		goto	  ThrottleUp		;(+1)
		skpnz				;(2) going down ?
		goto	  NoChange  		;(+1)

ThrottleDown:	addlw	  -2			;(1) 
		skpnc				;(1) 
		addwf	  ServoCount		;(1) servocount = prevcount - 2
		NO_OP	  3			;(3)
		goto	  Slowed		;(2)

NoChange: 	NO_OP	  5			;(5)
		goto	  Slowed		;(2)

ThrottleUp:	addlw	  2			;(1)
		skpc				;(1)
		addwf	  ServoCount		;(1) servocount = prevcount + 2
		NO_OP	  6			;(6)

Slowed:		movf	  ServoCount,w		;(1)
		movwf	  Index			;(1)
		
		ELSE	; mean-averaged throttle

                movf      ServoCount,W          ;(1)
                movwf     Index                 ;(1) Index = ServoCount (0-163) 
                movf      PrevCount,W           ;(1) 
                addwf     Index,F               ;(1)  
                rrf       Index,F               ;(1) Index = Average(ServoCount+PrevCount)
		NO_OP	  11			;(11)

		ENDIF

	        call      DoPwmPulse            ;(350) <=== Complete 2nd PWM cycle
		NO_OP	  4			;(4)
		StartPWM			;(4) ====> Start 3rd PWM cycle
                movlw     38                    ;(1) 
                subwf     Index,F               ;(1)  
                skpc                            ;(1) minimum = 1.2mS  
                clrf      Index                 ;(1)         
                movlw     75                    ;(1) 
                subwf     Index,W               ;(1) maximum = 1.8mS 
                skpnc				;(1)
		subwf	  Index,F		;(1)
		movlw	  76			;(1)
		btfsc	  Flags,UseTable2	;(1) using second throttle table ?
		addwf	  Index			;(1)
                clrf	  PCLATH		;(1) required for simulation only!
		NO_OP	  6			;(6)
                call      DoPwmPulse            ;(350) <=== Complete 3rd PWM cycle
                NO_OP     4                     ;(4)
                StartPWM                        ;(4) ====> Start 4th PWM cycle
                movf      Index,W               ;(1)
                call      Table                 ;(5) do table lookup
                movwf     PwmWidth              ;(1) 
                btfsc     PwmWidth,7            ;(2) reverse flag set ?
                goto      reversing             ;(+1)
                tstf      PwmWidth              ;(1) if PWM Width = 0
                skpnz                           ;(2) then stop
                goto      stopping              ;(+1) 
                movlw     ON_FORWARD            ;(1) MotorON set for Forward
                movwf     MotorON               ;(1)
                movlw     OFF_FORWARD           ;(1) store new MotorOFF in temp (must
                movwf     temp                  ;(1) complete this PWM with old MotorOFF) 
                goto      oldpwm                ;(2)
reversing:      bcf       PwmWidth,7            ;(1) remove reverse flag from PwmWidth
                movlw     ON_REVERSE            ;(1) MotorON set for Reverse
                movwf     MotorON               ;(1)      
                movlw     OFF_REVERSE           ;(1) store new MotorOFF for Reverse
                movwf     temp                  ;(1) 
                NO_OP     1                     ;(1) 
                goto      oldpwm                ;(2)
stopping:       movlw     ON_STOP               ;(1) MotorON set for Stop
                movwf     MotorON               ;(1)
                movlw     OFF_STOP              ;(1) Store new MotorOFF for Stop
                movwf     temp                  ;(1)     
                NO_OP     1                     ;(1)
oldpwm:         call      DoPwmPulse            ;(350) <=== Complete 4th PWM cycle 
                movf      temp,W                ;(1) 
                xorwf     MotorOFF,F            ;(1) are new MotorOFF bits same as old ?
                skpz                            ;(1) if not, we are changing direction
                movwf     GPIO                  ;(1) so turn motor OFF now!
                StartPWM                        ;(4) ====> Start next PWM cycle 
                movf      temp,W                ;(1) 
                movwf     MotorOFF              ;(1) set new Motor_OFF
                movf      ServoCount,W          ;(1) 
                movwf     PrevCount             ;(1) remember this servo pulse 
                NO_OP     2                     ;(2)
		clrwdt				;(1) Good pulse, so clear watchdog
BadPulse:       bcf       Flags,BadPulseBit     ;(1)
                movlw     PRECHARGE             ;(1)
                movwf     ServoCount            ;(1) precharge ServoCount                 
                bcf       Flags,PriorServoBit   ;(1) get ready for next servo pulse
                clrf      HiCycles              ;(1)               ''
                NO_OP     6                     ;(6)
                call      DoPwmPulse            ;(350) <=== Complete PWM cycle
                NO_OP     2                     ;(2)
                goto      MainPwmEntry          ;(2)


;
; Low battery - shutdown and wait for arming.
;
BattLoShutdown: movf      MotorOFF,W             
                movwf     GPIO                  ; Stop the motor
                
                goto      Rearm			; restart                 


                org	0x3ff
 		RETLW	0x80		; OSCCAL value programmed at factory

                END

