Code:
'****************************************************************'* Name : PWPS_Servo.INC Pulse Width Position Servo *
'* Author : Darrel Taylor *
'* Date : 5/1/2003 *
'* Version : 1.0 *
'* Notes : *
'* : *
'****************************************************************
DEFINE INTHAND INT_CODE ' Tell PBP Where the code starts on an interrupt
;wsave VAR BYTE $20 SYSTEM '$20 Save location for the W register if in bank0
;wsave1 VAR BYTE $A0 SYSTEM ' Save location for the W register if in bank1
;wsave2 VAR BYTE $120 SYSTEM ' Save location for the W register if in bank2
;wsave3 VAR BYTE $1A0 SYSTEM ' Save location for the W register if in bank3
;ssave VAR BYTE Bank0 SYSTEM ' Save location for the STATUS register
;psave VAR BYTE Bank0 SYSTEM ' Save location for the PCLATH register
W1 var word ' Temporary variable
y var byte ' Temporary variable
PulseTicks1ms var word ' # of Ticks for 1ms
TicksPeruSx100 var word ' # of Timer Ticks to = 1 us * 100
uS50Hz var word ' # of Ticks for 50 Hz with prescaler
PicOSC var byte Bank0 ' OSC value, Usable in PBP
OffTimePrescaler var byte ' Timer1 Prescale value during OFF period
Position var word ' 0 - 900 0 = 1.05ms 900 = 1.95ms
T1CONoff var byte Bank0
TMR1_ON_TICKS var word ' # of Tmr ticks for On Time
TMR1_OFF_TICKS var word ' # of Tmr ticks for Off Time
TMR1_ON_VAL var word Bank0 ' # to load TMR1 for On Time
TMR1_OFF_VAL var word Bank0 ' # to load TMR1 for Off Time
DataFlags var byte Bank0
Valid var DataFlags.0 ' 1 if Freq is valid - Set by CalcPWPS:
PWPSenabled var DataFlags.1 ' shows if PWPS is running or not
PWPSstate var DataFlags.2 ' Current state of PWPS output high or low
GIE var INTCON.7
PEIE var INTCON.6
TMR1IE var PIE1.0
TMR1ON var T1CON.0
goto GetOsc
' ------------------------------------------------------------------------
asm
INT_CODE
;if (CODE_SIZE <= 2)
; movwf wsave ; copy W to wsave register
; swapf STATUS,W ; swap status reg to be saved into W
; clrf STATUS ; change to bank 0 regardless of current bank
; movwf ssave ; save status reg to a bank 0 register
; movf PCLATH,w ; move PCLATH reg to be saved into W reg
; movwf psave ;6 ; save PCLATH reg to a bank 0 register
;endif
btfss PIR1, TMR1IF ; is TMR1IF set? Timer1 Interrupt Flag
GOTO NoTimerInt ; No. Bypass timer load
btfss _Valid ; Is Freq valid?
GOTO NoPWPS ; No. Halt PWPS
btfss _PWPSenabled ; is Software PWM enabled?
GOTO NoPWPS ; No. Halt PWPS
; Yes, then Set output and reload Timer1
btfss _PWPSstate ; Is Output High?
GOTO TurnON ;9/15 ; No.
TurnOFF
bcf _PWPSpin ; Set PWPSpin Low
bcf _PWPSstate ;
MOVF _T1CONoff,W ; Load OFF Period Prescaler and Turn off timer
MOVWF T1CON ;
MOVF _TMR1_OFF_VAL,W ; 1
ADDWF TMR1L,F ; 1 reload timer with Off Period value
BTFSC STATUS,C ; 1/2
INCF TMR1H,F ; 1
MOVF _TMR1_OFF_VAL+1,W ; 1
ADDWF TMR1H,F ; 1
BSF T1CON,TMR1ON ; 1 Turn it back on
GOTO TimerDone ;13/28
TurnON
bsf _PWPSpin ; Set PWPSpin High
bsf _PWPSstate ;
CLRF T1CON ; Clear Prescaler and Turn off timer
MOVF _TMR1_ON_VAL,W ; 1
ADDWF TMR1L,F ; 1 reload timer with On Period value
BTFSC STATUS,C ; 1/2
INCF TMR1H,F ; 1
MOVF _TMR1_ON_VAL+1,W ; 1
ADDWF TMR1H,F ; 1
bsf T1CON,TMR1ON ; 1 Turn it back on
GOTO TimerDone
NoPWPS
bcf T1CON,TMR1ON ; Turn off timer
bcf _PWPSpin ; Idle PWPSpin Low
TimerDone
bcf PIR1, TMR1IF ; 1/29 ; Clear Timer1 Interrupt Flag
NoTimerInt
;Movf psave,w ; Restore the PCLATH reg
;Movwf PCLATH
;swapf ssave,w ; Restore the STATUS reg
;movwf STATUS
;swapf wsave,f
;swapf wsave,w ; 6/35 ; Restore W reg
Retfie ; Exit the interrupt routine
endasm
' ------------------------------------------------------------------------
StartPWPS: ' Set Position before calling
low PWPSpin ' Set PWPSpin to Output and idle Low
GIE = 1 ' Global Interrupt Enable
PEIE = 1 ' Peripheral Interrupt Enable
TMR1H = 255 ' Load TMR1 with 65535, First tick will cause
TMR1L = 255 ' an interrupt that will load TMR1_???_VAL
SetPWPS: ' Set Position before calling
Position = Position min 1200 ' Make sure we don't go to far
gosub CalcPWPS
if Valid = 1 then
PWPSenabled = 1
lookdown OffTimePrescaler,[1,2,4,8],y
lookup y,[0,1,2,3],y
T1CONoff = y << 4
TMR1_ON_VAL = 65535 - TMR1_ON_TICKS + 7 ' Value to load in Timer1 for ON period
TMR1_OFF_VAL = 65535 - TMR1_OFF_TICKS + 7 ' Value to load in Timer1 for OFF period
TMR1IE = 1 ; Enable Timer1 Interrupts
T1CON = 1 ; Turn Timer1 on (no prescaler)
endif
return
StopPWPS
TMR1IE = 0
TMR1ON = 0
low PWPSpin ' Idle output Low
PWPSstate = 0
PWPSenabled = 0
return
CalcPWPS: ' Set Position before calling 0-900
Valid = 1
y = 99
LookDown PicOSC,[4, 8, 10, 12, 16, 20, 24, 25, 32, 33, 40],y
if y = 99 then OSCnotFound
;LookUp2 y,[ 1050, 2100, 2625, 3150, 4200, 5250, 6300, 6562, 8400, 8660, 10500],PulseTicks1ms
LookUp2 y,[ 900, 1800, 2250, 2700, 3600, 4500, 5400, 5625, 7200, 7425, 9000],PulseTicks1ms
lookup2 y,[ 100, 200, 250, 300, 400, 500, 600, 625, 800, 825, 1000],TicksPeruSx100
lookup y,[ 1, 1, 1, 1, 2, 2, 2, 2, 4, 4, 4],OffTimePrescaler
Lookup2 y,[20000,40000,50000,60000,40000,50000,60000,62000,40000,41000,50000],uS50Hz
W1 = TicksPeruSx100 * Position
W1 = Div32 100
TMR1_ON_TICKS = W1 + PulseTicks1ms
TMR1_OFF_TICKS = uS50Hz - (TMR1_ON_TICKS / OffTimePrescaler)
return
OSCnotFound:
Valid = 0
return
GetOsc: ' Retreive defined OSC value on Reset
asm
ifdef OSC
MOVE?CB OSC, _PicOSC
else
MOVE?CB 4, _PicOSC
endif
endasm
Re: PWPS Servo Problem
Bookmarks