W drugim programie nasza platforma Romi Chasssis będzie obracała się w prawo o 90 stopni do momentu zrobienia pełnego obrotu, następnie będzie obracać się w lewo aż powróci do pozycji startowej.

Jednak przed przystąpieniem do pisania programu musimy wykonać kilka obliczeń. Wiemy, że jeden obrót koła (1440 impulsów), przemieści naszą konstrukcje o 22 cm. Aby obrócić się o 90 stopni, podwozie musi pokonać 1/4 obwodu okręgu. Średnica tego okręgu to odległość między kołami. Dla podwozia Romi Chassis wynosi ona około 14 cm (140 mm). Obliczamy obwód koła, po którym będzie poruszała się konstrukcja:

Obwód_koła=2 · π · R

Obwód_koła=2 · π · (140/2)

Obwód_koła=2 · π · 70

Obwód_koła = 440 mm

Obwód okręgu, po którym będzie poruszało się podwozie wynosi 440 mm. My jednak chcemy, aby nasza platforma jezdna obracała się o kąt 90 stopni. Czyli musi ona pokonać 1/4 pełnego obwodu tego okręgu czyli 110 mm. Teraz obliczymy, ile impulsów z enkodera będzie potrzeba, aby pokonać taki dystans (110 mm) obliczymy to z prostej proporcji:

1440 impulsów = 220 mm

x impulsów =110 mm

czyli

220 · x = 1440 · 110

x = 720 impulsów

Z obliczeń wyszło, że aby obrócić się o 90 stopni należy wykonać 720 impulsów.

Napiszemy teraz program, który będzie obracał naszym podwoziem o 90 stopni, następnie zatrzyma je na 2 sekundy. Cykl ten będzie powtarzany do momentu, aż podwozie obróci się o 360 stopni (pełen obrót). Następnie podwozie ma się obrócić w przeciwnym kierunku, aż do momentu powrotu do pozycji startowej (obrót o 360 stopni).

Kod programu drugiego znajduje się poniżej (kod programu 2 znajduje się w sekcji do pobrania na końcu artykułu).

// Romi Chassis – obsługa enkoderów
// Program 2
// Autor: Patryk Mądry 
// Mikrokontroler 2017 r.

#define PinA 2
#define PinB 3

long licznik = 0;
int kierunek = 1;

void Jedz_do_przodu(int i)
{
  kierunek = 1;

  // Lewy silnik
  digitalWrite(6,LOW);
  analogWrite(10,i);

  // Prawy silnik 
  digitalWrite(7,LOW);
  analogWrite(11,i);
}

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

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

void Obrot_w_lewo(int i)
{
   kierunek = 0;
   // 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);
}

void setup() {
  Serial.begin(9600);
  pinMode(PinA,INPUT); 
  pinMode(PinB,INPUT); 

  attachInterrupt(0, Kanal_A, CHANGE);
  attachInterrupt(1, Kanal_B, CHANGE);
   
  pinMode(10,OUTPUT);
  pinMode(11,OUTPUT);

  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
}

void loop() {

  delay(3000);

  while(1)
  {
      for(int j=0; j<4;j++)
      {
        do
        {
          Serial.print("Licznik: "); 
          Serial.println(licznik);
          Obrot_w_prawo(20);
        }
        while(licznik <= 690);
      
        Stop();
        licznik=0;

        delay(2000);
      } 
    
      for(int j=0; j<4;j++)
      {
        do
        {
          Serial.print("Licznik: "); 
          Serial.println(licznik);
          Obrot_w_lewo(30);
        }

        while(licznik>=-690);
      
        Stop();
       
        licznik=0;
    
      } 
    
      delay(5000);
  } 
}

void Kanal_A()
{
  if (kierunek==1)
  {
    licznik++; 
  }
  else if (kierunek==0)
    {
      licznik--; 
    }
}

void Kanal_B()
{
   if (kierunek==1)
  {
    licznik++; 
  }
  else if (kierunek==0)
    {
      licznik--; 
    }
}

W stosunku do poprzedniego kodu programu musimy dołączyć funkcje do obrotu – Obrot_w_prawo(int i) oraz Obrot_w_lewo(int 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);
}

Deklaracje zmiennych oraz ustawienia zostają bez zmian w stosunku do pierwszego programu. Rożni się natomiast pętla główna programu.

void loop() {

  delay(3000);

  while(1)
  {
      for(int j=0; j<4;j++)
      {
        do
        {
          Serial.print("Licznik: "); 
          Serial.println(licznik);
          Obrot_w_prawo(20);
        }
        while(licznik <= 690);
      
        Stop();
        licznik=0;
      
        delay(2000);
      } 
    
      for(int j=0; j<4;j++)
      {
        do
        {
          Serial.print("Licznik: "); 
          Serial.println(licznik);
          Obrot_w_lewo(30);
        }
        while(licznik>=-690);
      
        Stop();
       
        licznik=0;
      } 
    
    delay(5000);
  } 
}

Po włączeniu zasilania konstrukcja czeka 3 sekundy, a następnie jest wykonywana pętla for(), która realizuje 4 obroty po 90 stopni. Następnie, w kolejnej pętli do…while(), jest realizowany obrót w przeciwnym kierunku (przeciwnie do kierunek wskazówek zegara), aż do momentu wykonania obrotu 360 stopni wokół własnej osi.

Obliczona przez nas wartość (720 impulsów) może powodować obrót o więcej niż 90 stopni. Może to być spowodowane przybliżeniami w obliczeniach, luzami na przekładniach czy poślizgiem kół podczas obrotu.

Działanie programu zostało pokazane na wideo: