Określenie drogi za pomocą enkoderów

[Bleebum]

Witam, 

Posiadam: 

• Silniki Pololu HP 50:1
• Enkodery
• Koła Solarbotics RW2

Wzorując się na tym artykule napisałem program: 

// Silnik Pololu HP 50:1 
// Koło Solarbotics RW2. Średnica opony wynosi d = 28 mm, szerokość 17 mm.
// Przebyta odległość = (ilość impulsów na obrót silnika / 360) * obwód koła
// Encoder ticks = (360 / obwód koła) * odległość
// Obwód koła = d * 3,14 = 87,92 mm 
// 36,73 tiki na cm

#include <PID_v1.h>

// ENKODERY
#define PIN_ENCODER_LEFT_A 0
#define PIN_ENCODER_LEFT_B 2
#define PIN_ENCODER_RIGHT_A 1
#define PIN_ENCODER_RIGHT_B 3

int MOTOR_LEFT_ENCODER = 0;
int MOTOR_RIGHT_ENCODER = 0;
float WHEEL_DIAMETER = 3.14 * 2.8;

// SILNIKI
#define PIN_MOTOR_LEFT_ENABLE 11 // PWM
#define PIN_MOTOR_LEFT_PHASE 8   // KIERUNEK
#define PIN_MOTOR_RIGHT_ENABLE 10
#define PIN_MOTOR_RIGHT_PHASE 7

void leftEncoderSpeed()
{
  MOTOR_LEFT_ENCODER++;
}

void rightEncoderSpeed()
{
  MOTOR_RIGHT_ENCODER++;
}
 
void setup()
{  
  Serial.begin(9600);//Initialize the serial port
  pinMode(PIN_MOTOR_LEFT_ENABLE, OUTPUT);
  pinMode(PIN_MOTOR_LEFT_PHASE, OUTPUT);
  pinMode(PIN_MOTOR_RIGHT_ENABLE, OUTPUT);
  pinMode(PIN_MOTOR_RIGHT_PHASE, OUTPUT);

  pinMode(PIN_ENCODER_LEFT_A, INPUT);
  pinMode(PIN_ENCODER_LEFT_B, INPUT);
  pinMode(PIN_ENCODER_RIGHT_A, INPUT);
  pinMode(PIN_ENCODER_RIGHT_B, INPUT);

  attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_LEFT_A), leftEncoderSpeed, RISING);
  attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_RIGHT_A), rightEncoderSpeed, RISING);
  
  // Funkcja attachInterrupt() definiuje sposób obsługi przerwania.
  // attachInterrupt(interrupt, function, mode) 
  // interrupt - numer przerwania
  // function - funkcja uruchamiana, gdy nastąpi przerwanie. Funkcja ta nie może mieć parametrów i nie może zwracać żadnej wartości.
  // mode - określa, kiedy przerwanie ma być wyzwolone. Możliwe są cztery warianty:
      // LOW - przerwanie wywołane, gdy stan wejścia jest w stanie LOW (0V)
      // CHANGE - przerwanie wywołane przy zmianie stanu wejścia
      // RISING - przerwanie wywołane przy zmianie stanu z niskiego (0V) na wysoki (5V)
      // FALLING - przerwanie wywołane przy zmianie stanu wejścia z wysokiego (5V) na niski (0V).
}
 
void loop()
{   

    double tickGoal = (360/WHEEL_DIAMETER)*10;

    if( MOTOR_RIGHT_ENCODER < tickGoal )
    {
      analogWrite(PIN_MOTOR_LEFT_ENABLE, 40);
      digitalWrite(PIN_MOTOR_LEFT_PHASE, 0);  

      analogWrite(PIN_MOTOR_RIGHT_ENABLE, 40);
      digitalWrite(PIN_MOTOR_RIGHT_PHASE, 1);
    } else
    {
      analogWrite(PIN_MOTOR_LEFT_ENABLE, 0);
      digitalWrite(PIN_MOTOR_LEFT_PHASE, 1); 

      analogWrite(PIN_MOTOR_RIGHT_ENABLE, 0);
      digitalWrite(PIN_MOTOR_RIGHT_PHASE, 1);

      //Serial.println("LEFT: "); 
      //Serial.println(MOTOR_LEFT_ENCODER);
      //Serial.println("RIGHT: "); 
      //Serial.println(MOTOR_RIGHT_ENCODER); 
    }
       

}

Ale dystans nie jest poprawnie obliczany. Czy istnieje jakiś inny sposób aby wyznaczyć przebytą drogę?  Chciałbym aby robot przejechał 10 cm.  

[marek1707]

Gdyby nie było tego fantastycznego artykułu kolegi deshipu:

To pewnie zadałbym Ci wiele pytań a tak mam tylko kilka:

19 godzin temu, Bleebum napisał:

dystans nie jest poprawnie obliczany

A jak jest? Czy w ogóle masz jakieś wyniki inne niż zero? Czy liczby są zawsze o jakiś współczynnik za małe lub za duże? A może o stałą wartość? Czy możesz pokazać kilka przykładowych liczb i zmierzonych przy tym odległości? Czy zacząłeś od podstaw czyli od pokręcania kołem ręcznie, powoli i czy wynik z enkodera był wtedy OK? Po jednym obrocie? Po dziesięciu? Po pięćdziesięciu? A gdy kręciłeś szybciej niż robią to silniki to też było OK?

19 godzin temu, Bleebum napisał:

WHEEL_DIAMETER = 3.14 * 2.8

Co to za wielkość? Koła wg sklepu mają średnicę 31.2 więc ani to średnica ani.. obwód - bo chyba to miałeś na myśli mnożąc coś przez Pi.

A teraz poświęć kilka minut na przeczytanie rzeczonego artykułu, potem jeszcze raz to samo dla utrwalenia nowej wiedzy i do roboty.

[Treker (Damian Szymański)]

19 godzin temu, Bleebum napisał:

Ale dystans nie jest poprawnie obliczany. Czy istnieje jakiś inny sposób aby wyznaczyć przebytą drogę?  Chciałbym aby robot przejechał 10 cm.  

Jeśli chcesz precyzyjnie sterować robotem to koniecznie zapoznaj się z artykułem: Sterowanie robotem mobilnym klasy (2,0). Znajdziesz w nim wskazówkę jak zrobić to w bardzo dobry sposób.