Ok, I did a reinstall and so far my intercept code is not working with HSERVO enabled. Here is the code that simply displays the six values on the LCD.
[code]’ Constant definitions
TRUE con 1
FALSE con 0
;Variable declaration
iOverflowTime var word
PULSE_SLOP con 10 ; try to define some slop in pulse size so dont just flop back and forth
aiPulse var sword (6)
aiPulseTimings var sword(6)
iPulseT var sword
iPulsePrev var sword
fPulseTimingsBusy var bit
fPulseDataValid var bit
fProcessingPulses var bit
fPulseChanged var bit
i var byte
;enablehservo
;=================================== LCD ======================================================
;USEHSERIAL con 1
;#if USEHSERIAL
;enablehserial
;#else
; define which LCD I am using…
SEETRON con 1
LCDPIN con p15
;#endif
#ifdef SEETRON
;LCD_BAUD con n9600
;LCD_BAUD con 206
LCD_BAUD con n2400
#else
LCD_BAUD con i2400
#endif
; variables for LCD… Not sure how to pass an array to a gosub so.
LCD_String var byte(17)
;============================== Start of CODE ================================================
; Initialization code
gosub LCD_Init
LCD_String = “PWM Intercept”, 0
gosub LCD_Print[1, 0]
;Interrupt init
ONINTERRUPT WKPINT_0,handle_intp0
ONINTERRUPT WKPINT_1,handle_intp1
ONINTERRUPT WKPINT_2,handle_intp2
ONINTERRUPT WKPINT_3,handle_intp3
ONINTERRUPT WKPINT_4,handle_intp4
ONINTERRUPT WKPINT_5,handle_intp5
;ONINTERRUPT TIMERAINT,handle_timera
ONASMINTERRUPT TIMERAINT, HANDLE_TIMERA_ASM
PMR5.bit0 = 1 ;enables pin as WKP interrupt instead of normal I/O
PMR5.bit1 = 1 ;enables pin as WKP interrupt instead of normal I/O
PMR5.bit2 = 1 ;enables pin as WKP interrupt instead of normal I/O
PMR5.bit3 = 1 ;enables pin as WKP interrupt instead of normal I/O
PMR5.bit4 = 1 ;enables pin as WKP interrupt instead of normal I/O
PMR5.bit5 = 1 ;enables pin as WKP interrupt instead of normal I/O
IEGR2.bit0 = 1 ;0 = Pin will interrupt on a falling edge, 1 to interrupt on a rising edge.
IEGR2.bit1 = 0 ;0 = Pin will interrupt on a falling edge, 1 to interrupt on a rising edge.
IEGR2.bit2 = 0 ;0 = Pin will interrupt on a falling edge, 1 to interrupt on a rising edge.
IEGR2.bit3 = 0 ;0 = Pin will interrupt on a falling edge, 1 to interrupt on a rising edge.
IEGR2.bit4 = 0 ;0 = Pin will interrupt on a falling edge, 1 to interrupt on a rising edge.
IEGR2.bit5 = 0 ;0 = Pin will interrupt on a falling edge, 1 to interrupt on a rising edge.
; TimerA regs
iOverflowTime = 0
TMA = 0x6 ; clk/32 (Wish there was /16 but try to cut down interrupts…)
fPulseTimingsBusy = FALSE
fProcessingPulses = FALSE
fPulseDataValid = FALSE
fPulseChanged = FALSE
aiPulse = 0,0,0,0,0,0
aiPulseTimings = 0,0,0,0,0,0
ENABLE WKPINT_0
ENABLE WKPINT_1
ENABLE WKPINT_2
ENABLE WKPINT_3
ENABLE WKPINT_4
ENABLE WKPINT_5
ENABLE TIMERAINT
;================================ Main Loop =================================================
main_loop
; I wish I had some semaphores!
if fPulseDataValid and !fPulseTimingsBusy then
fProcessingPulses = true
if !fPulseTimingsBusy then
' handle checks and sets not atomic need to handle race conditions...
' probably won't be problem as we have a window of time after CH6 completes before
' ch1 starts again.
' loop through all 6 times and convert into pulses.
iPulsePrev = 0
for i = 0 to 5
iPulseT = (aiPulseTimings(i) - iPulsePrev)*2 ; multiply by 2 to obtain us
if ((iPulseT + PULSE_SLOP) < aiPulse(i)) or ((iPulseT - PULSE_SLOP) > aiPulse(i)) then
aiPulse(i) = iPulseT
fPulseChanged = 1
endif
iPulsePrev = aiPulseTimings(i)
next
fPulseDataValid = FALSE
'
' Now if any of the values changed more than our slop factor, show them now
if fPulseChanged then
DISABLE WKPINT_0
DISABLE WKPINT_1
DISABLE WKPINT_2
DISABLE WKPINT_3
DISABLE WKPINT_4
DISABLE WKPINT_5
DISABLE TIMERAINT
gosub LCD_SetPos[0, 1]
for i = 0 to 5
if i = 3 then
gosub LCD_SetPos[0, 2]
endif
gosub LCDConvertNumToString[aiPulse(i)]
gosub LCD_Print[0,0]
LCD_STRING = " ",0
gosub LCD_PRINT[0,0]
next
serout S_OUT, i2400, [hex aiPulse(0), " ", hex aiPulse(1), 13, 10]
fPulseChanged = 0
ENABLE TIMERAINT
ENABLE WKPINT_0
ENABLE WKPINT_1
ENABLE WKPINT_2
ENABLE WKPINT_3
ENABLE WKPINT_4
ENABLE WKPINT_5
endif
' let the interrupts know that they can process again.
endif
fProcessingPulses = false
endif
pause 500
;pulsout 10, pulse_ch1*2
;servo P10, (pulse_ch1-1500)*2, 2
goto main_loop
'============================================================================================
’
’ Handle interrupts for each of the channels
’ It appears like the channels happen sequentially. So we will only watch the rising edge of Ch0 and then record
’ the times for falling edges for all 6 channels. After channel ch6 completes we will trigger main program to do any other
’ processing that may be needed.
handle_intp0
if fProcessingPulses = FALSE then
If (IEGR2 & 0x01) then ; process rising edge
; need to reset timer
TMA = 0xc ; reset timer
TMA = 0x6 ; clk/32
iOverflowTime = 0
fPulseTimingsBusy = 1
IEGR2.bit0 = 0 ;interrupt on a falling edge.
else
enable
aiPulseTimings(0) = iOverflowTime + TCA
IEGR2.bit0 = 1 ;interrupt on a falling edge.
endif
endif
resume
handle_intp1
if fPulseTimingsBusy then
enable
; if (irr1.bit6) then ; pending interrupt?
; irr1.bit6 = 0
; iOverflowTime = iOverflowTime + 256
; endif
aiPulseTimings(1) = iOverflowTime + TCA
endif
resume
handle_intp2
if fPulseTimingsBusy then
enable
aiPulseTimings(2) = iOverflowTime + TCA
endif
resume
handle_intp3
if fPulseTimingsBusy then
enable
aiPulseTimings(3) = iOverflowTime + TCA
endif
resume
handle_intp4
if fPulseTimingsBusy then
enable
aiPulseTimings(4) = iOverflowTime + TCA
endif
resume
handle_intp5
if fPulseTimingsBusy then
enable
aiPulseTimings(5) = iOverflowTime + TCA
fPulseDataValid = TRUE
fPulseTimingsBusy = FALSE
endif
resume
handle_timera
iOverflowTime = iOverflowTime + 256
resume
BEGINASMSUB
HANDLE_TIMERA_ASM
push.w r1 ; first save away ER1 as we will mess with it.
bclr #6,@IRR1:8 ; clear the cooresponding bit in the interrupt pending mask
mov.w @IOVERFLOWTIME:16,r1 ; Add 256 to our counter
;add.w #0x100:16,r1
inc.b r1h
mov.w r1, @IOVERFLOWTIME:16
pop.w r1
rte
ENDASMSUB
;============================================= LCD Helper functions ==================================================
;
; Serial LCD functions added by Kurt
;
LCD_Init
#ifdef USEHSERIAL
sethserial h9600,h8databits,hnoparity,h1stopbits
#endif
; make sure SerLCD has a chance to finish initializing after power-up
; Reset to 9600 and powerup... - Uncomment if having problems with LCD.
;pause 100
;serout LCDPIN, I9600, [0x12]
Pause(1000)
; Now lets try setting it to 2400 baud. - uncomment if having problems
;Serout LCDPIN, I9600, [0x7c] ; 124, <control k>
;pause 2
;Serout LCDPIN, I9600, [11] ; 124, <control k>
#ifndef SEETRON
; Set the backlight to low
;#ifdef USEHSERIAL
; HSerout [0x7C, 0x80] ; 124, 128 - Off
;#else
Serout LCDPIN, LCD_BAUD, [0x7C, 0x80] ; 124, 128 - Off
#endif
;#endif
; Output the clear screen command
gosub LCD_WriteCommand[1] ; // Clear the screen
Pause(5)
return
;
; Main LCD output function
;
line var sbyte
fPad var byte
LCD_Print[line, fPad]
if line = 1 then
gosub LCD_WriteCommand[0x80]
elseif line = 2
gosub LCD_WriteCommand[0xC0]
endif
Pause(10)
for line = 0 to 14 ; try now to write to last position as it appears to autoscroll...
if fPad <> 2 and LCD_String(line) > 0 then
#ifdef USEHSERIAL
hserout [LCD_String(line)]
#else
serout LCDPIN, LCD_BAUD, [LCD_String(line)]
#endif
else
if fPad = 0 then
goto LCDP_Enable
endif
fPad = 2
#ifdef USEHSERIAL
hserout " "]
#else
serout LCDPIN, LCD_BAUD, " "]
#endif
endif
Pause(1)
next
LCDP_Enable
return
;
;DISPLAY DATA RAM ADDRESSES
;CHARACTER 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
; + – -- – -- – -- – -- – -- – -- – -- – --
;LINE 1 | 80 81 82 83 84 85 86 87 88 89 8A 8B 8C 8D 8E 8F
;LINE 2 | C0 C1 C2 C3 C4 C5 C6 C7 C8 C9 CA CB CC CD CE CF
;
ich var byte
iline var byte
LCD_SetPos[ich, iline]
if iline = 1 then
gosub LCD_WriteCommand[0x80+ich]
else
gosub LCD_WriteCommand[0xC0+ich]
endif
pause 10
return
; clears the display and resets the current location to the upper left */
LCD_ClearAndReturn
gosub LCD_WriteCommand[1] ; // clear display
Pause(100)
return
fRight var byte
ichShift var byte
LCD_ShiftDisplay[fRight, ichShift]
if fRight <> 0 then
fRight = 0x1c
else
fRight = 0x18
endif
while (ichShift > 0)
gosub LCD_WriteCommand[fRight]
ichShift = ichShift -1
wend
return
; Send a command to the LCD, first send a command prefix character and then then command
cmd var byte
LCD_WriteCommand[cmd]
#ifdef USEHSERIAL
hserout [0xfe]
pause 10
hserout [cmd]
#else
serout LCDPIN, LCD_BAUD, [0xfe]
pause 10
serout LCDPIN, LCD_BAUD, [cmd]
#endif
return
iCnT1 var byte
iCnt2 var byte
tb var byte
iOutLast var sword
iOut var sword
LCDConvertNumToString[iOut]
if iOut < 0 then
LCD_String(0) = “-”
iOut = -iOut
iCnt1 = 1
iCnt2 = 1
else
iCnt1 = 0
iCnt2 = 0
endif
do
iOutLast = iOut
LCD_String(iCnt1) = "0" + iOut // 10
iOut = iOut / 10
iCnt1 = iCnt1 + 1
while (iOutLast >= 10)
LCD_String(iCnt1) = 0
iCnt1 = iCnt1 - 1
while (iCnt1 > iCnt2)
;swap LCD_String(iCnt1), LCD_String(iCnt2)
tb = LCD_String(iCnt1)
LCD_String(iCnt1) = LCD_String(iCnt2)
LCD_String(iCnt2) = tb
iCnt1 = iCnt1 - 1
iCnt2 = iCnt2 + 1
wend
return
[/code]
The LCD does not initialize properly at all. The Rover version of this does not output commands through HSERVO to the Motor controller. I will try to investigate more later. But need to clean…
Also on the SWAP command, I believe we had this conversation before and I think you said your code may depend on long values and may not have been tested with other variable sizes such as a byte. My guess is that you may have swapped long values that then extended beyond my array and wiped out something like the return address. If you want I can either post or send you a message with other code: Brat, Rover, …
Thanks
Kurt
Edit: Forgot to mention on Rover: I have a 2800mah 12v Battery. (Actually 2 6v in series). Then I have a variable voltage controller (from Lynxmotion) that supplies 6V several amps to the AAB. Battery was recharged yesterday.