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).
|
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 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 |
// 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).
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 |
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.
|
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 |
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:
