Dick Ivers
- 5th April 2010, 20:55
This is another attempt to create a servo driver based on instant interrupts. This one almost works, but not quite. I may attempting the imposible; if so, could someone please tell me. I really don't know how to interpret assembly routines.
This servo driver is based on Darrel Taylor's DT_INTS-14/TimerTemplate which can be found on this Forum. It is easy to set up a 50 hz interrupt to start and repeat standard servo frames. In addition to running a servo I'm driving a brushless motor controller, which also runs on standard servo frames. Testing is done with real hardware, including a motor, controller, and servo.
The routine shown below works as long as I do not attempt to change the motor drive pulses from full on to full off. The motor actually starts and runs at full speed. However, in order to stop the motor it is necessary to apply full off pulses. When I attempt to do this the program somehow scans through and picks up the full off setting, and applies this to the whole sequence. The result is that the motor will not start.
Most of the code shown is just a copy of the TimerTemplate. I added the interrupt handler and the few lines in the main section.
'device is PIC 16F684
'set registers
'...done...
DEFINE OSC 8 '8 Mhz oscillator
'declare variables
'....done...
' initialize ports and variables
portc.1 = 0 'turn off servo
portc.4 = 0 'initialize servo pulse low
portc.5 = 0 'initialize throttle pulse low
throttlepos = 400 '2*(200) = full throttle
servopos = 300 '2*(150) = mid range of servo
MTtime = 100 '10.0 second motor run time
BKtime = 15 '1.5 second brake time
ticks = 0 '0.02 seconds per tick
tenths = 0 '1/10 seconds
seconds = 0 'seconds
sum = 0 'total time in tenths, e.g. 100 = 10.0 sec.
'arm the motor
'Phoenix-10 brushless motor controller requires throttle-off arming
'for 4 seconds
'....done...
INCLUDE "DT_INTS-14 for F684.bas" ; Base Interrupt System with wsave for F684
INCLUDE "ReEnterPBP.bas" ; Include if using PBP interrupts
ASM
INT_LIST macro ; IntSource, Label, Type, ResetFlag?
INT_Handler TMR1_INT, ReloadTMR1, ASM, no ; MUST be first
INT_Handler TMR1_INT, _T1handler, PBP, yes
endm
INT_CREATE ; Creates the interrupt processor
ENDASM
;--- Match the desired interrupt frequency -------------------
;--- See http://DarrelTaylor.com/DT_INTS-14/TimerTemplate.html for more Info.
@Freq = 50 ; 50 Hz Frequency of Interrupts
@Prescaler = 1 ; Timers Prescaler setting
T1CON = $00 ; $00 = Prescaler 1:1, TMR1 OFF
@ INT_ENABLE TMR1_INT ; enable Timer 1 interrupts
GOSUB StartTimer ; Start the Timer
portc.1 = 1 'turn on servo power
low Led 'turn on led
Main:
; ---- Your Main Program goes here ----
if sum => MTtime then 'motor fully on for 10.0 seconds
high led 'turn off led .. this signals end of motor run
goto newtime1 'try to reset throttle to full off
endif
newtime1: ' brake motor for 1.5 seconds
' throttlepos = 200 ' 2*(100) throttle full off; brake on. This is
'the problem line. Does not work when uncommented
if sum => (MTtime + BKtime) then 'motor off, braking done
low led 'turn on led; signals end of braking
goto DT
endif
GOTO Main
'-------------------------------------------
DT:
@ INT_DISABLE TMR1_INT ; disable Timer 1 interrupts
portc.1 = 0 'turn off servo
end 'sleep until next power on cycle
;____This routine is Called on each TMR1 Interrupt____________________________
T1handler: 'interrupts are every 20 ms. These interrupts start each servo frame
'and also clock elapsed time.
pulsout portc.5,throttlepos 'send pulse to motor speed controller
pulsout portc.4,servopos 'send pulse to servo (pulse range is
Ticks = Ticks + 1 '1 ms to 2 ms for both pulses)
if Ticks = 5 then
Ticks = Ticks - 5
Tenths = Tenths + 1
if tenths = 10 then
Seconds = Seconds + 1
Tenths = 0
endif
sum = 10*Seconds + Tenths 'calculate time in tenths of seconds
endif
; ---- ---------------------------------- ----
@ INT_RETURN
;---[TMR1 reload - interrupt handler]-----------------------------------------
ASM ; Calculate Timer Reload Constant
ReloadInst = 8 ; # of Intructions used to reload timer
if ((Prescaler == 1)||(Prescaler == 2)||(Prescaler == 4)||(Prescaler == 8))
MaxCount = 65536 + (ReloadInst / Prescaler)
TimerReload = MaxCount - (OSC*1000000/4/Prescaler/Freq)
if ((TimerReload < 0) || (TimerReload > (65535-ReloadInst)))
error Invalid Timer Values - check "OSC", "Freq" and "Prescaler"
endif
else
error Invalid Prescaler
endif
ENDASM
@Timer1 = TMR1L ; map timer registers to a word variable
Timer1 VAR WORD EXT
TimerReload CON EXT ; Get the External Constant
TMR1ON VAR T1CON.0 ; Alias the Timers ON/OFF bit
;---Reload Timer1------
ASM
ReloadTMR1
MOVE?CT 0, T1CON, TMR1ON ; 1 stop timer
MOVLW LOW(TimerReload) ; 1 Add TimerReload to the
ADDWF TMR1L,F ; 1 value in Timer1
BTFSC STATUS,C ; 1/2
INCF TMR1H,F ; 1
MOVLW HIGH(TimerReload) ; 1
ADDWF TMR1H,F ; 1
MOVE?CT 1, T1CON, TMR1ON ; 1 start timer
INT_RETURN
ENDASM
;---Start/Stop controls -----
StartTimer:
Timer1 = TimerReload ; Load Timer
TMR1ON = 1 ; start timer
RETURN
StopTimer:
TMR1ON = 0 ; stop timer
RETURN
This servo driver is based on Darrel Taylor's DT_INTS-14/TimerTemplate which can be found on this Forum. It is easy to set up a 50 hz interrupt to start and repeat standard servo frames. In addition to running a servo I'm driving a brushless motor controller, which also runs on standard servo frames. Testing is done with real hardware, including a motor, controller, and servo.
The routine shown below works as long as I do not attempt to change the motor drive pulses from full on to full off. The motor actually starts and runs at full speed. However, in order to stop the motor it is necessary to apply full off pulses. When I attempt to do this the program somehow scans through and picks up the full off setting, and applies this to the whole sequence. The result is that the motor will not start.
Most of the code shown is just a copy of the TimerTemplate. I added the interrupt handler and the few lines in the main section.
'device is PIC 16F684
'set registers
'...done...
DEFINE OSC 8 '8 Mhz oscillator
'declare variables
'....done...
' initialize ports and variables
portc.1 = 0 'turn off servo
portc.4 = 0 'initialize servo pulse low
portc.5 = 0 'initialize throttle pulse low
throttlepos = 400 '2*(200) = full throttle
servopos = 300 '2*(150) = mid range of servo
MTtime = 100 '10.0 second motor run time
BKtime = 15 '1.5 second brake time
ticks = 0 '0.02 seconds per tick
tenths = 0 '1/10 seconds
seconds = 0 'seconds
sum = 0 'total time in tenths, e.g. 100 = 10.0 sec.
'arm the motor
'Phoenix-10 brushless motor controller requires throttle-off arming
'for 4 seconds
'....done...
INCLUDE "DT_INTS-14 for F684.bas" ; Base Interrupt System with wsave for F684
INCLUDE "ReEnterPBP.bas" ; Include if using PBP interrupts
ASM
INT_LIST macro ; IntSource, Label, Type, ResetFlag?
INT_Handler TMR1_INT, ReloadTMR1, ASM, no ; MUST be first
INT_Handler TMR1_INT, _T1handler, PBP, yes
endm
INT_CREATE ; Creates the interrupt processor
ENDASM
;--- Match the desired interrupt frequency -------------------
;--- See http://DarrelTaylor.com/DT_INTS-14/TimerTemplate.html for more Info.
@Freq = 50 ; 50 Hz Frequency of Interrupts
@Prescaler = 1 ; Timers Prescaler setting
T1CON = $00 ; $00 = Prescaler 1:1, TMR1 OFF
@ INT_ENABLE TMR1_INT ; enable Timer 1 interrupts
GOSUB StartTimer ; Start the Timer
portc.1 = 1 'turn on servo power
low Led 'turn on led
Main:
; ---- Your Main Program goes here ----
if sum => MTtime then 'motor fully on for 10.0 seconds
high led 'turn off led .. this signals end of motor run
goto newtime1 'try to reset throttle to full off
endif
newtime1: ' brake motor for 1.5 seconds
' throttlepos = 200 ' 2*(100) throttle full off; brake on. This is
'the problem line. Does not work when uncommented
if sum => (MTtime + BKtime) then 'motor off, braking done
low led 'turn on led; signals end of braking
goto DT
endif
GOTO Main
'-------------------------------------------
DT:
@ INT_DISABLE TMR1_INT ; disable Timer 1 interrupts
portc.1 = 0 'turn off servo
end 'sleep until next power on cycle
;____This routine is Called on each TMR1 Interrupt____________________________
T1handler: 'interrupts are every 20 ms. These interrupts start each servo frame
'and also clock elapsed time.
pulsout portc.5,throttlepos 'send pulse to motor speed controller
pulsout portc.4,servopos 'send pulse to servo (pulse range is
Ticks = Ticks + 1 '1 ms to 2 ms for both pulses)
if Ticks = 5 then
Ticks = Ticks - 5
Tenths = Tenths + 1
if tenths = 10 then
Seconds = Seconds + 1
Tenths = 0
endif
sum = 10*Seconds + Tenths 'calculate time in tenths of seconds
endif
; ---- ---------------------------------- ----
@ INT_RETURN
;---[TMR1 reload - interrupt handler]-----------------------------------------
ASM ; Calculate Timer Reload Constant
ReloadInst = 8 ; # of Intructions used to reload timer
if ((Prescaler == 1)||(Prescaler == 2)||(Prescaler == 4)||(Prescaler == 8))
MaxCount = 65536 + (ReloadInst / Prescaler)
TimerReload = MaxCount - (OSC*1000000/4/Prescaler/Freq)
if ((TimerReload < 0) || (TimerReload > (65535-ReloadInst)))
error Invalid Timer Values - check "OSC", "Freq" and "Prescaler"
endif
else
error Invalid Prescaler
endif
ENDASM
@Timer1 = TMR1L ; map timer registers to a word variable
Timer1 VAR WORD EXT
TimerReload CON EXT ; Get the External Constant
TMR1ON VAR T1CON.0 ; Alias the Timers ON/OFF bit
;---Reload Timer1------
ASM
ReloadTMR1
MOVE?CT 0, T1CON, TMR1ON ; 1 stop timer
MOVLW LOW(TimerReload) ; 1 Add TimerReload to the
ADDWF TMR1L,F ; 1 value in Timer1
BTFSC STATUS,C ; 1/2
INCF TMR1H,F ; 1
MOVLW HIGH(TimerReload) ; 1
ADDWF TMR1H,F ; 1
MOVE?CT 1, T1CON, TMR1ON ; 1 start timer
INT_RETURN
ENDASM
;---Start/Stop controls -----
StartTimer:
Timer1 = TimerReload ; Load Timer
TMR1ON = 1 ; start timer
RETURN
StopTimer:
TMR1ON = 0 ; stop timer
RETURN