[C] Arduino, program - proźba o pomoc

[cesiek]

Witam wszystkich , mam prożbę o pomoc w przeróbce programu do mojego robota, w założeniu ma on jechać do przodu, zatrzymywać się na około 3-4 sekundy po zobaczeniu przeszkody , zmienić kierunek.Problem jest taki że napęd jest zrealizowany na jednym silniku, a czujnik podczerwieni znajduje się na obracanej platformie( która powinna się się obrócić z w/w czujnikiem) którą napędza drugi silnik.Chodzi o to aby czujnik współpracował z jednym , jak i z drugim silnikiem , nie wiem jak to zrobić programowo.W podanym szkicu oba silniki występują jako napędowe . Podaje szkic programu do przeróbki i liczę na pomoc i z góry dziękuję.

int pwm1 = 6;  
int pwm2 = 5;  
int kierunek1 = 7;
int kierunek2 = 4;
int buzzPin =  3;    //Connect Buzzer on Digital Pin3
int czujnik = 2;
int dioda= 4;           
boolean MA = false;

void Silnik1(int predkosc, boolean kierunek)
       {
        analogWrite(pwm1, predkosc); //set pwm control, 0 for stop, and 255 for maximum speed
        if(kierunek) 
           digitalWrite(kierunek1,HIGH);    
        else
           digitalWrite(kierunek1,LOW);    
       }  

void Silnik2(int predkosc, boolean kierunek)
       {
         analogWrite(pwm2, predkosc);
        if(kierunek)
         digitalWrite(kierunek2,HIGH);    
       else
         digitalWrite(kierunek2,LOW);    
       }  

void setup() 
{ 
   int i;
   for(i=4;i<=7;i++)
   pinMode(i, OUTPUT); 
   pinMode(dioda, OUTPUT);
   pinMode(czujnik, INPUT);
   pinMode(buzzPin, OUTPUT);
}
void loop ()
{
  Silnik1(120, false);      
  Silnik2(120, true);
  if (!digitalRead(czujnik)) {        
                 Silnik1(110, true); 
                 Silnik2(120, false);
                 delay(200);
                 Silnik1(100,false); 
                 Silnik2(100,false);
                 delay (250);
 digitalWrite(buzzPin, HIGH);                 
 delay(1);
 digitalWrite(buzzPin, LOW); 
 delay(1);        
}
}