Ramię robota sterowane przerwaniami na Mega 2560

INFORMACJA: Poniższy post nie wydostał się z Poczekalni :(

Chciałbym sterować ramieniem robota poprzez Arduino Mega 2560. Sterowanie działa względnie prawidłowo kiedy okres pracy poszczególnych silników określam czasem. Jest to jednak kłopotliwe ponieważ pod różnym obciążeniem silników dosyć trudno precyzyjnie te czasy poustawiać. Dlatego zastosowałem enkodery na silnikach aby zliczać impulsy i na tej podstawie nimi sterować. Program dodatkowo wyświetla na LCD, zadane impulsy i zliczane z enkodera. Działa wszystko “prawie” idealnie, to znaczy zadziała mi ‘blok” tylko na jednym przerwaniu z pinu 2. Odliczy impulsy, silnik A się zatrzyma i nie przechodzi do kolejnego kroku. Na pewno jest błąd w programie, ale nie potrafię wykombinować gdzie. Program dla dwóch silników wygląda tak:

#include <LiquidCrystal.h>
LiquidCrystal lcd(12, 11, 7, 6, 5, 4);
word steps_1;
word steps_2;

//motor A ramie
int in1 = 23;
int in2 = 24;
int enA = 8;
//motor B szczeki
int test_1 = 25;
int test_2 = 26;
int enB = 9;

void setup()
{
pinMode(2, INPUT_PULLUP);
pinMode(3, INPUT_PULLUP);
attachInterrupt(0, onStep_2, LOW);
attachInterrupt(1, onStep_1, LOW);
pinMode(test_1, OUTPUT); //pracuje jako wyjscie
pinMode(test_2, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(enA, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
}

void loop()
{
delay(1000);
}

void onStep_1() //krok I
{
static unsigned long lastTime;
unsigned long timeNow = millis();
digitalWrite(test_2, HIGH); //start zamkniete szczeki
digitalWrite(test_1, LOW);
analogWrite(enA, 135); //moc dla ramienia
int A=50; //impulsy dla ramienia A
if (steps_1<A){
if (timeNow – lastTime < 50)
return;
steps_1++;
lcd.begin(16, 2);
lcd.print(“Krok 1: “);
lcd.print(A);
lastTime = timeNow;
lcd.setCursor(0,1);
lcd.print(“licznik: “);
lcd.print(steps_1);}
else
{
analogWrite(enA, 0);
}
return;
}
void onStep_2() //krok II
{
static unsigned long lastTime;
unsigned long timeNow = millis();
digitalWrite(in2, HIGH); //start zamkniete szczeki
digitalWrite(in1, LOW);
analogWrite(enB, 135); //moc dla ramienia B
int B=10; //impulsy dla ramienia B
if (steps_2<B){
if (timeNow – lastTime < 50)
return;
steps_2++;
lcd.begin(16, 2);
lcd.print(“Krok 2: “);
lcd.print(B);
lastTime = timeNow;
lcd.setCursor(0,1);
lcd.print(“licznik: “);
lcd.print(steps_2);}
else
{
analogWrite(enB, 0);
}
}

Na pewno błędy są w programie, ale już kolejny wieczór nie mogę wykombinować gdzie, będę wdzięczny za każdą podpowiedź.

INFORMACJA: Poniższy post nie wydostał się z Poczekalni :(