Picaxe LCD Direct Connection Problem

Hey There,

I've hit a snag on a project of mine. I've been trying to build a servo calibrator (to test a servo's limits and test them on robots before they're connected to the motherboard) which should operate by positioning a servo depending on the position of a potentiometer, and display the pulse rate on a 16x2 LCD. I've built one before, with an LED display and a single servo output, but this one should handle two servos.

Except it doesn't. I won't even handle one.  There's two major problems I've found which I'm looking for solutions to.

This is my code (Picaxe 20M chip). I copied the three LCD routines from the manual, the rest is mine.

 init: gosub LCDinit

eeprom 0, ("  Servo 1:  Servo 2: ")

let b1 = 12

gosub wrins

let b1 = 1

gosub wrins

main: readadc 1, b8

readadc 2, b9

let b1 = 128

gosub wrins

for b3 = 0 to 10

read b3, b1

gosub wrchr

next b3

let b4 = b8

gosub servocal

let b1 = 192

gosub wrins

for b3 = 10 to 20

read b3, b1

gosub wrchr

next b3

let b4 = b9

gosub servocal

goto main

servocal: let b7 = b4 / 100

let b1 = b7 + 48

gosub wrchr

let b5 = b7 * 100

let b7 = b4 - b5 / 10

let b1 = b7 + 48

gosub wrchr

let b6 = b7 * 10

let b7 = b4 - b5 - b6

let b1 = b7 + 48

gosub wrchr

return

LCDinit: let pins = 0 'Clear all output lines 

let b4 = 0 'Reset variable b3 

pause 200 'Wait 200 ms for LCD to reset. 

let pins = 48 'Set to 8-bit operation. 

pulsout 3,1 'Send data by pulsing ‘enable’ 

pause 10 'Wait 10 ms 

pulsout 3,1 'Send data again 

pulsout 3,1 'Send data again 

let pins = 32 'Set to 4-bit operation. 

pulsout 3,1 'Send data. 

pulsout 3,1 'Send data again. 

let pins = 128 'Set to two line operation 

pulsout 3,1 'Send data. 

let b1 = 14 'Screen on, cursor on instruction 

gosub wrins 'Write instruction to LCD 

return 

wrchr: let pins = b1 & 240 'Mask the high nibble of b1 into b2. 

high 2 'Make sure RS is high 

pulsout 3,1 'Pulse the enable pin to send data. 

let b2 = b1 * 16 'Put low nibble of b1 into b2. 

let pins = b2 & 240 'Mask the high nibble of b2 

high 2 'Make sure RS is high 

pulsout 3,1 'Pulse enable pin to send data. 

return 

wrins: let pins = b1 & 240 'Mask the high nibble of b1 into b2. 

pulsout 3,1 'Pulse the enable pin to send data. 

let b2 = b1 * 16 'Put low nibble of b1 into b2. 

let pins = b2 & 240 'Mask the high nibble of b2 

pulsout 3,1 'Pulse enable pin to send data. 

high 2 'Back to character mode 

return 

Now my two problems.

#1: That code works fine when displaying the pot readings on the LCD, but you may notice that there's no servo commands in there. That's because as soon as I add anything on my servo out pins (0 and 1) the LCD freezes. It just goes blank. Remove the lines, and it works fine.

 #2: One of my pot inputs gives logarithmic readings. This is weird considering I have two linear pots. Any pot I put on Servo 1's input is log, any pot on Servo 2's input it linear. Swapping the pots around just proves it's nothing to do with them.

 So... this is my problem. Has anyone got any ideas? Thanks!