Scout and Distract Robot

Let's Make Robots Projects Showcase
41

Doing Ok, I built it and run it around with the Laser 6 remote and I receive video back with the boostervision camera. I have not run it much yet outside as most days have been reasonably wet outside and those days that are not, we have contractors running around.

I ordered stuff like an Atom Pro and a bunch of cables and the like for phase 2 (delivery scheduled for Friday), where I now would like to make it run autonomous, but with information from the remote.

Step 1 is to plug all 6 channels from the RC receiver into the pro and decode the signals and in some cases just pass them on through. Maybe depending on the status of the landing gear switch. I believe I have read a few threads where others are doing similar things.

I will also be adding on the gripper plus sensors (some IR, some I2C (SRF08 SRF10 CMP10), encoder input, +++) and an LCD. I am not sure yet if the Atom Pro will be able to handle all of this (both I/O pin wise and horsepower), but I will start off with 1. I may use my spair SSC32 to offload the servos and whatever else makes sense. Or I may add on a second processor board. Not sure yet, but at least I will have some fun things to try out.

So far the voltage regulator appears to be working out fine for getting 6v from the 12vs (two 2800mah 6v batteries in series). Not sure yet what the best way to charge this. Currently I unscrew the top, disconnect each battery and use the 6v green charger. It would be nice to hook up some external jacks and simply plug in the 12v blue charger and charge them all at once.

Now back to checking up on the contractor…

42

Minor update. I have been playing around with the trying to have the Atom Pro trying to reliably intercepting the PWM signals from the Hitec receiver. My first attempt looked like:

[code]Variable declaration

PULSE_SLOP con 10 ; try to define some slop in pulse size so dont just flop back and forth
lPulse var long(6)
LPulseT var long

lPulsePrev var long(6)

bChannel var byte
bChannelMask var byte
pass var byte
PulseChanged var byte
TCNTStart var word(6)

…
;============================== Start of CODE ================================================
; Initialization code
gosub LCD_Init

;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

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 = 0 ;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.

; TimerW regs
TCRW.bit5 = 1 ;TIMERW counts on clk/8
TCRW.bit4 = 1 ;TIMERW counts on clk/8
TCRW.bit7 = 0 ;TCNT free running counter
TMRW.bit7 = 1 ;TCNT free running counter

pass = 0
PulseChanged = 0

ENABLE WKPINT_0
ENABLE WKPINT_1
ENABLE WKPINT_2
ENABLE WKPINT_3
ENABLE WKPINT_4
ENABLE WKPINT_5

LCD_String = “PWM Intercept”, 0
gosub LCD_Print[1, 0]

;================================ Main Loop =================================================
main_loop

; I wish I had some semaphores!

if PulseChanged then
	DISABLE WKPINT_0 
	DISABLE WKPINT_1
	DISABLE WKPINT_2
	DISABLE WKPINT_3
	DISABLE WKPINT_4
	DISABLE WKPINT_5

	gosub LCD_SetPos[0, 1]
	for i = 0 to 5
		if i = 3 then
			gosub LCD_SetPos[0, 2]
		endif
		gosub LCDConvertNumToString[lPulse(i)]
		gosub LCD_Print[0,0]
		LCD_STRING = " ",0
		gosub LCD_PRINT[0,0]
	next
	PulseChanged = 0
	ENABLE WKPINT_0 
	ENABLE WKPINT_1
	ENABLE WKPINT_2
	ENABLE WKPINT_3
	ENABLE WKPINT_4
	ENABLE WKPINT_5

	pause 500
endif

goto main_loop

'============================================================================================
’
’ Handle interrupts for each of the channels

handle_intp0
bChannel = 0
goto HandleChannel

handle_intp1
bChannel = 1
goto HandleChannel

handle_intp2
bChannel = 2
goto handleChannel

handle_intp3
bChannel = 3
goto handleChannel

handle_intp4
bChannel = 4
goto handleChannel

handle_intp5
bChannel = 5
goto handleChannel

HandleChannel
bChannelMask = 1 << bChannel

If (pass & bChannelMask) = 0 then 
	' Handle rare case of multiple channels active same time, by only resetting TCNT if we are the only channel...
	if pass = 0 then
		TCNT = 0
	endif
  	TCNTStart(bChannel) = TCNT 
  	IEGR2 = IEGR2 & ~bChannelMask ' interrupt on falling edge 
  	pass = pass | bChannelMask

else
lPulseT = (TCNT - TCNTStart(bChannel))/2 ; divide by 2 to obtain us
if ((lpulseT + PULSE_SLOP) < lPulse(bChannel)) or ((lPulseT - PULSE_SLOP) > lPulse(bChannel)) then
lPulse(bChannel) = lpulseT
PulseChanged = 1
endif
IEGR2 = IEGR2 | bChannelMask 'interrupt on a rising edge
pass = pass & ~bChannelMask
endif
Resume

…[/code]

I do get the interrupts properly and calculate numbers. I believe that the first channel comes in accurately, but the others timings are off as I believe the ammount of time I spend processing the falling edge of one channel delays the processing of the raising edge of the next one and so the timing is to short. I looked at the 6 channels using the Parallax USB Oscilloscope and it looks like the 6 channels are each output sequentially, starting with CH1. It looks like CH2 might go high just before CH1 goes low, but could be same time. After CH6 goes low their is a delay until the cycle is repeated. (Each cycle is something like 20ms).

For my next attempt I will assume that channels are sequential and on rising edge of CH1 I will reset the TCNT to zero and then wait for the falling edge of all 6 channels and simply save away TCNT difference from one channel to the next. I may also have the code do most of its processing in the dead time after CH6.

Anyone have any other suggestions?

43

…as LATE as my reply is i think this is a great option…over at e-savage.com we are working on this right now…Hate loosing this big BomBot contract out to TRAXXAS when HPI has EXACTLY what one might want…

…The E-savage is a PERFECT platform for this sort of thing and nowhere NEAR as ‘delicate’ as a rover…

44

Sorry but an e-savage and an e-maxx are much more delicate than any one of my rovers. Not only can you drop the standard ones off a building but they also come in bulletproof versions. And that’s real bullet-proof not a figure of speech.

45

Where are those test videos!!! :smiley:

But seriously… how do you bulletproof your rovers? I know what bullet proofing takes for military vehicles… its not light nor cheap.

46

…Evo my apologies for not knowing who or what you are involved in…nice stuff…

…but you also have to appreciate the difference in the price points…If i spent 1500+ dollars on MY E-Sav then it would be bulletproof too…I will take a closer look at your stuff though as i am very interested in it…

…but for a ‘normal’ user who might want a very simple, usuable and affordable (relative I know) ‘rover’ base, an E-Savage or that ‘other truck’ :unamused: are viable options…

…am i officially off topic yet? Sorry…

47

No offense taken bud. And you are absolutely right. I have seen some impressive things done with R/C trucks. And for the the price, they are a versatile and easily modifiable Remotely Operated independent suspension vehicle.

Absolutely e-savages are great platforms.

And Sienna, we use 1/2" Lexan/Poly Composites as well as Kevlar sandwiches for bulletproofing. The camera casing is an aluminum tube with a steel sleeve inside and a bulletproof lens. The pan/tilt assembly uses slipper clutches to absorb the impact of small caliber rounds and bird shot and prevent stripping of the gears.

48

For Future projects I may consider one of these Truck platforms, but I am having a lot of fun with the 4wd platform.

I have made a little progress since the previous post. I think I am now reasonably calculating the pulse widths for all 6 channels coming in from the receiver. This weekend I tried then outputing some of them with HSERVO commands which failed. I goofed and used the TimerW in my code, which HSERVO relies on. So I will probably convert my code over to TimerA, which will probably need to process the timer overflow interrupt, which is no big deal. The other alternative I am considering is to move all of the servo processing to an SSC-32, which I may want to do anyway as I think my ABB will soon run out of IO lines.
(6 hitec in, 2 to motor controller, 2 for Pan/Tilt, 3 for gripper, 4 for encoders,1 LCD, 2 I2C, ? IR sensors, …)

49

Updates?

50

I have been busy with non-robot related things (house building), so I have not had much time to work on it. Also ran into problems with Atom Pro beta releases, which took some of that time and appear to be resolved.

So far I have the Atom Pro receiving the PWM signals from the Hitec Receiver and then generating HSERVO commands to run the motors through the Sabertooth controller. Will soon have it also controlling the Pan/Tilt and the Gripper. I may also convert the Motor control to some other form like simple serial…

I have the second deck sitting out waiting for me to put on the external sensors and start doing the autonomous programming.

But I keep getting side tracked so this may take awhile. I would take pictures, but right now it simply looks like a standard rover with the Atom Pro screwed intot he top along with the Hitec receiver velcroed to case as well and six wires going between the two…

Now back to another diversion of getting the brat to work again with the current software…

51

Just thought I would post a quick update:

I am making some progress on having the Atom Pro properly intercept the PWM signals from the hitec receiver using ASM interrupt handlers and the HSERVO timer tick from the .17exp release. When I use it to then go on to output HSERVO commands, it does get a bit jumpy. I know that I need to add some checks for when the 32 bit timer counter rolls over, which is one problem. In the end I may offload the servo handling to an SSC-32 which would help it out a lot. Or potentially I could put a second processor board on the rover which handles these IOS and maybe communicates back to the PRO by something like I2C. The IRQ2 is connected to one channel of one of my motors and it is getting called, but now I need to do all of the work for checking the other channel and then also do it for the second encoder. Lots of work left, but I thought I would post the current code in case anyone is interested. Someday I will actually work on making the robot go outside…

The assembly code took me longer than it should have. Some of the hardware manual was unclear to me at times. Things like if I do something like: btst.b, #1,r1l, do you do a beq xxx when the actual bit is set in the register or when it is clear… Also I first tried doing something like: “bset.b #0,@FDATAVALID:16” thinking that it would work to set a byte variable to non-zero and have basic pick it up. It did not work, so I ended up doing something like: mov.b #0xff,r1l mov.b r1l,@FDAT…

Sure wish the IDE debugger allowed me to step through the assembly code and see the registers!

[code]’ Constant definitions
TRUE con 1
FALSE con 0

;Variable declaration
iOverflowTime var sword
PULSE_SLOP con 4 ; try to define some slop in pulse size so dont just flop back and forth

lEncoder1Cnt var slong
lLastPulse var long
aiPulse var sword (6)
alPulseTimingTICKS var long(6)
iPulseT var sword

fPulseTimingsBusy var byte
fPulseDataValid var byte
fProcessingPulses var byte
fPulseChanged var byte

i var byte
;============================== Motor or Servos ================================================
enablehservo

MOTORSERVO1 con p6
MOTORSERVO2 con p7
fMotorActive var bit
iMotor1Speed var sword
iMotor1SpeedPrev var sword
iMotor2Speed var sword
iMotor2SpeedPrev var sword

;=================================== 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)

;=============================- Interrupt definitions =======================================
;Interrupt init
ONASMINTERRUPT WKPINT,handle_WKP

ONASMINTERRUPT IRQ2INT, HANDLE_IRQ2

;============================== Start of CODE ================================================
; Initialization code

gosub LCD_Init

LCD_String = “PWM Intercept”, 0
gosub LCD_Print[1, 0]

;----------------------------- Init System components ---------------------------------------
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.

PMR1.bit6 = 1 ; enable pin to IRQ2 interrupt instead of normal I/O
IEGR1.bit2 = 1 ; Interrupt IRQ2 on risigin edge

#if 0
; TimerA regs
iOverflowTime = 0
TMA = 0x6 ; clk/32 (Wish there was /16 but try to cut down interrupts…)
#endif

fPulseTimingsBusy = FALSE
fProcessingPulses = FALSE
fPulseDataValid = FALSE
fPulseChanged = FALSE
aiPulse = 0,0,0,0,0,0
alPulseTimingTicks = 0,0,0,0,0,0

iMotor1Speed = 0
iMotor2Speed = 0

fMotorActive = FALSE
ENABLE WKPINT

ENABLE IRQ2INT
;================================ 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 and convert into pulse values.
		fPulseChanged = 0
		for i = 0 to 5				
			iPulseT = alPulseTimingTicks(i) / 16; convert ticks into ms
			if ((iPulseT + PULSE_SLOP) < aiPulse(i)) or ((iPulseT - PULSE_SLOP) > aiPulse(i)) then
				aiPulse(i) =  iPulseT
				fPulseChanged = 1
			endif
		next
		fPulseDataValid = FALSE
		'
		' Now See if we should update our motor speeds or servo locations.  Next round will be to take more than 1 sample for 
		' each pulse and average and if any of the values differ by some threshold, restart the samples as either something is changing or
		' there was a glitch in capturing the signals.
		
		if fPulseChanged then
		
			if !fMotorActive and (aiPulse(4) > 1500) then
				fMotorActive= TRUE
			endif

			' See if we are active
			if fMotorActive then
				if aiPulse(4) < 1500 then
					' Switch on transmitter set to not run motor...
					hservo [MOTORSERVO1\0, MOTORSERVO2\0]		; first make sure to stop the motors...
					HServoWait[MOTORSERVO1, MOTORSERVO2]
					;
				    hservo [MOTORSERVO1\-16000, MOTORSERVO2\-16000]
					fMotorActive = FALSE
					gosub DisplayPulses
				else
					' see if the speed or direction changed
					' The ranges intercepted appear to go from about 1100 to about 1900 center at 1500.  
					' hservo has a range of -12000 to 12000 center at 0 so each value intercepted equals 30 units.
					iMotor1SpeedPrev = iMotor1Speed
					iMotor2SpeedPrev = iMotor2Speed
					
					iMotor1Speed = (((aiPulse(0)-1500)*30) max 12000) min -12000
					iMotor2Speed = (((aiPulse(1)-1500)*30) max 12000) min -12000
					
					if (iMotor1Speed <> iMotor1SpeedPrev) or (iMotor2Speed <> iMotor2SpeedPrev) then						
						hservo [MOTORSERVO1\iMotor1Speed, MOTORSERVO2\iMotor2Speed]
					endif
				endif
			endif
					
		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

DisplayPulses
DISABLE WKPINT

gosub LCD_SetPos[0, 1]
for i = 0 to 5
	if i = 3 then
		gosub LCD_SetPos[0, 2]
	endif
	gosub LCDConvertNumToString[aiPulse(i)/100]
	gosub LCD_Print[0,0]
	LCD_STRING = " ",0
	gosub LCD_PRINT[0,0]
next
gosub LCDConvertNumToString[LENCODER1CNT]
gosub LCD_PRINT[0,0]
ENABLE	WKPINT

return

'============================================================================================
’
’ 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.
BEGINASMSUB
handle_WKP
;save workspace registers
push.l er0
push.l er1
push.l er2

;Get current timer tick
mov.w @_TIMERWTICK:16,e0
mov.w @TCNT,r0
btst #7,@TIERW:8
beq _DONTINC:8
inc.w #1,e0

_DONTINC:
; we want to enable interrupts as quickly as possible. So lets get the interrupt information, clear it as quickly as possible and then
; reenable interrupts.
mov.b @IWPR:8,r1l
; clear the interrupt flag as quickly as possible and enable interrupts.
mov.b r1l, r1h
and.b #0xc0,r1h
mov.b r1h,@IWPR:8
andc #0x7f,ccr ; clear the I bit

;Figure out which WKP happened

; Now see if it is WKP0   
; Use Shift Right to find which interupt happened.
shlr.b	r1l
bcc		_NOTWKP0:8
mov.w	#0, e1
bra		_WKPKNOWN:8

_NOTWKP0:
; we only want to process and of the other WKPs if we processed the rising edge of WKP0
mov.b @FPULSETIMINGSBUSY:16, r1h
;or.b r1h,r1h ; mov sets zero state
beq _WKPEXIT:16

shlr.b	r1l
bcc		_NOTWKP1:8
mov.w	#1*4,e1			;might as well have e1 with the offset from the array instead of just the index
bra		_WKPKNOWN:8

_NOTWKP1:
shlr.b r1l
bcc _NOTWKP2:8
mov.w #2*4,e1
bra _WKPKNOWN:8

_NOTWKP2:
shlr.b r1l
bcc _NOTWKP3:8
mov.w #3*4,e1
bra _WKPKNOWN:8

_NOTWKP3:
shlr.b r1l
bcc _NOTWKP4:8
mov.w #4*4,e1
bra _WKPKNOWN:8

_NOTWKP4:
; we will assume it must be 5 if we got to here!
mov.w #5*4,e1
xor.b r1l, r1l
mov.b r1l,@FPULSETIMINGSBUSY:16 ; Clear the PULSE PROCESSING Busy
mov.b #0xff, r1l
mov.b r1l,@FPULSEDATAVALID:16 ; Tell the main function it now has valid data

_WKPKNOWN

; OK, we now have register E01 with which WKP happend and E00 with 
mov.b	@FPROCESSINGPULSES, r1l		; If the main code is processing the input then don't do anything
;or.b	r1l,r1l						; I think mov will set the zero bit
bne		_WKPEXIT

; processing of WKP0 is special as we need to check if we are doing rising edge or not.
cmp.w	#0,e1
bne		_PH2_NOTWKP0:8

btst.b	#0,@IEGR2:8
beq		_PH2_WKP0_FALL_EDGE:8

bclr.b	#0,@IEGR2:8					;	set to interrupt on falling edge
mov.b	#0xff,r1l
mov.b	r1l,@FPULSETIMINGSBUSY:16	; SET PULSE PROCESSING Busy	
mov.l	er0, @LLASTPULSE
bra		_WKPEXIT

_PH2_WKP0_FALL_EDGE:
bset.b #0,@IEGR2:8 ; Set to interrupt on Rising Edge again
; fall through and process like other interrupts
_PH2_NOTWKP0:

; calculate the aiPulseTimings for this array value.
mov.w	e1,r1
xor.w	e1,e1	
add.w	#ALPULSETIMINGTICKS, r1 

mov.l	@LLASTPULSE,er2		; get the last pulse time
mov.l	er0, @LLASTPULSE	; save away this pulse time

sub.l	er2, er0			; Calculate the time difference
mov.l	er0,@er1			; Save away the time difference in the array

; Fall through

;restore workspace registers
_WKPEXIT
pop.l er2
pop.l er1
pop.l er0

rte

ENDASMSUB

#if USING_BASIC_FOR_INTERRUPTS
handle_WKP0
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_WKP1
if fPulseTimingsBusy then
enable
; if (irr1.bit6) then ; pending interrupt?
; irr1.bit6 = 0
; iOverflowTime = iOverflowTime + 256
; endif

	aiPulseTimings(1) = iOverflowTime + TCA
endif
resume

handle_WKP2
if fPulseTimingsBusy then
enable
aiPulseTimings(2) = iOverflowTime + TCA
endif
resume

handle_WKP3
if fPulseTimingsBusy then
enable
aiPulseTimings(3) = iOverflowTime + TCA
endif
resume

handle_WKP4
if fPulseTimingsBusy then
enable
aiPulseTimings(4) = iOverflowTime + TCA
endif
resume

handle_WKP5
if fPulseTimingsBusy then
enable
aiPulseTimings(5) = iOverflowTime + TCA
fPulseDataValid = TRUE
fPulseTimingsBusy = FALSE
endif
resume
;-----------------------------------------------------------------------------------
; Handle TimerA interrupt
; Try Assembly level handler.
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
andc #0x7f,ccr ; allow other interrupts to happen
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
#endif

;---------------------------------------------------------------------------------------
; Handle IRQ2 interrupt - We will have the encoder Channel A on this interrupt.
; - First pass we will only handle the rising level
; - later we may try to handle the falling as well
BEGINASMSUB
HANDLE_IRQ2
push.l er1 ; first save away ER1 as we will mess with it.
bclr #2,@IRR1:8 ; clear the IRQ2 bit in the interrupt pending mask
andc #0x7f,ccr ; allow other interrupts to happen

mov.l @LENCODER1CNT:32,er1		; increment our count of encoders we have seen
inc.l #1,er1
mov.l er1, @LENCODER1CNT:32
pop.l er1

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]