PDA

View Full Version : Interupt driven servo PWM output


comwarrior
- 15th August 2013, 00:10
18F46K22 PBP2.6C

Ok, wanting to command a servo... but, want to use interupts so that the pic can do other things and/or drive multiple servo's...

So, first of all, i started off doing it with PAUSEUS to make sure i get the timings right...



SERVO1 VAR LATD.0
SERVO1POS VAR BYTE

MAIN:
SERVO1POS = 128
FOR ALOOP = 1 to 250
SERVO1 = 1
PAUSEUS 988
PAUSEUS (SERVO1POS * 4)
SERVO1 = 0
PAUSEUS (20000 - (988 + (SERVO1POS * 4)))
NEXT ALOOP

SERVO1POS = 1

FOR ALOOP = 1 to 250
SERVO1 = 1
PAUSEUS 988
PAUSEUS (SERVO1POS * 4)
SERVO1 = 0
PAUSEUS (20000 - (988 + (SERVO1POS * 4)))
NEXT ALOOP

SERVO1POS = 255

FOR ALOOP = 1 to 250
SERVO1 = 1
PAUSEUS 988
PAUSEUS (SERVO1POS * 4)
SERVO1 = 0
PAUSEUS (20000 - (988 + (SERVO1POS * 4)))
NEXT ALOOP
GOTO MAIN


Simples... works perfectly...
So now i start doing the math and working out the preloads for timers with the help of an excell spreadsheet.



SERVO1 VAR LATD.0
SERVO1POS VAR BYTE
PRELOAD1 VAR WORD
PRELOAD2 VAR WORD
PRELOAD3 VAR WORD
SERVOCMDMODE VAR BYTE
SERVOPWMCOUNTER VAR WORD

PRELOAD1 = 63556 ' 990us '63556
TMR0_CON_PL1 CON %10000010 ' 1:8
PRELOAD2 = 65528 ' 4us '65528
TMR0_CON_PL2 CON %10000010 ' 1:8
PRELOAD3 = 29556 ' 17990us '29556
TMR0_CON_PL3 CON %10000010 ' 1:8

@ INT_ENABLE TMR0_INT
TMR0L = PRELOAD3.LOWBYTE
TMR0H = PRELOAD3.HIGHBYTE
T0CON = TMR0_CON_PL3
SERVOCMDMODE = 1

MAIN:
SERVO1POS = 0
PAUSE 5000
SERVO1POS = 128
PAUSE 5000
SERVO1POS = 255
PAUSE 5000
GOTO MAIN

SERVO_HANDLER:
IF SERVOCMDMODE = 2 Then
IF SERVOPWMCOUNTER < 256 THEN
T0CON = TMR0_CON_PL2
TMR0L = PRELOAD2.LOWBYTE
TMR0H = PRELOAD2.HIGHBYTE
IF SERVO1POS < SERVOPWMCOUNTER THEN
SERVO1 = 0
LATD.1 = 0
LATD.2 = 0
LATD.3 = 0
ELSE
SERVO1 = 1
LATD.1 = 0
LATD.2 = 1
LATD.3 = 0
ENDIF
SERVOPWMCOUNTER = SERVOPWMCOUNTER + 1
ELSE
T0CON = TMR0_CON_PL3
TMR0L = PRELOAD3.LOWBYTE
TMR0H = PRELOAD3.HIGHBYTE
SERVO1 = 0
LATD.1 = 0
LATD.2 = 0
LATD.3 = 1
SERVOCMDMODE = 1
ENDIF
@ INT_RETURN
ENDIF

IF SERVOCMDMODE = 1 Then
T0CON = TMR0_CON_PL1
TMR0L = PRELOAD1.LOWBYTE
TMR0H = PRELOAD1.HIGHBYTE
SERVOCMDMODE = 2
SERVO1 = 1
LATD.1 = 1
LATD.2 = 0
LATD.3 = 0
SERVOPWMCOUNTER = 1
@ INT_RETURN
ENDIF

@ INT_RETURN


And thats where i now have a problem and i don't understand why.
For debug purposes i added the three LATD outputs so that i could find where i was having issues.

First off, the frequency is 46.75... ok, i'm sure it can still handle it, and i can adjust this later.
Secondly, the duty cycle goes from 84.0% to 93.9% to 99.6%...
It should be 5%, 7.5% and 10%...
Debug output shows LATD1 at 84.4%...

LATD1 is for pre-duty phase, when in my code is SERVOCMDMODE = 1
It's on for 84% of the time and it shouldn't, it should be 5%...

I'm looking at my code thinking 'that is not possible'...

So, I've got to have made a stupid mistake somewhere and i can't see it. Can some wonderful person tell me what my stupid mistake is?
Thanks
comwarrior
- 17th August 2013, 12:41
Okay, this is worrying... Since no one has said anything that would indicate i havn't make a mistake...
So the question is why is it not doing what it should...
mark_s
- 17th August 2013, 16:21
From a quick look at your code. I don't see where your intererupt calls
"SERVO_HANDLER:". Also the endif's after @ INT_RETURN will never be executed since the program will return before hitting the end if. I can't analysis your code fully but I would restructure it. I did a quick cut and paste on your code. Untested it may get you going



SERVO1 VAR LATD.0
SERVO1POS VAR BYTE
PRELOAD1 VAR WORD
PRELOAD2 VAR WORD
PRELOAD3 VAR WORD
SERVOCMDMODE VAR BYTE
SERVOPWMCOUNTER VAR WORD

PRELOAD1 = 63556 ' 990us '63556
TMR0_CON_PL1 CON %10000010 ' 1:8
PRELOAD2 = 65528 ' 4us '65528
TMR0_CON_PL2 CON %10000010 ' 1:8
PRELOAD3 = 29556 ' 17990us '29556
TMR0_CON_PL3 CON %10000010 ' 1:8
SERVOCMDMODE = 1
'_________________________________________________ ___________
@ INT_ENABLE TMR0_INT
TMR0L = PRELOAD3.LOWBYTE
TMR0H = PRELOAD3.HIGHBYTE
T0CON = TMR0_CON_PL3
SERVO_HANDLER:
IF SERVOCMDMODE = 2 Then
IF SERVOPWMCOUNTER < 256 THEN
T0CON = TMR0_CON_PL2
TMR0L = PRELOAD2.LOWBYTE
TMR0H = PRELOAD2.HIGHBYTE
IF SERVO1POS < SERVOPWMCOUNTER THEN
SERVO1 = 0
LATD.1 = 0
LATD.2 = 0
LATD.3 = 0
ELSE
SERVO1 = 1
LATD.1 = 0
LATD.2 = 1
LATD.3 = 0
ENDIF
SERVOPWMCOUNTER = SERVOPWMCOUNTER + 1
ELSE
T0CON = TMR0_CON_PL3
TMR0L = PRELOAD3.LOWBYTE
TMR0H = PRELOAD3.HIGHBYTE
SERVO1 = 0
LATD.1 = 0
LATD.2 = 0
LATD.3 = 1
SERVOCMDMODE = 1
ENDIF


IF SERVOCMDMODE = 1 Then
T0CON = TMR0_CON_PL1
TMR0L = PRELOAD1.LOWBYTE
TMR0H = PRELOAD1.HIGHBYTE
SERVOCMDMODE = 2
SERVO1 = 1
LATD.1 = 1
LATD.2 = 0
LATD.3 = 0
SERVOPWMCOUNTER = 1
ENDIF

@ INT_RETURN
'_________________________________________________ _____________
MAIN:
SERVO1POS = 0
PAUSE 5000
SERVO1POS = 128
PAUSE 5000
SERVO1POS = 255
PAUSE 5000
GOTO MAIN

END
comwarrior
- 17th August 2013, 21:44
Mark, Thank you for the reply.
perhaps i should have explained more.
I'm using DT's interupts. SERVO_HANDLER is jumped to whenever TMR0 overflows and interrupts.

The endif's after the @ INT_RETURN do not need to be 'executed' they just define the end if that IF statement. The @ INT_RETURN is placed within the IF statement so that it does not attempt to phrase the second IF statement after executing the first one thus saving time and eliminating a bug.

MAIN: is their to make the servo move so that i can see it's throw and thus that it's working. Once the PC has setup itself, then it's vars it then needs to fall into the MAIN: loop. Once their it can interrupt into the SERVO_HANDLER when the TMR0 goes off...

Vars TMR0_CON_PL1 & PRELOAD1 are the TMR0 settings for the 1000us initial on for the control signal to the servo.
Vars TMR0_CON_PL2 & PRELOAD2 are the TMR0 setting for the PWM duration. This 4us duration is executed 255 times totalling over 1000us for the pwm period of the control signal to the servo.
Vars TMR0_CON_PL3 & PRELOAD3 are the TMR0 setting for the sleet time after the PWM has finished to make a 50Hz signal (20000us total) to the servo.
HenrikOlsson
- 18th August 2013, 11:26
Hi,
I think what Mark is saying is that there are parts of DT-Ints missing in your code, you don't seem to include the needed files and there's no place where you're actually defining/setting up the interrupts. Is that really the complete code you've posted? My guess is no since I don't think it would even compile like that.

What's your oscillator speed? 8MHz? Do you have that defined somewhere in your code?

I'm pretty sure that interrupting at 4us intervals isn't going to work, IF you're running at 8MHz that's only 8 instructions between each interrupt and it takes WAY WAY more than that for DT-Ints just to do the context save and restore, the interrupts are "running over them selves".

/Henrik.
comwarrior
- 18th August 2013, 11:50
ok, here is the full code... i didn't think the top bit was necessary...



@ __config _CONFIG1H, _FOSC_INTIO67_1H & _PLLCFG_ON_1H & _PRICLKEN_ON_1H & _FCMEN_OFF_1H & _IESO_OFF_1H
@ __config _CONFIG2L, _PWRTEN_ON_2L & _BOREN_SBORDIS_2L & _BORV_220_2L
@ __config _CONFIG2H, _WDTEN_OFF_2H & _WDTPS_32768_2H & _CCP2MX_PORTC1_3H & _PBADEN_OFF_3H
@ __config _CONFIG3H, _CCP3MX_PORTE0_3H & _HFOFST_OFF_3H & _T3CMX_PORTC0_3H & _P2BMX_PORTC0_3H & _MCLRE_INTMCLR_3H
@ __config _CONFIG4L, _STVREN_ON_4L & _LVP_OFF_4L & _XINST_OFF_4L & _DEBUG_OFF_4L
@ __config _CONFIG5L, _CP0_OFF_5L & _CP1_OFF_5L & _CP2_OFF_5L & _CP3_OFF_5L & _CP1_OFF_5L & _CP2_OFF_5L & _CP3_OFF_5L
@ __config _CONFIG5H, _CPB_OFF_5H & _CPD_OFF_5H
@ __config _CONFIG6L, _WRT0_OFF_6L
@ __config _CONFIG6H, _WRTC_OFF_6H & _WRTB_OFF_6H & _WRTD_OFF_6H
@ __config _CONFIG7L, _EBTR0_OFF_7L & _EBTR1_OFF_7L & _EBTR2_OFF_7L & _EBTR3_OFF_7L
@ __config _CONFIG7H, _EBTRB_OFF_7H

INCLUDE "DT_INTS-18.bas" ; Base Interrupt System
INCLUDE "ReEnterPBP-18.bas" ; Include if using PBP interrupts

ASM
INT_LIST macro ; IntSource, Label, Type, ResetFlag?
INT_Handler TMR0_INT, _SERVO_HANDLER, PBP, yes
endm
INT_CREATE ; Creates the interrupt processor
ENDASM

Define OSC 64 ' Set clock speed

OSCCON.7 = 0
OSCCON.6 = 1
OSCCON.5 = 1
OSCCON.4 = 1
OSCCON.1 = 0
OSCCON.0 = 0
OSCTUNE.6 = 1

ANSELA = %00000000
ANSELB = %00000000
ANSELC = %00000000
ANSELD = %00000000
ANSELE = %00000000

TRISB = 0
PORTB = 0
TRISC = 0
PORTC = 0
TRISD = 0
PORTD = 0
TRISE = 0
PORTE = 0
TRISA = 0
ALOOP VAR WORD

SERVO1 VAR LATD.0
SERVO1POS VAR BYTE
PRELOAD1 VAR WORD
PRELOAD2 VAR WORD
PRELOAD3 VAR WORD
SERVOCMDMODE VAR BYTE
SERVOPWMCOUNTER VAR WORD

PRELOAD1 = 63556 ' 990us '63556
TMR0_CON_PL1 CON %10000010 ' 1:8
PRELOAD2 = 65528 ' 4us '65528
TMR0_CON_PL2 CON %10000010 ' 1:8
PRELOAD3 = 29556 ' 17990us '29556
TMR0_CON_PL3 CON %10000010 ' 1:8

@ INT_ENABLE TMR0_INT
TMR0L = PRELOAD3.LOWBYTE
TMR0H = PRELOAD3.HIGHBYTE
T0CON = TMR0_CON_PL3
SERVOCMDMODE = 1

MAIN:
SERVO1POS = 0
PAUSE 5000
SERVO1POS = 128
PAUSE 5000
SERVO1POS = 255
PAUSE 5000
GOTO MAIN

SERVO_HANDLER:
IF SERVOCMDMODE = 2 Then
IF SERVOPWMCOUNTER < 256 THEN
T0CON = TMR0_CON_PL2
TMR0L = PRELOAD2.LOWBYTE
TMR0H = PRELOAD2.HIGHBYTE
IF SERVO1POS < SERVOPWMCOUNTER THEN
SERVO1 = 0
LATD.1 = 0
LATD.2 = 0
LATD.3 = 0
ELSE
SERVO1 = 1
LATD.1 = 0
LATD.2 = 1
LATD.3 = 0
ENDIF
SERVOPWMCOUNTER = SERVOPWMCOUNTER + 1
ELSE
T0CON = TMR0_CON_PL3
TMR0L = PRELOAD3.LOWBYTE
TMR0H = PRELOAD3.HIGHBYTE
SERVO1 = 0
LATD.1 = 0
LATD.2 = 0
LATD.3 = 1
SERVOCMDMODE = 1
ENDIF
@ INT_RETURN
ENDIF

IF SERVOCMDMODE = 1 Then
T0CON = TMR0_CON_PL1
TMR0L = PRELOAD1.LOWBYTE
TMR0H = PRELOAD1.HIGHBYTE
SERVOCMDMODE = 2
SERVO1 = 1
LATD.1 = 1
LATD.2 = 0
LATD.3 = 0
SERVOPWMCOUNTER = 1
@ INT_RETURN
ENDIF

@ INT_RETURN
HenrikOlsson
- 18th August 2013, 12:16
Hi,
OK, so you're running at 64MHz, that gives it bit more time but I still think that interrupting at 4us intervals is way to fast, there's only 64 instruction cycles between each interrupt and I think it takes more than that for the DT-Ints system just to save and later restore the system variables. And, even if it does work the code would spend most of its time in transit to and from the interrupt handler.

/Henrik.
LettyLocke
- 20th August 2013, 08:51
The time between transit and interrupt handler is the main culprit here in your servo pulse width modulation. Sometimes the turn around time makes us wait more and it doesn't work in the end even though everything seems right. Happens with me all the time. Connected a lipo battery once as a power supply and it never worked because of misconfiguration.
Good luck to you.
LettyLocke
- 22nd August 2013, 08:08
The time between transit and interrupt handler is the main culprit here in your servo pulse width modulation. Sometimes the turn around time makes us wait more and it doesn't work in the end even though everything seems right. Happens with me all the time. Connected a lipo battery (http://www.mrpositive.co.nz/lipo-batteries) once as a power supply and it never worked because of misconfiguration.
Good luck to you.

No more updates regrading the issue? I think the interval time of interrupts must be increased in order for it to work. Interrupt handlers are very tricky to work with.
Acetronics2
- 22nd August 2013, 20:57
some pointers ???

http://www.mikroe.com/forum/viewtopic.php?f=10&t=7081

Alain