[BASCOM] Wyłącznik na 89C2051. Program czasami działa, a czasami nie.

Przycisk Alias P3.0
P1.4 = 0                                                      'Wyzerowanie stanu przekaznika( P1.2 = wylaczony)
Przycisk = 1

'Program wlaczania

Sub On_u
   Przycisk = 1

   Do

         If Przycisk = 0 Then
          Bitwait Przycisk , Reset
          Wait 1
          Reset P1.4
          Waitms 650
          Przycisk = 1
          Call Off_u
         End If
   Loop
End Sub
'Program wylaczania
Sub Off_u

   Przycisk = 1
   Do
         If Przycisk = 0 Then
          Bitwait Przycisk , Reset
          Wait 1
          Set P1.4
          Waitms 650
          Przycisk = 1
          Call On_u
         End If
   Loop
End Sub

Dim A As Bit
Dim D As Bit
Dim C as Bit
A = P0.7  ' pin przycisku  
do 
If P0.7 <> A Then
    If A = 1 Then
    D = 1
    A = P0.7
     Else
     A = P0.7
     C = 1
     End If
     End If
If D = 1 Then
  P1.0 = 1     'pin przekaznika
    End If
If C = 1 Then
   P1.0 = 0
    End If

If D = 1 Then           
     D = 0
     End If
     If C = 1 Then
     C = 0
     End If
loop
end 

Debounce Przycisk,0,Obsluga_przycisku,SUB  'przycisk zwierany do masy 

Loop 

'******** OBSLUGA PRZYCISKU 
Obsluga_przycisku: 
  If Wlaczony_przekaznik=0 Then 
    Przekaznik=1 
    Wlaczony_przekaznik=1 
  Else 
    Przekaznik=0 
    Wlaczony_przekaznik=0 
  Endif 
Return

Dim A1 As Integer
Dim Stan As Integer
Dim Czas As Integer
Config Debounce = 100
Poz Alias P3.7
B1 Alias P3.0
Czas = 32
Do
Locate 1 , 1
Czas = Czas - 1
If Czas = 0 Then Set B1
Lcd "Podswietlanie -"
   Debounce Poz , 0 , On_off , Sub
   If Stan = 1 Then

 Lcd " ON" ; Spc(1)
 Set B1
Else
Lcd " OFF" ; Spc(1)
 Reset B1
End If

Loop

On_off:
Incr Stan
If Stan > 2 Then
  Stan = 1
End If

Return 

Do
   If Przycisk = 0 Then
      Waitms 25
      If Przycisk = 0 Then
         Przekaznik = Not Przekaznik
         Bitwait Przycisk , Set
      End If
   End If
Loop
End