bene inizio a rompervi le bolas in attesa che mi arrivi il programmatore

dunque partendo dal codice sorgente che mi sembra gia una buona base di partenza, allego il codice con le modifiche, che secondo me fanno quello che voglio io cioe ritardare di 5 secondi la rotazione del servo dopo la pressione del pulsante, il problema e poi come dico ritorna alla posizione iniziale dopo che ha fatto la rotazione di tot gradi impostato dal trimer?
Codice:
int tempo, gradi, x, y, valore, gradiRil, i;
void main (void){
ANSEL = 1; // configurazione input alanogico
TRISIO = 0b001011; // gpio0-1-3 in input gpio2 in output
valore = 40;
i=0;
while (1){
inizio:
tempo = ADC_READ(0);
delay_ms(5);
gradiRil = ADC_READ(1);
delay_ms(5000); //qui dico di ritardare 5secondi dopo la pressione tasto
tempo = (tempo+50)/50;
gradiRil = (gradiRil+50)/7;
for(x=0;x<tempo;x++){
for(y=0;y<valore;y++){
gpio.f2 = 1;
}
gpio.f2 = 0;
delay_ms(20);
}
if(gpio.f3)
i=i^1;
while(gpio.f3){}
if(i)
gradi = gradiRil;
else
gradi = 40;
if(valore==gradi)
goto inizio;
if(valore>gradi)
valore--;
else
valore++;
}}