;**********************************************************************
; *
;**********************************************************************
; *
; Filename: spdcnt2.asm *
; Date: 08/20/2001 *
; File Version: v1.0 *
; *
; Author: Joseph Julicher *
; Company: Microchip Technology *
; *
; *
;**********************************************************************
; *
; Files required: p12c509a.inc *
; *
; *
; *
;**********************************************************************
; *
; Notes: Interprets the 50hz PWM signals from an RC receiver into *
; 1.7khz pwm signals for driving a motor. *
; Step 1 is to wait for 100 consequtive low pulses to arm *
; The Run loop waits for a complete high pulse then *
; calculates the pwm period. The pwm is kept up to date *
; during all phases of measurment. *
; *
;**********************************************************************
list p=12c509a ; list directive to define processor
#include <p12c509a.inc> ; processor specific variable definitions
__CONFIG _CP_OFF & _WDT_ON & _MCLRE_OFF & _IntRC_OSC
;***** VARIABLE DEFINITIONS
pwm_counter EQU 0x07
pwm_value EQU 0x08
pwm_average EQU 0x09
pwm EQU 0x0A
timer EQU 0x0B
arm_counter EQU 0x0C
state EQU 0x0D
filter EQU 0x0F
temp_carry EQU 0x10
#define minimum_entry .96 ; these values are between 1.15msec and 1.85msec pulse
#define minimum_exit .99 ; they only allow 58 discrete throttle positions including
#define maximum_entry .154 ; 0 & 100%. The missing 6 pulses are at the high end.
#define maximum_exit .150
#define servo GPIO,4
#define output GPIO,0
#define pwm_reload .64
#define max_pwm .65
#define filter_reload .35
; states
#define normal 1
#define maximum 2
#define minimum 4
#define state_normal state,0
#define state_maximum state,1
#define state_minimum state,2
; macro to perform 1 cycle of PWM
do_pwm macro
movlw pwm_reload ; preload the reload value
decfsz pwm_counter,f ; decrement the counter value.
movf pwm_counter,w ; if the counter is not 0, load W with counter
movwf pwm_counter ; store w in the counter.
; this does an auto reload timer...
subwf pwm,W ; pwm - counter sets the borrow flag
rlf GPIO,f ; a left rotate places the borrow flag in GPIO
endm
delay10 macro
goto $+1
goto $+1
goto $+1
goto $+1
goto $+1
endm
;**********************************************************************
ORG 0x3FF ; processor reset vector
; movlw 0x80 ; timer scale factor
; Internal RC calibration value is placed at location 0x3FF by Microchip
; as a movlw k, where the k is a literal value.
ORG 0x000 ; coding begins here
movwf OSCCAL ; update register with factory cal value
; remaining code goes here
; setup
movlw 0x0A ; configure the watchdog timer for
OPTION
bcf output ; make sure the motor is OFF
movlw b'111110' ; set GPIO 0,1,2 as output, the rest are inputs
tris GPIO
movlw normal
movwf state
clrf pwm
clrf pwm_counter
clrf pwm_value
movlw filter_reload
movwf filter
; arm by waiting for the high time to go to minimum
restart
movlw .25 ; 1/2 second of arming pulses
btfss STATUS,NOT_TO ; if the TO bit is clear, the dog woke up and bit us
movlw .10 ; 1/5 second of arming pulses if the dog woke up
; this will force the pilot to throttle down and reapply power
movwf arm_counter
start
clrf timer
btfss servo ; wait here for the servo to go high.
goto $-1
loop_arm_high
btfss servo
goto loop_arm_low
incf timer,f
clrwdt ; wait a bit so this loop is 12usec long
goto $+1
goto $+1
goto $+1
goto loop_arm_high
loop_arm_low
; check to see if timer is <= minimum_entry
movlw minimum_entry
subwf timer,w ;
btfsc STATUS,C ; if timer < minimum_value, C is clear
goto restart ; the pulses are not minimum yet.
decfsz arm_counter,f ; start counting down to arming.
goto start ; get the next pulse
; fall through if we are armed
; chirp the motor to indicate armed
bsf output ; on
delay10
bcf output ; off
delay10
bsf output ; on
delay10
bcf output ; off
delay10
bsf output ; on
delay10
bcf output ; off
; Time the pulse
loop_back
clrwdt ; knock out the dog on the rising edge
; do not knock out the dog during the high pulse
do_pwm
clrf TMR0
nop
nop
loop_high
do_pwm ; run the pwm macro
btfsc servo ; loop while the servo is high
goto loop_high ; go do it again.
; falling edge, time to do the math for the next PWM value
; first update the PWM
nop
do_pwm ; do a pwm cycle
movf TMR0,W
movwf timer
; what state are we in?
btfsc state_normal
goto normal_state
nop
do_pwm
btfsc state_maximum
goto maximum_state
nop
do_pwm
btfsc state_minimum
goto minimum_state
nop
do_pwm
; we are not in hysterisis mode so look for the minimum
normal_state
do_pwm
movlw minimum_entry
subwf timer,w ; w = timer - minimum_entry.
rrf temp_carry ; save the carry flag
do_pwm
rlf temp_carry ; restore the carry flag
btfsc STATUS,C ; if timer < minimum_entry, C is clear
goto lp_chng_1 ;
nop
do_pwm
clrf pwm_value ; timer too small. PWM is at minimum
nop ; we are in minimum_state
nop
do_pwm
movlw minimum
movwf state ; the next pass will do the minimum state
nop
do_pwm
nop
goto lp_average
lp_chng_1
do_pwm
movlw maximum_entry
subwf timer,w ; w = timer - maximum_entry
rrf temp_carry ; save the carry flag
do_pwm
rlf temp_carry ; restore the carry flag
btfss STATUS,C ; if timer > maximum_entry, C is set
goto lp_chng_2
nop
do_pwm
movlw max_pwm ; setup the pwm for maximum value
movwf pwm_value
nop
do_pwm
movlw maximum
movwf state ; the next pass will do the maximum state
nop
do_pwm
nop
goto lp_average
lp_chng_2 ; trivial cases checked, time to scale..
do_pwm
movlw minimum_entry
subwf timer,w
movwf pwm_value
lp_average ; rolling /2 average
do_pwm
; if new value is less than old value...
; set to new value and reset the filter
movf pwm,w
subwf pwm_value,w ; w = pwm_value - pwm.
rlf temp_carry ; save the carry flag
do_pwm
rrf temp_carry ; restore the carry flag
btfsc STATUS,C ; if pwm_value < pwm, C is clear
goto lp_greater ; if pwm_value !< pwm, check for >
nop
do_pwm
movf pwm_value,w ; simple filter & update the output
addwf pwm,f
nop
do_pwm
bcf STATUS,C
rrf pwm,f
nop
do_pwm
goto loop_low_entry
; if new value is equal to old value...
; reset the filter
lp_equal
do_pwm
movf pwm,w
subwf pwm_value,w ; w = pwm_value - pwm.
movwf temp_carry ; use temp carry to store the result.
do_pwm
movf temp_carry,w ; restore the result. This will restore Z
btfss STATUS,Z ; if pwm_value = pwm, Z is set
goto lp_greater
nop
do_pwm
goto loop_low_entry
; if new value is greater than old value...
; increment the decrement the pre_filter...
; if the pre_filter is 0, increment the filter...
; if the filter has an overflow, increment the pwm
lp_greater
do_pwm
incf pwm,f ; increment the pwm towards the pwm_value
incf pwm,f
incf pwm,f
do_pwm
goto loop_low_entry
minimum_state
do_pwm
movlw minimum_exit
subwf timer,w ; w = minimum_exit - timer
rlf temp_carry ; save the carry flag
do_pwm
rrf temp_carry ; restore the carry flag
btfss STATUS,C ; if timer > minimum_exit, c is set so goto normal state
goto stay_minimum
movlw normal
movwf state
nop
stay_minimum
do_pwm
clrf pwm_value
goto lp_average
maximum_state
do_pwm
movlw maximum_exit
subwf timer,w ; w = maximum_exit - timer
rlf temp_carry ; save carry flag
do_pwm
rrf temp_carry ; restore carry flag
btfsc STATUS,C ; if timer < maximum_exit, c is clear so goto normal state
goto stay_maximum
movlw normal
movwf state
nop
stay_maximum
do_pwm
movlw max_pwm
movwf pwm_value
nop
do_pwm
nop
goto lp_average
loop_low_entry
clrwdt ; knock out the dog after the math
; do not knock out the dog during the low pulse...
loop_low
do_pwm
btfss servo ; loop until high
goto loop_low
nop
do_pwm
goto loop_back ; it is high, so go do the high loop
END |
/1 
发表于 2006-11-29 09:25
收藏
发表于 2006-11-30 12:49
