; =============================================================================
; Speed 400 speed controller embedded program
; speed400.asm
;
; ################ Copyright 1998 Michael J. Norton ######################
; ######## May be copied as desired and modified as needed ###############
; ######### so long as this copyright notice is preserved ################
;
;------------------------------------------------------------------------------
; The following comments were added by Bruce Abbott (bhabbott@paradise.net.nz)
;------------------------------------------------------------------------------
;
; Watch out for these quirks in the PIC12C509 architecture!
;
; - Calls and Computed Gotos can only target the lower half of each 512 
;   word program page. This means 0x000-0x0ff when PA0=0, 0x200-0x2ff 
;   when PA0=1. Direct Gotos can reach anywhere in the current page.
;
; - To execute a call or goto whose destination is in Page 1, you need to 
;   set PA0=1. Don't forget to clear it again before executing a goto or 
;   call to Page 0!
;
;
;
; Summary of Changes:
;
; V1.3
;
; - Added HiCount, counts number of PWM cycles while servo pulse is high.
;   This gives a (very) coarse indication of pulse width. Now we can filter 
;   out pulses which are shorter than 0.4mS or longer than 2.7mS (previously
;   these would be misinterpreted as valid in-range values!).
;
; - Removed Brake and Max Throttle LED outputs. 
;
; - GP4 pin now outputs an arming 'beep' and glitch count.
;   
; - Arming 'beep' also sent to the motor.
;
; - GP5 pin indicates if radio signal was lost (watchdog timed out). 
;
; - Changed PWM timing to improve top-end throttle control.
;
;
;
;

                PROCESSOR PIC12C509
                INCLUDE   <P12C509.inc>
                radix     dec

; ==========================================================================
; 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

; Erasable unit control - set to 1 of osccal value has been erased.  It will
; use a nominal half scale value.  Set to 0 for OTP parts.
NO_OSCCAL_VALUE EQU       0

; Bit definitions for the GPIO register and theTRIS register
StopBit         EQU       1               ; Turns on brake
RunBit          EQU       2               ; Turns on motor
ServoBit        EQU       0               ; Input servo pulse
BattOkBit       EQU       3               ; Input battery status
GreenLedBit     EQU       4               ; Hi if Watchdog timeout occured
RedLedBit       EQU       5               ; pulsed Hi for each bad servo pulse
TrisBits        EQU       H'FF' & ~((1 << StopBit) | (1 << RunBit) | (1 << GreenLedBit) | (1 << RedLedBit))

; 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 a 272mS timeout (approx. 13 servo pulses).
;
;
OptionBits      EQU       B'11011100'

; ==========================================================================
; Algorithm constants
PWM_Period      EQU       47              ; Number of 7-instr code cycles
PWM_Start       EQU       4               ; Lowest cycle count
BadPulseBit     EQU       7               ; 0x80 ; pulse width out of range
BrakeEnableBit  EQU       6               ; 0x40 ; brake pulse width range
ArmPulsesReqd   EQU       5               ; consecutive in-range servo pulses
BrakeDelay      EQU       25              ; delay before braking (*20mS) 
LoBattDelay     EQU       25              ; braking delay if battery low

;===========================================================================
; Macro to create offsets for file registers in RAM
;

ByteAddress     SET 8     ; user file registers start here

BYTE            MACRO     ByteName
ByteName        EQU       ByteAddress
ByteAddress     SET       ByteAddress+1
                ENDM

; ==========================================================================
; GP Register Definitions.  
;

                BYTE       Flags           ; various boolean flags            
                BYTE       PwmCount        ; counts down to zero during PWM output  
                BYTE       PwmWidth        ; Initial value to be loaded into PwmCount  
                BYTE       ServoCount      ; length of servo pulse 0-185 * 7uS (0.9-2.2mS)
                BYTE       HiCount         ; coarse servo pulse length (ServoCount/47)              
                BYTE       Command         ; PWM width (0-43) + BrakeEnableBit
                BYTE       ArmPulseCount   ; arming safety delay (counts down)
                BYTE       ServoPulse      ; current averaged servo pulse width
                BYTE       LastPulse       ; last averaged servo pulse width
                BYTE       BrakeDelayCount ; number of servo pulses until brake engages
                BYTE       StatCopy        ; copy of STATUS at startup

; Bits in Flags

BattLow         EQU       0
PriorServoBit   EQU       1

; Bits in Command

BrakeBit        EQU       6

; =========================================================================
; Helper for making strings of no-op instructions
NO_OP           MACRO     count
NO_OP_COUNT     SET       count
                WHILE     NO_OP_COUNT
                nop
NO_OP_COUNT     SET       NO_OP_COUNT - 1
                ENDW
                ENDM

; =========================================================================
; Macro to set the initial state of the motor fet.  W contains the count
; on exit. The code in this Macro uses 5 clocks.
;
DoRunBit        MACRO                          
                movf      PwmWidth, W         
                skpz                     
                bsf       GPIO, RunBit
                skpnz
                bcf       GPIO, RunBit
                ENDM

; =========================================================================
; Macro to keep up with the servo pulse duration
; The code in this Macro uses 2 clocks.
;
DoServoCount    MACRO                          
                btfsc     GPIO, ServoBit
                incf      ServoCount, F
                ENDM

; =========================================================================
; =========================================================================
; =========================================================================
;
; Now for some code
;
; Critical instruction timings are shown in brackets eg. ;(2) = 2 microseconds 
; (assuming the PIC is driven with a 4Mhz oscillator).
;
Coldstart:      ORG       0                

                IF        NO_OSCCAL_VALUE  ; Set this if EPROM was erased
                movlw     H'70'            ; Nominal value. (replace with the correct
                ENDIF                      ; value for your chip!)

; Capture the timer calibration value.
                movwf     OSCCAL          ; Get pre-programmed value.

                movfw     STATUS          ; remember state of status register  
                movwf     StatCopy             

                bcf       STATUS, PA0     ; Call/Goto destinations are in Page0

                               
; ==========================================================================
; The following sequence is from the book.  It moves the
; prescaler from tmr0 to the watchdog without accidental resets.
;
                clrwdt                    
                clrf      TMR0            
                movlw     OptionBits | 7  
                option                    
                clrwdt
                movlw     OptionBits
                option
                clrwdt

                clrf      GPIO
                movlw     TrisBits
                tris      GPIO                  ; All outputs are low

                btfss     StatCopy,NOT_TO
                bsf       GPIO,GreenLedBit      ; show if there was a watchdog timeout

; jump to high ROM area (low area needed for subroutines)  

                bsf       STATUS,PA0            ; (Page1)
                goto      Rearm                               


; ===========================================================
; The main PWM loop
;
; Two timing loops have to be simultaneously maintained:-
;
; 1/ Count servo pulse every 7 clocks. DoServoCount takes 2 
;    clocks, thus 5 clocks are available between each instance.
;  
; 2/ DoRunBit + 17 clocks + DoPwmPulse + 2 clocks etc. This creates 
;    the PWM cycle time of 329 clocks, which corresponds to a PWM
;    frequency of 3Khz.
;
ServoIsHigh:    bsf       Flags, PriorServoBit ;(1) servo pulse has started!
                DoServoCount                   ;(2) 3rd precycle
                btfsc     Flags, BattLow       ;(2) If battery was ever low, quit now
                goto      BattLoShutdown       ;    
                incf      HiCount,F            ;(1) HiCount = ServoCount/47 (+-47)
                skpnz                          ;(1) Limit HiCount to 255 maximum
                decf      HiCount,F            ;(1)
MainPwmLoop:    DoServoCount                   ;(2) 4th Precycle
                NO_OP     1                    ;(1)                
                call      DoPwmPulse           ;(4,2,5) DoServoCount inside DoPwmPulse 
                DoServoCount                   ;(2) 1st Precycle
MainPwmEntry:   DoRunBit                       ;(5)
                DoServoCount                   ;(2) 2nd precycle
                movwf     PwmCount             ;(1)
                btfsc     GPIO, ServoBit       ;(2) is servo pulse Hi ? 
                goto      ServoIsHigh          ;(+1)  
                btfsc     Flags, PriorServoBit ;(2)  Servo pulse now low, but was it high?
                goto      EndOfPulse           ;(+1) yes, then must be end of pulse
                clrf      ServoCount           ;(1) no, so reset servo pulse count
                DoServoCount                   ;(2) 3rd precycle
                btfsc     Flags, BattLow       ;(2) 
                goto      BattLoShutdown       ;    If battery was ever low, quit now
                goto      MainPwmLoop          ;(2) 
                


; ============================================================================
; End of Servo Pulse. We don't have to count servo pulses now, but we still
; have to maintain the PWM cycle timing! 
;
; DoRunBit  
; +17 cycles
; DoPwmCycle 
; +2 cycles
; DoRunBit
; etc.
;
; Starting 8 clocks after last DoRunBit 
;
EndOfPulse:     movfw     ServoCount            ;(1) save servo pulse count (it could 
                movwf     ServoPulse            ;(1) be corrupted by next DoPwmPulse)
                movlw     3                     ;(1)
                subwf     HiCount,W             ;(1) HiCount < 3 ?
                skpc                            ;(1)
                bsf       Flags,BadPulseBit     ;(1) servo pulse may be too short!
                skpc                            ;(1) 
                btfsc     ServoPulse,7          ;(1) servo count >= 128 ?
                bcf       Flags,BadPulseBit     ;(1) if yes, then pulse not too short  
                call      DoPwmPulse            ;<<< 
                NO_OP     2                     ;(2)
                DoRunBit                        ;>>>
                movwf     PwmCount              ;(1)
                movlw     8                     ;(1)
                subwf     HiCount,W             ;(1) HiCount >= 8 PWM cycles ?
                skpnc                           ;(1)
                bsf       Flags,BadPulseBit     ;(1) servo pulse is too long!
                clrf      HiCount               ;(1) reset HiCount
                movlw     128                   ;(1) min servo pulse width = 0.9mS
                subwf     ServoPulse,F          ;(1)                              
                movlw     186                   ;(1) max servo pulse width = 2.2mS
                subwf     ServoPulse,W          ;(1)  
                skpnc                           ;(1) pulse width in range ?  
                bsf       Flags,BadPulseBit     ;(1)                 
                btfsc     Flags,BadPulseBit     ;(2)  ignore bad servo pulse!
                goto      BadPulse              ;(+1)
                movfw     LastPulse             ;(1)  calculate average of count and
                addwf     ServoPulse,F          ;(1)  last count, to reduce jitter. 
                rrf       ServoPulse,F          ;(1)  average = (count + last) / 2
                call      DoPwmPulse            ;<<<  
                NO_OP     2                     ;(2)
                DoRunBit                        ;>>>  still using old PWM                                     movwf     PwmCount              ;(1)            
                movwf     PwmCount              ;(1)
                movf      ServoPulse, W         ;(1)       
                call      ServoLookup           ;(7)  (Page1) decode pulse length  
                bcf       STATUS, PA0           ;(1)  (page0)
                movwf     Command               ;(1) 
                andlw     63                    ;(1)  remove Brake bit 
                movwf     PwmWidth              ;(1)  set new PWM width
                btfsc     Command,BrakeBit      ;(2)  Braking?
                goto      Braking               ;(+1) 

NoBrake:        bcf       GPIO, StopBit         ;(1)  Turn off the brake 
                NO_OP     1                     ;(1)
                call      DoPwmPulse            ;<<<  still using old PWM 
                movlw     BrakeDelay            ;(1) 
                movwf     BrakeDelayCount       ;(1)  reset brake delay count
                DoRunBit                        ;>>>
                movwf     PwmCount              ;(1) using new PWM
                goto      GoodPulse             ;(2)
;
; Here the brake is to be turned on. Let the motor slow down first. PWM 
; timing isn't critical while braking, because the motor is always off.
;
Braking:        bcf       GPIO,RunBit           ;(1)  Stop the motor
                call      DoPwmPulse            ;<<<  For delay only 
                decfsz    BrakeDelayCount,F     ;(2)  Time to turn on the Brake?
                goto      GoodPulse             ;(+1) If not, wait for another servo pulse

BrakeOn:        incf      BrakeDelayCount,F     ;(1)  Don't let count go below zero!
                bsf       GPIO, StopBit         ;(1)  Turn on the brake
 
GoodPulse:      movf      ServoPulse,W          ;(1)  remember good servo pulse
                movwf     LastPulse             ;(1)  
                clrwdt                          ;(1)  reset the watchdog timer
NextPulse:      NO_OP     9                     ;(9)
                bcf       Flags, PriorServoBit  ;(1)  servo pulse is finished      
                clrf      ServoCount            ;(1)  reset servo pulse count
                call      DoPwmPulse            ;<<<
                goto      MainPwmEntry          ;(2)  DoRunBit is next

;
; Servo pulse had bad timing, so just keep the existing PWM going. 
; Don't remember the bad pulse. Don't reset the watchdog timer (so
; too many consecutive bad pulses will cause a restart!)
;
BadPulse:       bsf       GPIO,RedLedBit        ;(1) start glitch counter pulse 
                NO_OP     1                     ;(1)
                call      DoPwmPulse            ;<<< still using old PWM
                bcf       GPIO,RedLedBit        ;(1) end glitch counter pulse
                bcf       Flags,BadPulseBit     ;(1)
                DoRunBit                        ;>>> still using old PWM
                movwf     PwmCount              ;(1)
                NO_OP     3                     ;(3) 
                goto      NextPulse             ;(2)  


;
; Low battery - shutdown and wait for arming.
;

BattLoShutdown: bcf       GPIO, RunBit          ; Stop the motor

; Delay to let motor slow down before turning on brake

                movlw     LoBattDelay           ; set low battery brake delay count 
                movwf     BrakeDelayCount       
LoBattServoLo:  clrwdt                          
                btfss     GPIO, ServoBit        ; start of next pulse?
                goto      LoBattServoLo         ; no, wait for start of pulse
LoBattServoHi:  clrwdt
                btfsc     GPIO, ServoBit        ; end of pulse?
                goto      LoBattServoHi         ; no, wait for end of pulse
                decfsz    BrakeDelayCount,F     ; count down, brake delay done? 
                goto      LoBattServoLo         ; no, wait for another servo pulse

                bsf       GPIO, StopBit         ; brake on
                bsf       STATUS,PA0            ; page 1
                goto      Rearm                 




; ==========================================================================
; Servo pulse to command converter.  Returns with command in W.
; See speed400.inc for the table at 0x200 which is being referenced
; This routine takes 7 clocks to execute.
;
ServoLookup:    bsf       STATUS, PA0   ; Set CALL destination to Page1
                movwf     PCL           ; goto 0x200+W 





; ==========================================================================
; The main PWM pulse routine.  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 PWM_Count register reaches zero.
;
; NOTE: This is the LAST subroutine allowed, as it extends past the
;       256 byte boundary!  
;
DoPwmPulse:
PWM_LOOP_COUNT  SET       PWM_Period - PWM_Start  ; 43 times
                WHILE     PWM_LOOP_COUNT
                
                btfss     GPIO, BattOkBit       ;(2) check for low battery
                bsf       Flags, BattLow
                DoServoCount                    ;(2) measure servo pulse length
                decf      PwmCount,F            ;(1)
                skpnz                           ;(1) end of PWM hi pulse?
                bcf       GPIO, RunBit          ;(1) yes, then turn off motor
                
PWM_LOOP_COUNT  SET       PWM_LOOP_COUNT - 1
                ENDW
                
                retlw     0                     ;(2)
;
; NOTES: 1/ Do NOT add any subroutines here!
;        2/ Do NOT add more code than will fit in the first Page (512 words). 
                          


; ==========================================================================
; Servo command lookup table.  The index is 0-255, which is the lower 8 bits
; of the servo pulse width in 7 microsecond steps.  The output is:
;   PWM_width | ((invalid width)? 0x80 : 0) | ((Brake width range)? 0x40 : 0)
; The PWM width is in a range of 0 to 63, where 0 inhibits turning on the
; motor.
;
; NOTES: 1/ PA0 must be set to 1 before jumping to here.
;        2/ The table must start at 0x200. 
;
                ORG       H'200'          ; Low part of second page (PA0=1)
                INCLUDE   "spd400.inc"    ;

;------------------------------------------------------------------------------
;
; NOTE: - More code can be added here (256 words are available). However, no 
;         subroutines are allowed here!
;        
;       - PA0 must be set before jumping here, and cleared before jumping to
;         any location in Page0. It's OK to call subroutines in the lower half 
;         of Page0 from here, but make sure that PA0 is cleared first!
;
;------------------------------------------------------------------------------


; Note that we can get to here with the brake on if the battery got low.  
; If we got here from power up, the brake is off.  In all cases, the motor 
; is off.  Don't leave with a low battery
;
Rearm:          clrwdt
                btfss     GPIO, BattOkBit       ; Don't leave if battery is low
                goto      Rearm                         
;
; Hunt for a whole servo pulse before looking for arm pulses
;
RaLoop1:        btfss     GPIO, BattOkBit
                goto      Rearm                         
                btfss     GPIO, ServoBit
                goto      RaLoop1
RaLoop2:        btfss     GPIO, BattOkBit
                goto      Rearm                         
                btfsc     GPIO, ServoBit
                goto      RaLoop2

                movlw     ArmPulsesReqd
                movwf     ArmPulseCount

RaLoop3:        clrf      ServoCount
                clrwdt
;
; Measure length of the next servo pulse                
;
RaLoop4:        btfss     GPIO, BattOkBit
                goto      Rearm                         
                btfss     GPIO, ServoBit       ; wait until servo pulse is high       
                goto      RaLoop4             
RaLoop5:        incf      ServoCount, F        ;(1)
                skpnz                          ;(2)       
                goto      Rearm                ;    restart if > 255
                NO_OP     1                    ;(1) loop must be 7 clocks                
                btfsc     GPIO, ServoBit       ;(1) 
                goto      RaLoop5              ;(2) until servo pulse is low

; accept arming pulses that are within brake range

                clrwdt
                movlw     128                  ; min count = 0.9mS 
                subwf     ServoCount,F                
                movlw     43                   ; max count = 1.2mS
                subwf     ServoCount,W         ; is this a legal arming pulse?
                skpnc
                goto      RaLoop1              ; No - start over
                decfsz    ArmPulseCount,F      ; Do we have enough arm pulses?
                goto      RaLoop3              ; Not yet.
;
; Play a 2KHz 'beep' sound for 1/8 second, indicating successful arming.
; Sound can be heard from the motor, or (better) from a speaker connected 
; to GP4.  
;
Beep:           bcf       GPIO,StopBit         ; brake off                           
                clrf      PwmCount
beep1:          bsf       GPIO,RunBit          ; motor on for 28uS 
                NO_OP     13                   ; 
                bsf       GPIO,RedLedBit       ; start speaker pulse 
                NO_OP     13                   ; 
                bcf       GPIO,RunBit          ; motor off
                movlw     60
                movwf     PwmWidth
beep2:          clrwdt                     
                decfsz    PwmWidth,F           ; wait 0.24mS
                goto      beep2                 
                bcf       GPIO,RedLedBit       ; end speaker pulse
                movlw     60
                movwf     PwmWidth
beep3:          clrwdt                     
                decfsz    PwmWidth,F           ; wait 0.24mS
                goto      beep3                
                decfsz    PwmCount,F
                goto      beep1                ; do 256 cycles (1/4 second)
;
; sync to end of next servo pulse.
;
servo_hi:       btfss     GPIO,ServoBit        ; wait for start of pulse       
                goto      servo_hi             
servo_lo:       btfsc     GPIO,ServoBit        ; wait for end of pulse
                goto      servo_lo                              

Armed:          clrf      PwmWidth             ; Hold motor off
                clrf      ServoCount
                clrf      HiCount              
                clrf      LastPulse            
                clrf      Flags                ; Servo pulse is lo, battery is OK
                movlw     BrakeDelay           
                movwf     BrakeDelayCount      ; Precharge brake delay 

;
; Arming complete, jump to main operating loop. The motor is off, and 
; will stay off until the next servo pulse has been processed.
;
                bcf       STATUS,PA0           ; (Page0)
                goto      MainPwmLoop

                END

