Mirley - Elektronika i Programowanie

Witam, na wstępie napiszę może założenia projektu.

Celem projektu jest zbudowanie bezprzewodowego sterownika do gazowego pieca CO.
Schemat sterownika:

Zasada działania:
Sterownik steruje pracą elektrozaworu gazu (grzanie) oraz pracą pompki wody. Sterownik po włączeniu zasilania przechodzi do stanu czuwania. W momencie podania stanu wysokiego na PinC.5 zostają wysterowane tranzystory T1 oraz T2 (jednoczesna praca elektrozaworu i pompki). Tranzystory są tak długo wysterowane jak długo jest podany stan wysoki na PinC.5, w momencie pojawienia się stanu niskiego na PinC.5, elektrozawór zostaje wyłączony, natomiast pompka pracuje nadal przez określony czas. Czas określamy poprzez przełącznik obrotowy, który zwiera do zera odpowiednie wejście (do wyboru mamy 15, 30, 45, 60 minut czasu pracy pompki). Po upływie tego czasu, pompka wody zostaje odłączona, program wraca do początku (stan czuwania). Jeżeli w trakcie pracy samej pompki, sterownik znów otrzyma stan wysoki na PinC.5, ma za zadanie ponownie wysterować elektrozawór gazu i pompkę, po pojawieniu się stanu niskiego na PinC.5, sterownik ma na nowo zacząć odliczać czas pracy samej pompki. Dodatkowo sterownik sprawdza czy na pewno jest wybrany jakiś czas pracy (czy np. nie odpięła się wtyczka od przełącznika), gdyby tak się zdarzyło po wyłączeniu grzania, sterownik nie pozwala na włączenie pompki lub ponownego grzania dopóki usterka nie zostanie naprawiona. Błąd nastawy czasu jest sygnalizowany poprzez diodę LED.

Reasumując w punktach:
1) Sterownik w stanie czuwania.
2) Stan wysoki na PinC.5 -> Sterownik włącza elektrozawór i pompkę.
3) Stan niski na PinC.5 -> Sterownika wyłącza elektrozawór, pompka pracuje przez ustalony czas (Nastawa 15, 30, 45, 60 minut).
4) Sterownik powraca do stanu czuwania.

*3) Sterownik dostaje znów stan wysoki, przestaje odliczać czas i powraca do punktu 2.

Mój problem polega na tym że odliczanie czasu jest zrealizowane komendą "WAIT xxx s." co blokuje procesor, a tym samym nie pozwala na ponowne włączenie grzania, póki nie skończy się jedno odliczanie. Próbowałem się z tym uporać poprzez Timery, jednakże w żaden sposób nie mogłem sobie z tym poradzić. W początkowych założeniach projekt był prostszy i jako początkujący w programowaniu dałbym sobie z nim radę, no ale po drodze od górnie zostały mi narzucone dodatkowe warunki, na rozwiązanie których nie mam już pomysłu.

Poniżej kod, który napisałem do tej pory, program testowałem na płytce testowej z stąd drobne różnice względem schematu:

'Definiujemy typ mikroprocesora - Mega644P:           'Docelowo Mega8
$regfile = "m644pdef.dat"
'Definiujemy taktowanie kwarcu - 16MHz:
$crystal = 16000000                                         'Docelowo 8MHz
 
'Definiujemy wyjscia:
'Gaz
Config Pinc.1 = Output
'Woda
Config Pinc.2 = Output
'Blad
Config Pinc.3 = Output
 
'Definiujemy wejscia:
'Radiowe
Config Pinc.5 = Input
 
'Czasowe:
'T = 15min = 900s
Config Pinb.0 = Input
'T = 30min = 1800s
Config Pinb.1 = Input
'T = 45min = 2700s
Config Pinb.2 = Input
'T = 60min = 3600s
Config Pinb.3 = Input
 
'Przypisujemy nazwy portom:
'Wejscia:
G1 Alias Portc.1
W1 Alias Portc.2
B1 Alias Portc.3
 
'Wejscia:
R1 Alias Portc.5
T15 Alias Portb.0
T30 Alias Portb.1
T45 Alias Portb.2
T60 Alias Portb.3
 
'Podciągamy wejscia wewn. rezystorem:
Set T15
Set T30
Set T45
Set T60
 
 
'Główny program:
 
'Pętla P0
P0:
Do
'Ustawiamy stan wyjść:
Reset G1 : Reset W1 : Set B1
 
'Jeżeli przycisk R1 wciśnięty przejdź do Pętli P1:
If R1 = 0 Then
Gosub P1
'Jeżeli nie to wróć do początku Pętli P0:
Else
Gosub P0
End If
Loop
'Koniec Pętli P0.
 
'Pętla P1:
P1:
Do
'Jeżeli R1 dalej jest wciśnięty Włącz W1 i G1:
If R1 = 0 Then
Set W1 : Set G1
'Gdy przycisk zwolniony, to wyłącz G1 i przejdź do Pętli P2
Else
Reset G1
Gosub P2
End If
Loop
'Koniec Pętli P1
 
'Pętla P2:
P2:
Do
 
M0:
'Jeżeli przęłącznik czasu jest ustawiony na 15, to czekaj 900s wyłącz W1 i wróć
'do początku Pętli P0:
If T15 = 0 Then
Wait 900
Reset W1
Gosub P0
'Jeżeli przełącznik nie jest na tej pozycji skocz do pod Pętli M1
Else
Gosub M1
End If
 
M1:
'Jeżeli przęłącznik czasu jest ustawiony na 30, to czekaj 1800s wyłącz W1 i wróć
'do początku Pętli P0:
If T30 = 0 Then
Wait 1800
Reset W1
Gosub P0
'Jeżeli przełącznik nie jest na tej pozycji skocz do pod Pętli M2
Else
Gosub M2
End If
 
M2:
'Jeżeli przęłącznik czasu jest ustawiony na 45, to czekaj 2700s wyłącz W1 i wróć
'do początku Pętli P0:
If T45 = 0 Then
Wait 2700
Reset W1
Gosub P0
'Jeżeli przełącznik nie jest na tej pozycji skocz do pod Pętli M3
Else
Gosub M3
End If
 
M3:
'Jeżeli przęłącznik czasu jest ustawiony na 60, to czekaj 3600s wyłącz W1 i wróć
'do początku Pętli P0:
If T60 = 0 Then
Wait 3600
Reset W1
Gosub P0
'Jeżeli przełącznik nie jest na tej pozycji czekaj 1s, włacz B1 i skocz
'do pod Pętli M0:
Else
Wait 1
Reset G1
Reset W1
Reset B1
Gosub M0
End If
 
Loop
Return
'Koniec Pętli P2.
 
End
'Koniec Programu