W pierwszym programie podwozie Romi Chassis przejedzie 22 cm (jeden obrót koła). Następnie zatrzyma się na 3 sekundy (3000 ms), a następnie wróci na miejsce startu (czyli przejdzie 22 cm do tyłu). To zadanie realizuje to poniższy program (kod programu 1 znajduje się w sekcji do pobrania na końcu artykułu):
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 |
// Romi Chassis – obsługa enkoderów // Program 1 // 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 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) { //Jedz do przodu przez 1440 impulsów (jeden obrot koła, około 22 cm) while(licznik <=1440) { Jedz_do_przodu(20); Serial.print("Licznik: "); Serial.println(licznik); } // Zatrzymanie Stop(); delay(3000); //Jedz do tylu przez 1440 impulsów (jeden obrot koła, około 22 cm) while(licznik>0) { Serial.print("Licznik: "); Serial.println(licznik); Jedz_do_tylu(20); } // Zatrzymanie Stop(); delay(3000); } } 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 powyższym kodzie wykorzystamy funkcje (Jedz_do_przodu(int i) oraz Jedz_do_tyłu(int i)), które napisaliśmy podczas realizacji pierwszego projektu z podwoziem Romi Chassis.
Pierwszym krokiem będzie zadeklarowanie, do których wyprowadzeń będą podłączone kanały A i B enkodera (piny 2 oraz 3):
|
1 2 |
#define PinA 2 #define PinB 3 |
Następnie deklarujemy zmienne potrzebne do pracy enkodera:
|
1 2 |
long licznik = 0; int kierunek = 1; |
Następnie trzy funkcje: Jedz_do_przodu(int i), Jedz_do_tylu(int i) oraz Stop() do poruszania się podwozia oraz jego zatrzymywania.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 |
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 Stop() { // Lewy silnik digitalWrite(6,LOW); analogWrite(10,0); // Prawy silnik digitalWrite(7,LOW); analogWrite(11,0); } |
W stosunku do funkcji użytych w pierwszej części projektu, dodano zmienną kierunek, która określa czy podwozie porusza się do przodu (wartość 1) czy do tyłu (wartość 0). Taka informacja będzie potrzebna do zliczania impulsów z enkodera (o czym w dalszej części artykułu).
Następnie ustawiamy prędkość transmisji portu szeregowego, deklarujemy wyzwalanie przerwań na pinach 2 i 3 oraz ustawiamy linie wyjściowe do sterowania silnikami.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 |
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); } |
Następnie przechodzimy do pętli głównej programu:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 |
void loop() { delay(3000); while(1) { //Jedz do przodu przez 1440 impulsów (jeden obrot koła, około 22 cm) while(licznik <=1440) { Jedz_do_przodu(20); Serial.print("Licznik: "); Serial.println(licznik); } // Zatrzymanie Stop(); delay(3000); //Jedz do tylu przez 1440 impulsów (jeden obrot koła, około 22 cm) while(licznik>0) { Serial.print("Licznik: "); Serial.println(licznik); Jedz_do_tylu(20); } // Zatrzymanie Stop(); delay(3000); } } |
Mamy tutaj dwie pętle while(). W pierwszej z nich podwozie będzie poruszało się do przodu z wartością PWM równą 20, gdy wartość licznika będzie mniejsza niż 1440. Gdy warunek ten nie jest spełniony (wartość licznika będzie większa niż 1440), następuje zatrzymanie konstrukcji na 3 sekundy. W następnej pętli while(), podwozie porusza się do tyłu do momentu, gdy licznik będzie mniejszy niż 0.
Na końcu programu mamy dwie funkcje Kanal_A() oraz Kanal_B(). Są one wywoływanie, gdy wystąpi przerwanie od kanału A lub B enkodera. Gdy kierunek jest równy 1 (konstrukcja porusza się do przodu), wartość licznika jest zwiększana. Natomiast gdy kierunek jest 0 (czyli konstrukcja porusza się do tyłu), wartość licznika jest zmniejszana.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
void Kanal_A() { if (kierunek==1) { licznik++; } else if (kierunek==0) { licznik--; } } void Kanal_B() { if (kierunek==1) { licznik++; } else if (kierunek==0) { licznik--; } } |
Działanie programu pierwszego zostało pokazane na poniższym materiale wideo:
