C/C++ - Programmazione ascensore in Linguaggio C

Come da titolo devo programmare un ascensore elettrico in linguaggio C
il programma viene inserito in arduino 2009
vengono utilizzati tre sensore reed(ingressi) uno per piano
tre pulsanti a pressione(ingressi) uno per piano
due motori( due uscite ciascun motore)
tre piani di lavoro dell'ascensore: piano terra, primo piano, secondo piano

funzionamento: l'ascensore dovrebbe salire o scendere in base al piano in cui si trova (indicato dai sensori) e in base al piano da cui viene chiamato (scelto dal pulsante premuto). Il motore principale fa andare l'ascensore al piano richiesto fermandosi quando il sensore del piano desiderato si setta con l'arrivo della cabina, dopo di che le porte dell'ascensore vengono aperte dal motore secondario, quello della porta, che in seguito le richiuderà.

piedini utilizzati:
pinc0 = sensore piano terra
pinc1 = sensore primo piano
pinc2 = sensore secondo piano
pinc3 = tasto piano terra
pinc4 = tasto primo piano
pinc5 = tasto secondo piano
pinb3 = input 1 driver porta
pinb4 = input 2 driver porta
pinb5 = input 1 driver motore principale
pinb6 = input 2 driver motore principale
ho già buttato giù il programma ma ho un problema: i motori non girano quando vengono premuti i pulsanti di richiamo dell'ascensore posizionati su ogni piano, mentre nel caso in cui l'ascensore si trovi al piano terreno (e quindi sensore piano terreno settato) i motori iniziano a girare per conto loro
qualcuno che riesce ad aiutarmi?

/*
 * pr.c
 *
 * Created: 10/05/2014 09:25:48
 *  Author: utente
 */ 

#include <avr/io.h>
#include <avr/interrupt.h>

#define TOGGLEBIT(ADDRESS,BIT) (ADDRESS^=(1<<(BIT)))
#define CHECKBIT(ADDRESS,BIT) (ADDRESS&=(1<<(BIT)))
#define SETBIT(ADDRESS,BIT) (ADDRESS|=(1<<(BIT)))
#define CLEARBIT(ADDRESS,BIT) (ADDRESS&=~(1<<(BIT)))

#define sali 1
#define scendi 2
#define fermo 0

#define aperta 1
#define chiusa 0

char tasto_primo_piano_old=0; //vecchia lettura tasto primo piano
char tasto_secondo_piano_old=0; //vecchia lettura tasto secondo piano
char tasto_piano_terra_old=0; //vecchia lettura tasto piano terra

char piano_ascensore =128; // livello del piano; 3 valori possibili: 0,1,2 se non siamo al piano il valore è 128
char piano_selezionato =128;

char stato_porta = chiusa; //indica se la porta è aperta o meno 
char apri_porta=0;
char chiudi_porta=0;
char conteggio=0;
char motore_ascensore =0;


void init(void)
{
	DDRB=0xFF;//uscite (motori)
	PORTB=0x00;
	DDRD=0xFF;
	DDRC=0x00; //ingressi (sensori piani e tasti di chiamata)
	TCCR1A=0x00;  //normal mode
	TCCR1B=0x05;  //prescaler a 1024
	TIMSK1|=0x01;  //abilitazione interrupt timer1
	TCNT1H=0xC2;
	TCNT1L=0xF6;
}
//si suppone che non si possano premere contemporaneamente più tasti
ISR(TIMER1_OVF_vect,ISR_BLOCK)//controllo ogni 1 ms lo stato dei tasti e l'apertura della porta
{
	TCNT1H=0xC2;
	TCNT1L=0xF6;
	if((CHECKBIT(PINC,PINC4) == 1) && (tasto_primo_piano_old == 0) && (piano_selezionato == 128))
	//se il tasto primo piano è premuto e prima non lo era
	//fare il confronto con piano 128 serve per far sì che una volta scelto uno dei 3 piani possibili
	//questi if non siano più effettuati e così servo solo il piano scelto per la prima volta
	{
		piano_selezionato = 1;
		tasto_primo_piano_old=1;
	}
	else if((CHECKBIT(PINC,PINC5) == 1) && (tasto_secondo_piano_old == 0) && (piano_selezionato == 128))
	//se il tasto secondo piano è premuto e prima nn lo era
	{
		piano_selezionato=2;
		tasto_secondo_piano_old=1;
	}
	else if((CHECKBIT(PINC,PINC3) == 1) && (tasto_piano_terra_old == 0) && (piano_selezionato == 128))
	//se il tasto piano terra è premuto e prima nn lo era
	{
		piano_selezionato=0;
		tasto_piano_terra_old=1;
	}
	else
	{
		tasto_primo_piano_old = CHECKBIT(PINC,PINC4);
		tasto_secondo_piano_old = CHECKBIT(PINC,PINC5);
		tasto_piano_terra_old = CHECKBIT(PINC,PINC3);
	}
	
	if ((stato_porta == aperta) && (chiudi_porta==1)) //se la porta è aperta e devi chiuderla
	{	
		conteggio = 500;
		if(conteggio > 0) 
		{
			conteggio--;//vedi quanto tempo impieghi ad aprire la porta
			SETBIT(PORTB,PINB2);//apri la porta
			CLEARBIT(PORTB,PINB3);
		} 
		else
		{
			CLEARBIT(PORTB,PINB2);//terminato il conteggio la porta rimane ferma aperta
			CLEARBIT(PORTB,PINB3);
			chiudi_porta = 0;
			stato_porta = chiusa;//ormai la porta è chiusa
		} 
	} 
	else if ((stato_porta==chiusa) && (apri_porta==1))//se la porta è chiusa e devo aprirla
	{	
		conteggio = 500;
		if(conteggio > 0) 
		{
			conteggio--;//vedi quanto tempo impieghi a chiudere la porta
			CLEARBIT(PORTB,PINB2);//chiudi la porta
			SETBIT(PORTB,PINB3);
		} 
		else
		{
			CLEARBIT(PORTB,PINB2);//terminato il conteggio la porta rimane ferma chiusa
			CLEARBIT(PORTB,PINB3);
			apri_porta = 0;//ormai la porta è aperta
			stato_porta = aperta;
		} 
	} 
	
	switch (motore_ascensore)
	{
		case fermo:
			CLEARBIT(PORTB,PINB4);//ascensore fermo
			CLEARBIT(PORTB,PINB5);
			break;
		case sali :
			SETBIT(PORTB,PINB4);//ascensore sale
			CLEARBIT(PORTB,PINB5);
			break;
		case scendi :
			CLEARBIT(PORTB,PINB4);//ascensore scende
			SETBIT(PORTB,PINB5);
			break;
	}
}		
		
int main(void)
{
	init();
	sei();
	while(1)
	{		
		if		(CHECKBIT(PINC,PINC0)==1)	piano_ascensore = 0; 
		else if (CHECKBIT(PINC,PINC1)==1)	piano_ascensore = 1;
		else if (CHECKBIT(PINC,PINC2)==1)	piano_ascensore = 2;
		else								piano_ascensore = 128;//sono fra 2 piani
		
		switch(piano_selezionato)
		{
			case 0://se seleziono il piano terra
				if ((piano_ascensore == 1) || (piano_ascensore == 2))//se sono al primo o la secondo piano
				{
					if (stato_porta == aperta)	chiudi_porta = 1;
					else						motore_ascensore = scendi;
					//appena la porta è chiusa l'ascensore scende
				}
				else if (piano_ascensore == 0)
				{
					motore_ascensore = fermo;//l'ascensore sta fermo
					if (stato_porta == chiusa)	apri_porta = 1;//se la porta è chiusa la apro
					else						piano_selezionato = 128;//ho gestito la richiesta di cambiamento piano
				}
				break;
			case 1://se seleziono il primo piano 
				if (piano_ascensore == 2)//se sono al secondo piano
				{
					if (stato_porta == aperta)	chiudi_porta = 1;
					else						motore_ascensore = scendi;
					//appena la porta è chiusa l'ascensore scende
				}
				else if (piano_ascensore == 0)
				{
					if (stato_porta == aperta)	chiudi_porta = 1;
					else						motore_ascensore = sali;
					//appena la porta è chiusa l'ascensore sale
				}
				else if (piano_ascensore == 1)
				{
					motore_ascensore = fermo;//l'ascensore sta fermo
					if (stato_porta == chiusa)	apri_porta = 1;//se la porta è chiusa la apro
					else						piano_selezionato = 128;//ho gestito la richiesta di cambiamento piano
				}
				break;
			case 2://se scelgo il secondo piano 
				if ((piano_ascensore == 1) || (piano_ascensore == 0))//se sono al primo o al piano terra
				{
					if (stato_porta == aperta)	chiudi_porta = 1;
					else						motore_ascensore = sali;
					//appena la porta è chiusa l'ascensore sale
				}
				else if (piano_ascensore == 2)
				{
					motore_ascensore = fermo;//l'ascensore sta fermo
					
					if (stato_porta == chiusa)	apri_porta = 1;//se la porta è chiusa la apro
					else						piano_selezionato = 128;//ho gestito la richiesta di cambiamento piano
				}
				break;
			default:
				break;
		}
	}
}