Bascom: Zawieszanie programu przy sterowaniu bramą dwuskrzydłową 89C2051

Witam
Pisze w imieniu kolegi co probuje sobie napisać program do sterowania bramy, z góry dziekuje za pomoc
'uP 89C2051
'Jest to program który ma za zadanie sterować bramą dwuskrzydłową (dwa silniki)i wczasie sterowania wysyłać na RS232
'info ilości zliczonych impuslów, potrzebne mi to jest do uruchomienia.
'Poniższy program ma za zadanie sprawdzać stan na P1.2 którą steruje pilot określajacy czy brama ma sie otwierać
' zzy zamykać. Cały problem jest w tym że jak dodałem funkcje print (RS232) program zaczął sie zawieszać, dodatkow zauważułem
' ze jeden z liczników pomimo zerowania ma juz wartość 8

$baud = 9600
$crystal = 11059200
Config Sda = P3.2
Config Scl = P3.3
Config Timer0 = Counter , Gate = Internal , Mode = 2
Config Timer1 = Counter , Gate = Internal , Mode = 2
Enable Timer0
Enable Timer1
Dim Aa As Byte
Dim Aaa As Byte
Dim Bb As Byte
Dim Bbb As Byte
Dim 1 As Byte
Dim 2 As Byte
Dim Cc As Byte
Declare Sub Zam
Declare Sub Otw
Declare Sub Licz
Declare Sub Zap

P1.4 = 0
P1.5 = 0
P1.6 = 0
P1.7 = 0
Aaa = 11
Bbb = 11
Do
If P1.2 = 0 Then Zam

If P1.2 = 1 Then Otw
Loop
'--------------------'

Sub Zam
Aa = 0
Bb = 0
P1.4 = 1
P1.5 = 1
P1.6 = 0
P1.7 = 0
Do
Set P3.7
If P1.2 = 1 Then Otw
Start Counter0
Start Counter1
Waitms 100
Reset P3.7
Waitms 100
Aa = Counter0
Bb = Counter1
If Aa > Aaa Then
P1.4 = 0
Counter0 = 0
End If
If Bb > Bbb Then
P1.5 = 0
Counter1 = 0
End If
' Print Aa : tu zawiesza sie , instr. do kontroli zmiennych'
Loop
End Sub
'---------------------'
Sub Otw
Aa = 0
Bb = 0
P1.4 = 0
P1.5 = 0
P1.6 = 1
P1 .7 = 1
'--------------------'
Do
Set P3.7
If P1.2 = 0 Then Zam
Start Counter0
Start Counter1
Waitms 100
Reset P3.7
Waitms 100
Aa = Counter0
Bb = Counter1
If Aa > Aaa Then
P1.6 = 0
Counter0 = 0
'---------- zmienna aa po załączeniu ma już nal. 8 impulsow'
End If
If Bb > Bbb Then
P1.7 = 0
Counter1 = 0
End If
Loop
End Sub
'-------------------c.d.n.'
Sub Licz
I2cstart
I2cwbyte 160

I2cstop
End Sub
'--------------------'
Sub Zap
I2cstart
I2cwbyte 160
I2cwbyte 1
I2cwbyte Aaa
I2cwbyte 2
I2cwbyte Bbb
I2cstop
Waitms 10
End Sub