spis treści - lixfactory.com programowania lixbot racer.pdf · programowanie w scratch polega na...
TRANSCRIPT
S t r o n a | 2
Spis treści Wstęp ....................................................................................................................................................... 3
Co powinieneś wiedzieć? ......................................................................................................................... 3
Bezpieczeństwo ........................................................................................................................................ 4
Przygotowanie środowiska pracy ............................................................................................................. 5
Instalacja i konfiguracja środowiska mBlock ............................................................................................ 7
Podstawy programowania w Scratch ..................................................................................................... 10
Pierwsze kroki .................................................................................................................................... 10
Miganie diodą ..................................................................................................................................... 10
Sterowanie servo ................................................................................................................................ 13
Sterowanie silnikiem .......................................................................................................................... 15
Pomiar odległości ............................................................................................................................... 17
Wykrywanie linii ................................................................................................................................. 18
Od Scratch’a do C ................................................................................................................................... 20
Instalacja i konfiguracja środowiska Arduino ......................................................................................... 24
Programowanie w C ............................................................................................................................... 28
Pierwsze kroki .................................................................................................................................... 28
Komunikacja z komórką za pośrednictwem modułu bluetooth ........................................................ 28
S t r o n a | 3
Wstęp PROPOX to firma z wieloletnią tradycją. Stoją za nami lata doświadczeń i
zadowoleni klienci zarówno z Polski jak i z zagranicy. Zapewniamy
bezproblemową i kompleksową obsługę w jak najkrótszym czasie. Nasi specjaliści
gotowi są w każdej chwili podzielić się swoim doświadczeniem, czy pomóc
rozwiązać ewentualny problem. Jesteśmy pasjonatami i dlatego jesteśmy
najlepsi!
Systematycznie rozszerzamy naszą ofertę, by wypełniać motto naszej firmy -
"Many Ideas One Solution". Pragniemy, by nasze produkty stanowiły kompletny
zestaw narzędzi wykorzystywany zarówno przez profesjonalnych konstruktorów,
jak i hobbystów, skracając czas i obniżając koszty projektowania nowych
systemów.
Firma Propox Sp. z o.o.
Co powinieneś wiedzieć? Projekt L I X B O T oparty jest na licencji GPL, która pozwala na darmowe
rozpowszechnianie i udoskonalanie urządzenia. Ograniczeniem co do jego
możliwości jest jedynie nasza wyobraźnia.
Jako firma posiadająca w sprzedaży zestaw części do projektu i jednocześnie
pomysłodawcy całego przedsięwzięcia oferujemy także pełne wsparcie techniczne
i pomoc w rozwijaniu konstrukcji w formie forum: http://forum.propox.com
oraz strony internetowej, która jest platformie poświęcona http://lixbot.com.
Podstawowy zestaw opiera się na module ARDUINO UNO, który pozwala na łatwe
programowanie dla osób, które dopiero się go uczą, bądź chcą się nauczyć.
Oprócz tego dołączony jest także moduł portów, czujnik ultradźwiękowy, moduł
komunikacji Bluetooth, czujnik IR, 3 czujniki odbiciowe i sterownik silników.
Teraz pozostaje Nam jedynie życzyć dobrej zabawy i "wielu błędów podczas
kompilacji programów"! :)
Pozdrawiamy,
Propox
.........a teraz do dzieła!!!
S t r o n a | 4
Bezpieczeństwo 1. Urządzenie posiada deklarację zgodności CE, potwierdzoną
certyfikatem. Deklaracja jest dostępna na stronie internetowej
http://propox.com
2. Produkt jest w pełni bezpieczny. Przeznaczony do użytku osób
powyżej 16 roku życia.
3. W przypadku awarii lub uszkodzenia w układzie elektrycznym lub
mechanicznym urządzenia, a w przypadku jakichkolwiek wątpliwości
związanych ze zrozumieniem komunikatu o niesprawności lub
określeniem sposobu jej usunięcia, natychmiast wyłącz zasilanie i
skontaktuj się z serwisem.
To urządzenie może być wykorzystywane na terenie wszystkich państw
UE. W miejscach użytkowania urządzenia należy przestrzegać przepisów
krajowych i lokalnych.
S t r o n a | 5
Przygotowanie środowiska pracy Do pracy potrzebujesz jednego robota z serii LIXBOT: LIXBOT Racer
ROBOT lub LAZY BALL ROBOT oraz
Komputer z systemem operacyjnym Windows,
Kabel USB,
Środowisko mBlock do programowania w Scratch,
Środowisko Arduino do programowania w C.
Połączenia elektroniczne przygotuj zgodnie ze schematem poniżej.
Zadbaj o właściwe podłączenie szczególnie pinów PWM gdyż biblioteki
arduino korzystają z wewnętrznych zegarów, które także są używane do
obsługi PWM. Pamiętaj, że moduł Bluetooth wymaga dzielnika
rezystancyjnego ponieważ pracuje on na poziomach 3,3V.
Nr pinu Arduino Czujnik/Płytka
PIN0 RX Bluetooth TX
S t r o n a | 6
PIN1 TX Bluetooth RX
PIN2 TX HC-SR04 Trig
PIN3 TX L298N - ENA
PIN4 RX HC-SR04 Echo
PIN5 RX Line Follower1 signal
PIN6 TX L298N - IN1
PIN7 TX L298N – IN2
PIN8 TX L298N – IN3
PIN9 TX L298N – IN4
PIN10 RX Servo signal
PIN11 TX L298N – ENB
PIN12 RX Line Follower1 signal
PIN13 RX Line Follower1 signal
Timer0 używany jest w Arduino do obsługi funkcji delay() i millis().
Timer1 używany jest w Arduino do obsługi funkcji dotyczących servo.
Timer2 używany jest w Arduino do obsługi funkcji tone().
Piny PWM wykorzystują odpowiednio:
- Pin 5 i 6 - Timer0,
- Pin 9 i 10 – Timer1,
- Pin 11 i 3 – Timer2.
Jeżeli nie jesteś w posiadaniu któregoś z robotów możesz zakupić je w
Naszym sklepie internetowym, gdyż będą Ci one potrzebne do nauki
programowania.
S t r o n a | 7
Instalacja i konfiguracja środowiska mBlock Program możesz pobrać ze strony http://www.mblock.cc/download
Zainstaluj pobrany program. Instalacja to standardowy proces, do którego jesteś przyzwyczajony
działając w swoim systemie operacyjnym.
Po uruchomieniu programu napotkasz na okno jak na poniższym rysunku.
Aby skonfigurować środowisko mBlock musisz wybrać typ programowanej płytki. W naszym
przypadku jest to Arduino UNO. Wybierz z menu: „Płytki->Arduino UNO”.
S t r o n a | 8
Teraz jesteś gotowy do instalacji w swoim środowisku drivera’a potrzebnego do obsługi i
programowania płytki Arduino. W tym celu wybierasz z Menu opcje: „Połącz->Zainstaluj Arduino
driver”.
Kolejny krok to wybranie portu szeregowego do którego podłączyłeś płytkę Arduino. Z menu wybierz:
„Połącz->Port szeregowy->TWÓJ PORT”.
S t r o n a | 9
Teraz jesteś gotowy do napisania swojego pierwszego programu !!!
S t r o n a | 10
Podstawy programowania w Scratch
Pierwsze kroki Programowanie w Scratch polega na „przeciąganiu myszką” komend/instrukcji/poleceń (słów tych
używamy zamiennie) z panelu komend do panelu skryptów.
W panelu komend mamy kilka zakładek, które zawierają komendy ogólne, komendy do sterowania
duszkami w panelu obsługi grafiki oraz komendy specyficzne dla płytki arduino.
Komendy dla płytki arduino znajdują się w zbiorze „Roboty”.
Każdy program musi się jakoś zaczynać do tego celu w panelu „Roboty” mamy polecenie „Arduino
program”.
Miganie diodą Pomysł jest taki abyś napisał swój pierwszy program, który spowoduje, że dioda umieszczona na
płytce arduino zacznie migać. Jak to zrobić? A no prosto.
Miganie diody polega na tym, ze przez 1s sią ona pali, a potem przez 1s się ona nie pali – i tak w kółko.
Polecenie do zapalenia diody to „ustaw pin cyfrowy 13 na wysoki”.
Polecenie do zgaszenia diody to „ustaw pin cyfrowy 13 na niski”.
Polecenie do czekania 1s to „czekaj 1s”
S t r o n a | 11
Polecenie do „i tak w kółko” nazywa się pętlą i w mblock brzmi ono „zawsze”.
W mblock wygląda to następująco.
Teraz program trzeba wgrać i robimy to poprzez wybranie z menu polecenia Edytuj->Tryb arduino
S t r o n a | 12
Pojawi się nam wtedy po prawej okienko z kodem w C oraz przyciskami u góry „Załaduj na Arduino”
Naciskamy załaduj i czekamy, aż komputer dokona kompilacji i prześle program do naszej płytki
Arduino.
S t r o n a | 13
Aby wszystko się powiodło upewnij się, że
Masz podłączony kabel z komputera do płytki
W aplikacji mBlock masz podłączoną płytkę i w menu Połącz->Port szeregowy masz wybrany
odpowiedni port.
Postęp programowania będziesz obserwować w okienku na środku oraz w prawym dolnym rogu
aplikacji.
Sterowanie servo Przypomnij sobie jak zostało podłączone serwo do płytki Arduino. Ważne są oczywiście piny sterujące.
W naszym przykładzie to pin PWM nr 10.
Polecenie do ustawienia serva wygląda następująco
„90” to położenie środkowe servo
„0” to skrajne lewe a 180 to skrajne prawe.
S t r o n a | 14
Wypróbujmy następujący program.
Program ten obraca servem od lewego położenia do prawego co krok zdefiniowany na 10 stopni.
Przeanalizujmy instrukcje.
Pojawiła się nowa instrukcja „jeżeli” dostępna w zbiorze kontrola.
Jest to instrukcja, która sprawdza jaki jest wynik warunku. Jak sama nazwa mówi jeżeli warunek
spełniony to zrób to co jest zapisane w środku.
Warunek sprawdzamy za pomocą zielonych komend dostępnych w zbiorze wyrażenia. W naszym
przypadku to warunek równości.
S t r o n a | 15
Pomarańczowy blok to blok reprezentujący zmienną. W naszym przykładzie mamy zdefiniowane 2
zmienne:
Kat_servo
Krok
Zmienne definiujemy w zbiorze dane i bloczki. Do operacji na zmiennych używamy funkcji
ustaw
zmień
Wgraj powyższy program i zobacz jak działa servo.
Sterowanie silnikiem Przypomnij sobie jak zostały podłączone silniki do płytki Arduino. Ważne są oczywiście piny sterujące.
W naszym przykładzie to piny:
PIN3 TX L298N - ENA
PIN6 TX L298N - IN1
PIN7 TX L298N – IN2
PIN8 TX L298N – IN3
PIN9 TX L298N – IN4
PIN11 TX L298N – ENB
Do sterowania silnikami stworzymy funkcje/blok instrukcji, który wygląda następująco:
Blok ten powoduje, że prawe koła jadą do przodu. Jakie instrukcje mamy dostępne?
S t r o n a | 16
Instrukcja ustaw pin PWM
Piny PWM to są takie piny dla których można definiować wypełnienie impulsu.
Wypełnienie przedstawia poniższy rysunek.
W silnikach arduino od wypełnienia zależy prędkość silnika. Im wyższa wartość wypełnienia tym
szybciej silnik się kręci. Możemy ustawiać wartości od 0 do 255. Nasze silniki zaczynają się poruszać z
minimalną prędkością gdy ustawimy wartość 100 a z maksymalną wartość 255.
Dodatkowo musimy wybrać kierunek poruszania się silników. Sterowanie kierunkiem odbywa się na
dwóch bitach. W naszym przykładzie pin 8 ustawiony na wartość „wysoki” a pin 9 na wartość „niski”
powoduje, że silnik kreci się do przodu. Odwrotne ustawienie tzn pin 8 ustawiony na wartość „niski” a
pin 9 na wartość .wysoki” spowoduje, że silnik będzie się kręcił do tyłu.
Wypróbujmy następujący program.
S t r o n a | 17
Program ten porusza naszym robotem cyklicznie do przodu i do tyłu.
Dla porządku zdefiniujmy jeszcze metodę/blok stop, który przyda nam się w kolejnym przykładzie.
Pomiar odległości Przypomnij sobie jak został podłączony czujnik odległości. Ważne są oczywiście piny sterujące. W
naszym przykładzie to
PIN2 TX HC-SR04 Trig
PIN4 RX HC-SR04 Echo
Do zmierzenia odległości wystarcza nam prosta instrukcja
S t r o n a | 18
Umiejąc już mierzyć odległość i poruszać silnikami możemy napisać prosty program, który zatrzymuje
naszego robota przed przeszkodą.
Wykrywanie linii Przypomnij sobie jak został podłączony czujnik zbliżeniowy/wykrywania linii. Ważne są oczywiście
piny sterujące. W naszym przykładzie do nauki wykorzystamy pin 12.
Program polega na tym, że jak czujnik wykryje powierzchnię, która odbija światło (np. biała kartka) to
zapali się dioda na płytce arduino. W przeciwnym wypadku dioda zgaśnie.
Wypróbujmy następujący program.
S t r o n a | 19
Jeżeli wszystkie przykłady uruchomiłeś to WIELKIE BRAWA – umiesz już programować w Scratch.
W następnym rozdziale nauczymy się programować w C.
S t r o n a | 20
Od Scratch’a do C Swoją przygodę z C warto zacząć w środowisku mBlock obserwując jak kod w Scratch’u jest
tłumaczone na kod w C. Aby to zrobić użyj opcji w menu: Edytuj->Tryb arduino”.
Jak to zrobisz to po prawej stronie w środowisku mBlock pojawi się dodatkowy panel z kodem C.
Teraz możesz porównać jak zmienia się kod C jak Ty zmieniasz kod w Scratch’u.
S t r o n a | 21
Napisany przez Ciebie program do migania diodą w Scratch’u wygląda tak:
Przetłumaczony na C wygląda tak:
#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
void setup(){
pinMode(13,OUTPUT);
}
void loop(){
digitalWrite(13,1);
_delay(1);
digitalWrite(13,0);
_delay(1);
_loop();
}
void _delay(float seconds){
long endTime = millis() + seconds * 1000;
while(millis() < endTime)_loop();
}
void _loop(){
}
S t r o n a | 22
Dwie podstawowe funkcje w C to setup() i loop(). Funkcje te wywoływane są przez framework
arduino po zaprogramowaniu płytki. Funkcja setup() wołana jest raz po wystartowaniu procesora (np.
jak włączymy zasilanie i/lub po wciśnięciu reset. Funkcja loop() wywoływana jest cyklicznie w trakcie
działania programu. Oznacza to, że jak funkcja loop() zakończy działanie to framework arduino za
chwilę zawoła ją ponownie. W funkcji setup() przygotowujemy procesor do pracy a w funkcji loop()
umieszczamy kod naszego głównego programu.
Funkcja _delay(float seconds) oraz funkcja _loop() wygenerowana została przez środowisko
mBlock po to aby umożliwić implementację funkcji czekaj dostępnej w Scratch’u.
Spróbuj zdefiniować w Scratch’u blok zapalDiode i zgasDiode.
Kod przetłumaczony na C wygląda następująco:
#include <Arduino.h>
#include <Wire.h>
S t r o n a | 23
#include <SoftwareSerial.h>
void zapalDiode();
void zgasDiode();
void zapalDiode()
{
digitalWrite(13,1);
}
void zgasDiode()
{
digitalWrite(13,0);
}
void setup(){
pinMode(13,OUTPUT);
}
void loop(){
zapalDiode();
_delay(1);
zgasDiode();
_delay(1);
}
void _delay(float seconds){
long endTime = millis() + seconds * 1000;
while(millis() < endTime)_loop();
}
void _loop(){
}
Zwróć uwagę, że w C powstały dwie metody zapalDiode() i zgasDiode(). Metody te wołane są w
funkcji loop().
Po doświadczeniach z konwersją Scratch’a na C jesteś gotowy do instalacji środowiska Arduino i
rozpoczęcia doświadczeń z C.
S t r o n a | 24
Instalacja i konfiguracja środowiska Arduino Program możesz pobrać ze strony http://www.arduino.org/downloads
Zainstaluj pobrany program. Instalacja to standardowy proces, do którego jesteś przyzwyczajony
działając w swoim systemie operacyjnym.
Po uruchomieniu programu napotkasz na okno jak na poniższym rysunku.
Podobnie jak w środowisku mBlock, aby skonfigurować środowisko Arduino musisz wybrać typ
programowanej płytki. W naszym przypadku jest to Arduino UNO. Wybierz z menu: „Narzędzia-
>Płytka->Arduino/Genuino Uno”.
S t r o n a | 25
Kolejny krok to wybranie portu szeregowego do którego podłączyłeś płytkę Arduino. Z menu wybierz:
„Narzędzia->Port ->TWÓJ PORT”.
S t r o n a | 26
W arduino dostępne jest wiele przykładów programów w C. Możesz je znaleźć w menu: „Plik-
>Przykłady”.
S t r o n a | 27
Po obejrzeniu i wypróbowaniu szeregu przykładów będziesz gotowy do napisania swojego pierwszego
programu w C !!! Nie zapomnij, że obsługę silników, servo, oraz czujników masz już opanowaną bo
robiłeś to w Scratch. Zawsze możesz zobaczyć jak środowisko mBlock konwertuje Twoje przykłady na
C.
S t r o n a | 28
Programowanie w C
Pierwsze kroki Jeżeli nie znasz C to możesz skorzystać z podręczników dostępnych online np.:
https://pl.wikibooks.org/wiki/C lub inny dostępny w Internecie.
Komunikacja z komórką za pośrednictwem modułu bluetooth W tym przykładzie napiszemy program, który będzie łączył się poprzez bluetooth z komórką i właśnie
za jej pomocą będziemy sterować naszym robotem.
Do tego celu zainstaluj na komórce program „Bluetooth Electronics” (będziemy go nazywać BC)
dostępny na stronie http://www.keuwl.com/apps/bluetoothelectronics. Program ten przyda się
właśnie do sterowania naszym robotem.
Otwórz nowy projekt w arduino w celu napisania programu obsługującego robota poprzez Bluetooth.
Bluetooth podłączony jest do arduino poprzez piny 0 i 1. Piny te standardowo w arduino służą do
obsługi komunikacji szeregowej i w taki sposób będziemy komunikować się z Bluetooth. Odczyt
danych z Bluetooth zrealizujemy za pomocą metody Serial.read(), zapis natomiast za pomocą metody
Serial.write(). W celu zainicjowania biblioteki używamy funkcji Serial.begin(9600), wywoływanej w
metodzie setup(). Z płytki arduino będziemy komunikować się z programem BC zainstalowanym na
komórce. W tym celu należy skonfigurować komórkę i ustalić konfigurację kontrolek w programie BC.
Program BC na komórce ma możliwość zbudowania aplikacji z kontrolkami, którymi możemy
sterować. Konfiguracja kontrolek polega na zdefiniowaniu ciągów znaków, które będą wysyłane przez
moduł Bluetooth w przypadku manipulacją daną kontrolką. Przykładowe okienko z kontrolkami
przedstawia poniższy rysunek:
W naszym przykładzie użyjemy Pad’y, Terminal, Switch’a, Button’a, Indycator’y.
S t r o n a | 29
Pad1 i pad2 pełnia taką sama funkcję I zostały umieszczone po to aby pokazać w jaki sposób możemy
sterować robotem przy pomocy różnych manipulatorów. Konfiguracja dla padów wygląda
następująco:
Min value – 0
Max value – 720
A and Y - off
Magntiude an Angle (deg) – on
Start with - _S
End with K
Send on change – on
Send Cotinuously - off
Konfiguracja dla terminal wygląda następująco:
Sent – off
Received – off
Both - on
Konfiguracja dla switch wygląda następująco:
Turn On Text - _J
Turn Off Text - _M_SR0A0K
Repeat Send When Switch is On – off
Reapeat every (ms) – 100
pad1 terminal
switch
button pad2
Indicator - dzwięk Indicator - światło
S t r o n a | 30
Current Switch State – Off
Konfiguracja dla button wygląda następująco:
Press Text - _SR0A0K
Release Text – pusty napis
Konfiguracja dla indicator światło wygląda następująco:
Receive Character - L
Red – wg uznania
Green – wg uznania
Blue – wg uznania
Konfiguracja dla indicator dzwięk wygląda następująco:
Receive Character - S
Select sound – według uznania
Volume – według uznania
Jak już mamy skonfigurowaną komórkę przejdźmy do pisania programu na arduino.
Zdefiniujmy funkcję do czytania znaków z Bluetooth’a a tym samym z programu BC zainstalowanego
na komórce. Wygląda ona następująco:
char readChar() {
while (!Serial.available())_loop();
return Serial.read();
}
Funkcję te będziemy używać w naszej pętli loop() w celu podjęcia decyzji co ma robić robot.
void loop() {
int r = 0, a = 0;
char tmp;
float distance = _getDistance(&angle, _auto);
if (_auto == 1) {
calculateSpeed(&speedLeft, &speedRight, 255, 270 + angle * SERVO_STEP);
}
if (Serial.available()) {
if (Serial.read() == '_') {
tmp = readChar();
switch (tmp) {
//SR255A360K
case 'S':
readChar();
r = Serial.parseInt();
readChar();
a = Serial.parseInt();
S t r o n a | 31
Serial.print("R ");
Serial.print(r, DEC);
Serial.print(" A ");
Serial.println(a, DEC);
calculateSpeed(&speedLeft, &speedRight, r, a);
//K
readChar();
break;
case 'J':
_auto = 1;
Serial.println("AUTO ON");
break;
case 'M':
_auto = 0;
Serial.println("AUTO OFF");
break;
}
}
}
if (distance < 20 & speedLeft != 0 & speedRight != 0) {
_stop();
jedz_tyl(255);
Serial.println("*LR255G0B0");
Serial.println("*S");
_delay(0.3);
_stop();
} else if (speedLeft == 0 & speedRight == 0) {
_stop();
Serial.println("*LR0G0B255");
} else {
jedz(speedLeft, speedRight);
Serial.println("*LR0G255B0");
}
}
Zwróć uwagę, że w fukcji loop decyzje o tym co ma robić robot są podejmowane na podstawie
znaków otrzymywanych z programu BE. Znaki są odbierane i decyzja o zachowaniu robota
podejmowana jest w switch’u, który interpretuje odbierane znaki.
Ważna jest też funkcja do obliczenia prędkości lewego I prawego koła na podstawie otrzymanych
wartości z program BE. Wygląda ona następująco:
void calculateSpeed(int* speedLeft, int* speedRight, int r, int a) {
a = a + 45;
double _a = ((double)a) * 2.0 * PI / 360.0;
double spdL, spdR;
spdR = cos(_a);
spdL = sin(_a);
spdL = (r * spdL);
*speedLeft = spdL;
spdR = (r * spdR);
*speedRight = spdR;
if (*speedRight > 255) {
S t r o n a | 32
*speedRight = 255;
} else if (*speedRight < -255) {
*speedRight = -255;
}
if (*speedLeft > 255) {
*speedLeft = 255;
} else if (*speedLeft < -255) {
*speedLeft = -255;
}
Serial.print(*speedLeft, DEC);
Serial.println(*speedRight, DEC);
}
Oczywiście niezbędne są też funkcje związane z pomiarem odległości oraz poruszaniem silnikami.
Poniżej znajduje się pełny listing programu.
#include <Arduino.h>
#include <Servo.h>
/*
Timer0 is an 8-bit timer, it can hold a maximum value of 255. It is
used by delay() and millis(), so there are consequences when messing with
it
Timer1 is a 16-bit timer, it can hold a maximum of 65535 (an unsigned
16-bit integer). The Arduino Servo library uses this timer
Timer2 is an 8-bit timer used by the Arduino tone() function
Pins 5 and 6: controlled by Timer0
Pins 9 and 10: controlled by Timer1
Pins 11 and 3: controlled by Timer2
*/
#define SERVO_STEP 10
#define DISTANCES 180/SERVO_STEP+1
Servo servo;
long servoStartTime = 0;
int pos = 0, direction_ = SERVO_STEP;
float distances[DISTANCES];
String str = "";
int speedLeft = 0, speedRight = 0;
char _auto = 0;
int angle = 0;
float getDistance(int trig, int echo) {
pinMode(trig, OUTPUT);
digitalWrite(trig, LOW);
delayMicroseconds(2);
digitalWrite(trig, HIGH);
delayMicroseconds(10);
S t r o n a | 33
pinMode(echo, INPUT);
return pulseIn(echo, HIGH, 30000) / 58.0;
}
void jedz_tyl_prawa(char val)
{
analogWrite(11, val);
digitalWrite(8, 0);
digitalWrite(9, 1);
}
void jedz_przod_prawa(char val)
{
analogWrite(11, val);
digitalWrite(8, 1);
digitalWrite(9, 0);
}
void jedz_przod_lewa(char val){
analogWrite(3, val);
digitalWrite(6, 0);
digitalWrite(7, 1);
}
void jedz_tyl_lewa(char val) {
analogWrite(3, val);
digitalWrite(6, 1);
digitalWrite(7, 0);
}
void _stop() {
analogWrite(3, 0);
digitalWrite(6, 0);
digitalWrite(7, 0);
analogWrite(11, 0);
digitalWrite(8, 0);
digitalWrite(9, 0);
}
void jedz_przod(char val){
jedz_przod_lewa(val);
jedz_przod_prawa(val);
}
void jedz_tyl(char val){
jedz_tyl_lewa(val);
jedz_tyl_prawa(val);
}
void setup() {
pinMode(3, OUTPUT);
pinMode(6, OUTPUT);
pinMode(5, OUTPUT);
pinMode(7, OUTPUT);
S t r o n a | 34
pinMode(8, OUTPUT);
pinMode(9, OUTPUT);
pinMode(11, OUTPUT);
Serial.begin(9600);
servo.attach(10);
pos = 90;
servo.write(pos);
for (int i = 0; i < DISTANCES; i++) {
distances[i] = 200;
}
}
float _getDistance(int* angle, char auto_) {
if (servoStartTime + 50 < millis()) {
float distance = getDistance(2, 4);
int j = 0;
distance += getDistance(2, 4);
distance += getDistance(2, 4);
distance += getDistance(2, 4);
distances[pos / SERVO_STEP] = distance / 4;
for (int i = 0; i < DISTANCES; i++) {
if (i > j) {
if (distances[i] > distances[j]) {
j = i;
}
}
}
*angle = j;
if (auto_ == 1) {
pos += direction_;
if (pos == 180) direction_ = -SERVO_STEP;
if (pos == 0) direction_ = SERVO_STEP;
} else {
pos = 90;
}
servo.write(pos);
servoStartTime = millis();
}
return distances[9];
}
char readChar() {
while (!Serial.available())_loop();
return Serial.read();
}
void loop() {
int r = 0, a = 0;
char tmp;
float distance = _getDistance(&angle, _auto);
if (_auto == 1) {
S t r o n a | 35
calculateSpeed(&speedLeft, &speedRight, 255, 270 + angle * SERVO_STEP);
}
if (Serial.available()) {
if (Serial.read() == '_') {
tmp = readChar();
switch (tmp) {
//SR255A360K
case 'S':
readChar();
r = Serial.parseInt();
readChar();
a = Serial.parseInt();
Serial.print("R ");
Serial.print(r, DEC);
Serial.print(" A ");
Serial.println(a, DEC);
calculateSpeed(&speedLeft, &speedRight, r, a);
//K
readChar();
break;
case 'J':
_auto = 1;
Serial.println("AUTO ON");
break;
case 'M':
_auto = 0;
Serial.println("AUTO OFF");
break;
}
}
}
if (distance < 20 & speedLeft != 0 & speedRight != 0) {
_stop();
jedz_tyl(255);
Serial.print("L");
_delay(0.3);
_stop();
} else if (speedLeft == 0 & speedRight == 0) {
_stop();
} else {
jedz(speedLeft, speedRight);
}
}
void calculateSpeed(int* speedLeft, int* speedRight, int r, int a) {
a = a + 45;
double _a = ((double)a) * 2.0 * PI / 360.0;
double spdL, spdR;
spdR = cos(_a);
spdL = sin(_a);
spdL = (r * spdL);
*speedLeft = spdL;
spdR = (r * spdR);
*speedRight = spdR;
S t r o n a | 36
if (*speedRight > 255) {
*speedRight = 255;
} else if (*speedRight < -255) {
*speedRight = -255;
}
if (*speedLeft > 255) {
*speedLeft = 255;
} else if (*speedLeft < -255) {
*speedLeft = -255;
}
Serial.print(*speedLeft, DEC);
Serial.println(*speedRight, DEC);
}
void jedz(int speedLeft, int speedRight) {
if (speedLeft > 0 & speedRight > 0) {
jedz_przod_lewa((char)speedLeft);
jedz_przod_prawa((char)speedRight);
} else if (speedLeft > 0 & speedRight < 0) {
jedz_przod_lewa((char)speedLeft);
jedz_tyl_prawa((char) - speedRight);
} else if (speedLeft <0 & speedRight>0) {
jedz_tyl_lewa((char) - speedLeft);
jedz_przod_prawa((char)speedRight);
} else if (speedLeft < 0 & speedRight < 0) {
jedz_tyl_lewa((char) - speedLeft);
jedz_tyl_prawa((char) - speedRight);
} else {
_stop();
}
}
void _delay(float seconds) {
long endTime = millis() + seconds * 1000;
while (millis() < endTime)_loop();
}
void _loop() {
}
Teraz możesz zaprogramować swojego robota I zobaczyć jak działa. Podczas programowania odłącz
linie RX i TX urządzenia Bluetooth w Arduino . Bluetooth powoduje konflikt elektryczne z układem
programującym.
Po zaprogramowaniu i podłączeniu spowrotem modułu Bluetooth jesteś gotowy do uruchomienia
komórki oraz programu BE. Po jego uruchomieniu nawiąż połaczenie przez Bluetooth. Na komórce
ekran BE wygląda następująco.
S t r o n a | 37
Teraz możesz wypróbować jak działa napisany przez Ciebie program – POWODZENIA !!!!!