; ============================================================================= ; 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 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