Jednak, gdy będziemy chcieli napisać program realizujący bardziej skomplikowaną trasę, będzie on zajmował wiele linijek kodu, w którym łatwo się pogubić. Dlatego utworzymy odpowiednie funkcje które będą realizować następujące zadania: jazda do przodu, jazda do tyłu, obrót w prawo, obrót w lewo oraz zatrzymanie (stop).

Aby obracać się w prawo, lewy silnik musi kręcić się do przodu, a prawy do tyłu. W analogiczny sposób realizowany jest obrót w lewo – wtedy silnik prawy musi obracać się do przodu, natomiast lewy do tyłu. Napisane funkcje prezentują się następująco:

void Jedz_do_przodu(int i)
{
  // Lewy silnik
  digitalWrite(6,LOW);
  analogWrite(10,i);
  // Prawy silnik 
  digitalWrite(7,LOW);
  analogWrite(11,i);
}

void Jedz_do_tylu(int i)
{
  // Lewy silnik
  digitalWrite(6,HIGH);
  analogWrite(10,i);
  
  // Prawy silnik 
  digitalWrite(7,HIGH);
  analogWrite(11,i);
}

void Obrot_w_prawo(int i)
{
   // Lewy silnik
  digitalWrite(6,LOW);
  analogWrite(10,i);
  
  // Prawy silnik 
  digitalWrite(7,HIGH);
  analogWrite(11,i);
}

void Obrot_w_lewo(int i)
{
   // Lewy silnik
  digitalWrite(6,HIGH);
  analogWrite(10,i);
  
  // Prawy silnik 
  digitalWrite(7,LOW);
  analogWrite(11,i);
}

void Stop()
{
   // Lewy silnik
  digitalWrite(6,LOW);
  analogWrite(10,0);
  
  // Prawy silnik 
  digitalWrite(7,LOW);
  analogWrite(11,0);
}

Argumentem funkcji jest wartość PWM, która jest przekazywana do funkcji generującej sygnał PWM – analogWrite().

W programie głównym stworzyłem następujący program.

    for(int j=0;j <4; j++)
    {
     Jedz_do_przodu(100);
     delay(1000);
     
     Stop();
     delay(500);
     
     Obrot_w_prawo(50);
     delay(600);
     
     Stop();
     delay(500);
    }

Powyższy kod programu realizuje jazdę podwozia po obwodzie kwadratu. Najpierw robot jedzie przez 1 sekundę (1000 ms) do przodu, następnie zatrzymuje się na 0,5 sekundy (500 ms). Później obraca się w prawo przez 600 ms, taki cykl wykonuje się cztery razy. Czas obrotu należy dobrać doświadczalnie metodą prób i błędów. Pełen program znajduje się w sekcji Plik do pobrania.

Jeżdżenie po „kwadracie” zostało zaprezentowane na poniższym wideo: