list E=0,C=132 __config _XT_OSC & _WDT_ON & _CP_OFF subtitl "SPD - PWM Speed Controller" page ; Copyright 1996-2001 Andy Shaw ; ; This file is part of spd. ; ; spd is free software; you can redistribute it and/or modify ; it under the terms of the GNU General Public License as published by ; the Free Software Foundation; either version 2 of the License, or ; (at your option) any later version. ; ; Neptune is distributed in the hope that it will be useful, ; but WITHOUT ANY WARRANTY; without even the implied warranty of ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ; GNU General Public License for more details. ; ; You should have received a copy of the GNU General Public License ; along with spd; if not, write to the Free Software ; Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA ; ; Contact ; Details of the Neptune sub control system can be found at: ; www.gloomy-place.com ; for further information to report fixes etc. please contact Andy Shaw ; andy@gloomy-place.com ; page ; Module: spd.asm ; Author: Andy Shaw ; Date: 28/12/97 ; ; Copyright (C) Andy Shaw 1996, 1997 All Rights Reserved. ; ; Description ; This module provides the firmware for the spd PWM speed control hardware. ; It reads a standard RC pulse train and converts this into a PWM output ; stream plus a direction control signal (used to operate a relay). Special ; features include... ; ; Failsafe Automatically stops on loss of signal ; SafeStart Waits for stick to be in neutral before starting ; No Chatter Dual switching points to reduce relay chatter ; SoftStart Provides power feed in a ramped manner ; SoftCurve Table driven power curve ; ; The current implementation operates at a frequency of 2KHz (64*8uS steps) with a minimum ; pulse width of 16uS, the step granularity is 8uS. ; ; page ; ; Input assignment ; PORTB bit 4 RX input ; ; Output assignment ; PortA bit 1 PWM Pulse ; PortA bit 0 Direction ; PortA bit 2 Status Indicator ; page include "common\as16c7x.inc" page ; Configuration #define TASKS 1 #define RXINPUT 1 ; ; include "common\asgen.inc" ; Variables CBLOCK ; Start Ram variable allocation GenStat ; General status PwmRTim ; PWM Reverse relay timer PwmCur ; Current PWM value (signed +-32) PwmTim ; PWM Timer PwmP0 ; Computed phase 0 PWM pulse length PwmIO ; PWM Output bits PwmBase ; PWM base time ThVal ; Smoothed version of Throttle input ThPrev ; Previous value ThCnt ; Number of consecutive matches DspVal ; Current value to be displayed DspTime ; Timer used for display ENDC ; End of var allocation page ; Constants DSPTICK equ ((.1000/6)/RXCYCLE) PWMCYC equ .64 ; Total length of PWM cycle PWMLONG equ (PWMCYC/2) ; Mask to test for long portion of cycle PWMSTOP equ (.250)/(.10) ; Time to wait when passing through zero PWMREV equ (.2000)/(.10) ; Time to leave reverse relay turned on for THMATCH equ 1 ; Number of consecutive values required to be acceptable page ;Port Usage PACON1 equ B'00000011' ; RA0, RA1, RA2, RA3 Digital PADIR equ B'11100000' ; All output PACON0 equ B'00000000' ; Disable ADC PBDIR equ B'00010000' ; All output except for 4 PWM equ 1 ; PWM output pin DIR equ 0 ; Direction control DISP equ 2 ; Status LED RX0 equ 4 ; RX input RXCMASK equ 0x10 page ;PWM special values PWMOFF equ 0 ; PWM Off PWMMAX equ .255 ; PWM Full power PWMDIR equ .1 ; Set PWM direction ; General status bits GSFULL equ 0 ; Full power GSREV equ 1 ; Move backwards GSSTART equ 2 ; Waiting for start sequence GSNOSIG equ 3 ; Signal lost ; Display values (i.e. number of times to flash the LED) DSNORM equ 1 ; normal DSSTART equ 2 ; Waiting for start DSREV equ 3 ; Reverse relay on DSFULL equ 4 ; Full power DSNOSIG equ 6 ; Signal Lost page ; ; Normal Entry point ; org 0 goto Start ; ; Interrupt Entry point ; org 4 InterruptVector goto CheckInterrupt nop nop nop nop nop DT "SPD1-RC Version XX (c) Andy Shaw 1999" CheckInterrupt push ; Always save registers etc btfsc INTCON, RBIF ; Is it a PORTB int ? goto ServicePORTB ; yes so go do it goto EndInterrupt ; No so all done EndInterrupt pop ; restore things retfie ; return and re-enable ints page ; ; The following table provides the power curve used by the speed controller. Call ; the function below with a value 0..32 and the required PWM value will be returned ; in W. Actual PWM values are 4..255. Special cases are 0 - Off and 1 which is used ; to indicate that the direction relay should be set but no power applied. ; The table is placed here to avoid page boundary problems. PWMLookup movwf Temp movlw HIGH PWMTable movwf PCLATH movf Temp, W call Abs ; get rid of any signed stuffed addwf PCL, F ; go get actual value PWMTable retlw PWMOFF ; Off (deadband) retlw PWMOFF retlw PWMDIR ; Off but relay on retlw PWMDIR retlw PWMDIR ; "" retlw PWMDIR retlw PWMDIR ; "" retlw PWMDIR retlw .2 ; Start of power curve retlw .3 retlw .4 retlw .5 retlw .6 retlw .7 retlw .8 retlw .9 retlw .10 retlw .11 retlw .12 retlw .13 retlw .15 retlw .16 retlw .18 retlw .19 retlw .21 retlw .22 retlw .24 retlw .26 retlw .28 retlw .30 retlw .32 retlw .34 retlw .36 retlw .38 retlw .40 retlw .43 retlw .45 retlw .48 retlw .50 retlw .52 retlw .54 retlw .56 retlw .58 retlw .60 retlw .62 retlw PWMMAX ; Full power retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX retlw PWMMAX page ; ; Task definitions ; These are the tasks that make up the controller. They are all scheduled ; by timer events. They are defined in priority order StartupTasks Tasks ; Begin startup task Task0 ; default task clrwdt ; make sure we don't timeout nop ; Odd ball instructions reduces RF int. nop ; On 40MHz nop ; Lord knows why! call RxProcess ; Processing incomming RX pulses call TaskTick ; Check the default task timer EndT0 Task TE20MS ; 20ms Tasks call RxTimeout ; make sure we are getting a signal call DisplayOutput ; Say what is going on call SystemArm btfss GenStat, GSSTART; Have we armed correctly yet? ExitTs ; yes so exit this task set EndT EndTs MainTasks Tasks ; Begin set of tasks Task0 ; Define default task clrwdt ; make sure watch dog timer is clear call PWMRun ; Go run the PWM pulse generator call RxProcess ; Process incomming RX pulses call TaskTick ; check the default system timer EndT0 Task TE10MS ; 10ms Tasks call PWMGenerateMove ; Decide what we should do next call PWMCompute ; and go and do it EndT Task TE20MS ; 20ms Tasks call RxTimeout ; make sure we are getting a signal call DisplayOutput ; Say what is going on EndT EndTs ; End of task definitions page include "common\asgen.asm" page ; ; Main entry point chack to see if we have had a WDT reset if we have enter ; a special routine to display trace information on the LED. Note in release ; builds of the firmware we do not do this and treat WDT reset as a normal start. ; Start goto NormalStart btfss STATUS, NOT_PD goto NormalStart btfss STATUS, NOT_TO goto TimeOutReset goto NormalStart NormalStart call SystemInit ; Initialise basic system call TasksInit ; Setup the tasking system call PORTAInit ; Setup PORTA call PORTBInit ; setup the I/O ports on B. call TMR0Init ; Initialise the TMR0 call RxInit ; Initialise RX stuff call ThInit ; Initialise throttle call PWMInit ; Initialise PWM stuff call DisplayInit ; Initiliase the display system call SystemStart ; and finally get ready to go goto StartupTasks ; Go Start things going ; ; Entry Point for Watch Dog Timer Reset ; TimeOutReset goto TimeOutReset page NoiseFilter DeadN 1 page ; ; ; Functions ; ; Setup PORTA for two analog input chans and one digital output ; PORTAInit clrf PORTA ; Clear things before we change to output bsf STATUS, RP0 ; select bank 1 ifdef ADCON1 movlw PACON1 ; get type of pins movwf ADCON1 & 0x7f ; set them endif movlw PADIR ; get direction movwf TRISA & 0x7f ; Set state bcf STATUS, RP0 ; select bank 0 clrf PORTA ; make sure all bits are off ifdef ADCON0 movlw PACON0 ; get A/D control movwf ADCON0 ; set bits endif return ; ; Setup PORTB for various input/output settings ; PORTBInit clrf PORTB ; Clear things before we switch to output bsf STATUS, RP0 ; select bank 1 movlw PBDIR ; get direction movwf TRISB & 0x7f bcf STATUS, RP0 ; select bank 0 clrf PORTB movf PORTB, W bcf INTCON, RBIF ; Clear port B interrupt flag bsf INTCON, RBIE ; Enable Portb int on change return ; ; Initialise the basic system. Clear watchdog timer. Clear status and get ready to run. ; SystemInit clrf GenStat ; Set intial state return ; ; Start the overall system. Clear any system status and enable ; interrupts. ; SystemStart bsf GenStat, GSSTART; put us into startup mode bsf GenStat, GSNOSIG; Say no signal for now retfie ; all done enable ints ; ; SystemArm ; To prevent possible injury when the system is turned on we wait for the control stick to be ; placed in neutral. This function checks for a valid signal and if the signal is neutral it clears ; the GSSTART state to indicate that the system is ready to run. ; SystemArm btfsc GenStat, GSNOSIG; Wait until have a signal return call ThGetValue ; Get current control value call PWMLookup ; Use table to give us a deadband andlw 0xfe ; are we in it btfss STATUS, Z ; if so Z is set return ; no so keep waiting bcf GenStat, GSSTART; Indicate that we are now running return ; ; Throttle processing functions ; ; The following routines provide additional processing of the raw RX input values to ; provide a smoothed throttle value for the rest of the system. We make use of a simple ; smoothing system such that new data is onlt accepted after n consecutive identical ; values and small amounts of jitter are ignored. ; ThInit clrf ThVal ; set value to nutral clrf ThPrev ; make previous value same as current movlw THMATCH ; Setup match count movwf ThCnt ; ready to go return ; ; Function called when a new raw Rx value is available. Performs simple smoothing ; and filtering before making values available to the rest of the system. Called ; with new raw Rx value in W ; Rx0NewValue ; Rx Chan 0 Event entry point ThNewValue movwf Temp2 ; Save new value for later subwf ThPrev, W ; compare with previous value call NoiseFilter ; Ignore small changes andlw 0xff ; Set status bits btfss STATUS, Z ; are they the same? goto NewValue ; No decfsz ThCnt, F ; have we had enough of them yet? return ; no so just count as good movf ThVal, W ; get current value subwf Temp2, W ; Get delta from new call NoiseFilter ; ignore noise addwf ThVal, F ; and change it incf ThCnt, F ; set back up for next time return ; all done NewValue movlw THMATCH ; Got a different value reset match count movwf ThCnt ; ready for next time movf Temp2, W ; get latest value movwf ThPrev ; and move to previous return ; all done ; Helper function for throttle processing. Return the current value of the throttle. ; ThGetValue movf ThVal, W return RxSignalOK bcf GenStat, GSNOSIG; Say we are now seeing a signal return RxSignalLost bsf GenStat, GSNOSIG; Indicate signal lost return ; ; PWM Functions ; The PWM pulse series is software generated. The main function RunPWM is called to generate ; a complete PWM cycle (off and on). It geneates the shorter part of the cycle (either on or ; off) inline (this allows very short pulses. The longer part of the cycle is actually still ; running when the RunPWM function returns, it is completed on the next call. The TMR0 value ; is used to determine the length of this longer phase. The other two routines: GenerateMove ; and ComputePWM generate the target values for the PWM based on user input and pre-compute ; phase 0 and 1 times etc. ; ; The actual length of the phase is defined by a lookup table (PWMLookup) this table is ; also used to control switching of the direction control relay ahead of time by use ; of special values within the table. ; ; ; Setup the software PWM generator. This consits of an IO word (PWMIO) two ; phase lengths (PWMP0, PwmP1) and a base time (PwmBase. The phase lengths indicate ; the value to set the timer to for this phase. ; PWMInit clrf PwmIO ; make sure all is clear movlw PWMCYC/2 ; Load a half cycle movwf PwmP0 ; Set phase 0 clrf PwmBase ; Initialise timer base clrf PwmCur ; and clear current state movlw .20 ; Set a short settling time movwf PwmTim ; before we start clrf PwmRTim ; disable relay timeout return ; all done ; ; Compute PWM target values. This function performs the PWM 'move generation' function ; it looks at the current value and the desired target value and from this computes what ; the new value should be. We take care to ensure that we allow time for the motor to ; slow before changing direction and also provide a "soft start" feature as the speed ; is increased or decreased. ; PWMGenerateMove btfss GenStat, GSNOSIG; Have we lost our signal goto CheckDelay clrf PwmCur ; We have no signal force stop return ; and return CheckDelay decfsz PwmTim, F ; Can we execute a new command yet return ; no so keep current value ; incf PwmTim, F ; Set standard timeout of 1 call ThGetValue ; Get current control value movwf Temp ; and save for later call PWMLookup ; see what we are aiming for andlw 0xfe ; is it in the deadband? btfsc STATUS, Z ; if so we get back zero clrf Temp ; in deadband so target is zero ; movf PwmCur, F ; Are we currently at zero btfsc STATUS, Z ; if so then we just need a normal move goto StandardMove ; so go do it movf Temp, W ; is the target 0 btfsc STATUS, Z ; goto AllStop ; yes it so so make sure we stop xorwf PwmCur, W ; do we have different sign bits? andlw 0x80 ; if we do then we must pass through zero btfss STATUS, Z ; in which case we must ensure we stop goto AllStop ; to allow relay to change ; StandardMove movlw 0x80 ; prepare for signed compare xorwf Temp, F ; sort out signs movf PwmCur, W ; get current setting xorlw 0x80 ; sort out sign subwf Temp, W ; compare this with new target btfsc STATUS, Z ; are we actually the same ? return ; yes so nothing more to do btfsc STATUS, C ; decide what direction we need to move incf PwmCur, F ; and move that way btfss STATUS, C ; is it the other way? decf PwmCur, F ; yes so move that way return ; new value now set ; AllStop movf PwmCur, W ; get current speed call Abs ; make absolute call Div2 addlw PWMSTOP ; use to make delay longer if going fast movwf PwmTim ; set the delay clrf PwmCur ; and force a stop return ; ; Helper function. Set the state of the direction control relay as required. We build ; in hysteresis to prevent relay chatter. But to limit power consumption we turn off the ; relay if the control is left in nutral for more than a few seconds. ; SetDirection andlw 0xff ; Is the output off ? btfsc STATUS, Z ; if so Z is set goto PowerOff ; yes so do special timeout processing btfss PwmCur, 7 ; Test sign bit of current setting goto TurnOff ; No sign bit so turn reversing relay off bsf PORTA, DIR ; bit set so turn on relay for reverse TurnOn movlw PWMREV ; get relay timeout value movwf PwmRTim ; and save it bsf GenStat, GSREV ; say we are in reverse return ; all done TurnOff bcf PORTA, DIR ; not set so clear direction bit clrf PwmRTim ; Turn off the relay timer bcf GenStat, GSREV return PowerOff movf PwmRTim, F ; Are we doing a timeout? btfsc STATUS, Z ; if not timer is zero return ; no ao all done decfsz PwmRTim, F ; yes so have we finished yet ? return ; no goto TurnOff ; timer has expired so turn relay off ; ; ComputePWM ; This function pre-computes the two phases of the PWM cycle. Phase 0 is always the longer ; part of the cycle. The result of ComputePWM is three pre-set values PwmP0 - the time for ; phase 0, ~PwmP0 the time for phase 1 and PwmIO which is the IO control word used to actually ; toggle the pulse between phases. Three cases are recognised: full on, full off and normal ; mode. For the normal case steps are taken to ensure that phase 1 is always the longest. This ; function is also responsible for setting up the direction and PWM output ports. ; PWMCompute movf PwmCur, W ; get target value call PWMLookup ; and get actual speed movwf Temp ; and save this call SetDirection ; Make sure relay is set in correct mode movf Temp, W ; get required speed andlw ~PWMDIR ; check for special cases btfsc STATUS, Z ; is it 0 - all off goto SetOff ; yes go do it addlw 2 ; now check for full power btfsc STATUS, Z ; was value 255 - full on goto SetOn ; yes go do it SetNormal bcf GenStat, GSFULL ; Turn off full power indicator clrf PwmIO ; make sure bits are all clear bsf PwmIO, PWM ; and indicate we need to toggle PWM decf Temp,F ; Adjust actual on time to allow for rounding movf Temp, W ; get desired speed andlw PWMLONG ; Is this the longer phase? btfsc STATUS, Z ; if so Z will not be set goto ShortOn ; Phase 1 is on cycle LongOn movf Temp, W ; get on cycle movwf PwmP0 ; Store as long cycle (phase 0) bsf PORTA, PWM ; make sure we are currently on return ShortOn movf Temp, W ; get on cycle sublw PWMCYC&0xff ; get complement movwf PwmP0 ; and store as long cycle (phase 0) bcf PORTA, PWM ; make sure we are currently off return SetOff movlw PWMCYC/2 ; set to half a full cycle movwf PwmP0 ; for phase 0 clrf PwmIO ; don't toggle output bcf PORTA, PWM ; Make sure output is off bcf GenStat, GSFULL ; Turn off full power return SetOn movlw PWMCYC/2 ; set to half a full cycle movwf PwmP0 ; and phase 0 clrf PwmIO ; don't toggle output bsf PORTA, PWM ; make sure output is on bsf GenStat, GSFULL ; Indicate full power mode return ; ; PWMRun ; Main PWM generation routine. This function generates the PWM cycle. Whyen called we are part ; way through the long part of the cycle and the output bits must be set accordingly. This function ; will wait for then of the long phase toggle the output and then wait for the end of the shorter ; phase. It will then toggle the output bit, setup for the long phase and return. Note that this ; means that this function must be called again within PWMCYC/2 or the long cycle will run too long. ; This function is very timing sensitive. It has several hand tuned loops which are padded to retain ; synchronization with the system timer. ; PWMRun ; Synchronize with TMR0 WaitZero btfss TMR0, 0 ; We need to sync with timer 0 goto WaitZero ; so wait for a zero value WaitOne btfsc TMR0, 0 ; Then sync on rising edge goto WaitOne ; then rest of code will be in sync nop ; And sync with the actual test nop ; ditto nop ; and again ; Wait for end of phase 0 movf TMR0, W movwf Temp2 WaitTarget0 ; nop ; Fillers to make sure loop is 8 nop ; instructions long movf PwmBase, W ; Get base subwf TMR0, W ; get current time clrf STATUS subwf PwmP0, W ; at target? btfsc STATUS, C ; if so C is clear goto WaitTarget0 ; Not ready yet addlw 1 btfss STATUS, Z nop movf PwmIO, W ; get port status operator xorwf PORTA, F ; and toggle output ; Now wait for end of phase 1 WaitTarget1 nop ; make sure loop is nop ; 8 instructions long movf PwmBase, W ; get base subwf TMR0, W ; get current time sublw PWMCYC ; at end of cycle? btfsc STATUS, C ; if so C is clear goto WaitTarget1 ; Not ready yet subwf PwmBase, F ; and move base forward movf PwmIO, W ; get port status operator xorwf PORTA, F ; and toggle output movlw PWMCYC addlw 2 addwf PwmBase, F return ; allow other tasks to run ; ; Status Indicator Display ; The system status is indicated by a flashing LED. Various numbers of flashes ; followed by a slightly longer gab are used to indicated a priority based status ; system ; ; Initialise the display system ready for startup ; DisplayInit movlw 1 ; Get intial value for counter and display movwf DspVal ; this will force getting of actual movwf DspTime ; and time on first call return ; ; Display current internal state by flashing LED. ; We double the actual value to be displayed and then display ; a one on odd values and a zero on even values ; DisplayOutput decfsz DspTime, F ; Decrement timer return ; not yet time so return decfsz DspVal, F ; decrement display value goto DisplayValue ; go display whatever call GetDisplay ; got to end so get new display value in W movwf DspVal ; and store it bcf STATUS, C ; make sure carry is clear rlf DspVal, F ; and double the value (see above) movlw DSPTICK*2 ; get long gap movwf DspTime ; and store it bcf PORTA, DISP ; clear the LED return ; all done DisplayValue bcf PORTA, DISP ; turn LED off btfsc DspVal, 0 ; are we doing a gap ? bsf PORTA, DISP ; no turn LED on movlw DSPTICK ; reload timer movwf DspTime ; ready for next tick return ; all done ; ; Return the value to be displayed by the status LED in the W register ; we test for the most important things first ; GetDisplay btfsc GenStat, GSNOSIG; Have we lost signal ? retlw DSNOSIG ; yes btfsc GenStat, GSSTART; Waiting for start? retlw DSSTART ; yes btfsc GenStat, GSFULL ; At full power retlw DSFULL ; yes btfsc GenStat, GSREV ; Reverse relay on? retlw DSREV ; yes retlw DSNORM ; say things are normal ; END