BAscom AVR - Jak poprawnie używać funkcji gosub dla zmiany pętli?

Witam
oto mój kawałek kodu

$crystal = 8000000


Dim A As Byte
Dim B As Byte
Dim C As Byte
Dim D As Byte
Dim E As Byte
B = 3
C = 2
D = 5
E = 10

Config Portc = &B10001111
Portc = &B01111111

Disable Interrupts
If Pinc.6 = 0 Then Gosub Ppp
If Pinc.6 = 1 Then Gosub Tttt
Ppp:
Do
Portc.0 = 0
For A = 1 To B
Wait 1
Next
Portc.0 = 1
For A = 1 To C
Wait 1
Next
Loop
End

Ttt:
Do
Portc.0 = 0
For A = 1 To D
Wait 1
Next
Portc.0 = 1
For A = 1 To E
Wait 1
Next
Loop
End
Niedziała mi funkcja gosub.Tzn działa jedna pętla .Jak zmienie stan na wejściu Pinc.6 wtedy nie zmienia się do drugiej pętli.Chodzi mi o to że jak na wejściu pinc.6 jest jedynka wtedy wykonuje się np pętla ttt a jak jest zero wtedy wykonuje się petla ppp.
Co jest źle?
Dziękuje za pomoc

Program będzie działał w zależności od stanu rejestru Pinc.6 po włączeniu zasilania lub po resecie uC. Jeśli na pinie portu C.6 będzie 0 wtedy program wpadnie w pętlę po Ppp: i już z niej nie wyjdzie do końca świata, a co najmniej do resetu lub wyłączenia zasilania. Jeśli będzie 1 to to samo zrobi z pętlą po Tttt:

Jak rozumiem chcesz za pomocą Pinc.6 sterować czy będzie wykonywał się pierwszy podprogram czy drugi. Napisz więc tak:



W ten sposób uC sprawdzi stan wejścia Pinc.6 po każdym wykonaniu danego podprogramu i następnie wykona się odpowiedni ustawiony wcześniej stanem podanym na Pinc.6

Witam
Dzięki wszystko działa OK!!
Niemogłem się kapnąć o co biega.
Mam jeszcze jedno pytanie jak najszybciej zrobić ustawianie wartości B,C,D,E na trzech przyciskach ?NP SET i góra dół .
Dziękuje za pomoc