Programowanie sekwencji ruchów manipulatora

[Spwrt]

Witam mój problem polega na tym, że nie mam pojęcia jak napisać program, żeby serwa poruszały się płynnie tzn. żeby każde z serw poruszało się jednocześnie, a nie najpierw pierwsze wykona ruch, potem kolejne... Próbowałem zrobić to na zasadzie pętli w pętli ale niestety nie działa. Ogólnie robot ma dwa tryby: ręcznego sterowania (joystick z potencjometrów) i wgrana sekwencja ruchów, nad którą właśnie pracuje.Tryb się zmienia za pomocą przycisku. Oto program ( na samym końcu są pętle o które głównie się rozchodzi). Dzięki z góry !

 #include <Servo.h> //Biblioteka
Servo serwomechanizm_1;// Odwołanie do serwa nr 1
Servo serwomechanizm_2;// Odwołanie do serwa nr 2
Servo serwomechanizm_3;// Odwołanie do serwa nr 3
Servo serwomechanizm_4;// Odwołanie do serwa nr 4
Servo serwomechanizm_5;// Odwołanie do serwa nr 5
Servo serwomechanizm_6;// Odwołanie do serwa nr 6


void setup()
{
Serial.begin(9600);
pinMode(2,INPUT_PULLUP); //Włącznik
pinMode(12,OUTPUT); // dioda (Arduino)
serwomechanizm_1.attach(3); // serwo nr 1 podłączone do pinu 3  
serwomechanizm_2.attach(5); // serwo nr 2 podłączone do pinu 5
serwomechanizm_3.attach(6); // serwo nr 3 podłączone do pinu 6
serwomechanizm_4.attach(9); // serwo nr 4 podłączone do pinu 9
serwomechanizm_5.attach(10); // serwo nr 5 podłączone do pinu 10
serwomechanizm_6.attach(11); // serwo nr 6 podłączone do pinu 11 
}
void loop()
{

digitalWrite(12, HIGH);
int odebraneDane_1 = analogRead(A0); // odczyt wartości z pinu A0
odebraneDane_1 = map(odebraneDane_1, 0, 1024, 0, 180); //Przeskalowanie odczytu z ADC na stopnie (0-180)

int odebraneDane_2 = analogRead(A1); // odczyt wartości z pinu A1
odebraneDane_2 = map(odebraneDane_2, 0, 1024, 23, 180); 

int odebraneDane_3 = analogRead(A2); // odczyt wartości z pinu A2
odebraneDane_3 = map(odebraneDane_3, 0, 1024, 23, 180); 

int odebraneDane_4 = analogRead(A3); // odczyt wartości z pinu A3
odebraneDane_4 = map(odebraneDane_4, 0, 1024, 0, 180);

int odebraneDane_5 = analogRead(A4); // odczyt wartości z pinu A4
odebraneDane_5 = map(odebraneDane_5, 0, 1024, 0, 180); 

int odebraneDane_6 = analogRead(A5); // odczyt wartości z pinu A5
odebraneDane_6 = map(odebraneDane_6, 0, 1024, 0, 180); 

if(digitalRead(2) == LOW)
{

serwomechanizm_1.write(odebraneDane_1);
serwomechanizm_2.write(odebraneDane_2);
serwomechanizm_3.write(odebraneDane_3);
serwomechanizm_4.write(odebraneDane_4);
 serwomechanizm_5.write(odebraneDane_5);
 serwomechanizm_6.write(odebraneDane_6);

Serial.print(odebraneDane_1, DEC);
Serial.print("[SERWO 1]\t");

Serial.print(odebraneDane_2, DEC);
Serial.print("[SERWO 2]\t");

Serial.print(odebraneDane_3, DEC); 
Serial.print("[SERWO 3]\t");

Serial.print(odebraneDane_4, DEC);
Serial.print("[SERWO 4]\t");

Serial.print(odebraneDane_5, DEC);
Serial.print("[SERWO 5]\t");

Serial.print(odebraneDane_6, DEC);
Serial.println("[SERWO 6]\t");

}
else 
{

 for (int pozycja = 0; pozycja<= 80; pozycja += 1) 
   {
   serwomechanizm_6.write(pozycja);            
   delay(15);
   }    

 for (int pozycja = 0; pozycja <= 100; pozycja += 1) 
   {
   serwomechanizm_5.write(pozycja);            
   delay(15);
   }             
   }    
}

[Gość es2]

Założyłem wątek "Pozbądź się delay" (coś w tym rodzaju) o wielowątkowości ale nie było zainteresowania wręcz przeciwnie został bardzo chłodno przyjęty. Jeden z programów forumowicza zmodyfikowałem. Nie wiedzę tego wątku, więc pewnie trzeba szukać w koszu a tam były poruszane problemy wykonywania wielu działań jednocześnie.

Jak nie znajdziesz tego wątku napisz tu http://www.avt.4ra.pl/viewforum.php?id=21

[Elvis]

es2, jak rozumiem życie niedocenianego geniusza jest ciężkie, więc piszesz żeby się wyżalić - bo raczej niewiele ta odpowiedź komukolwiek pomoże. Nie dość, że nie doceniają Twojej wszechwiedzy, wytykają błędy, bezczelnie krytykują, a na koniec ignorują - więc najlepsze rozwiązanie to uciec na inne forum. Co prawda puste, ale chociaż bez niepotrzebnej krytyki.

Spwrt, nie słuchaj złych rad o wielowątkowości. Do tego co chcesz zrobić wystarczy Ci jeden wątek i jedna prosta pętla. Ale ta prostota nie będzie taka łatwa na początek, więc proponuję rozwiązać ten problem małymi krokami.

Najpierw zastanów się jak napisałbyś program, w którym masz tylko jedno serwo, jedną pętlę, ale opóźnienia delay() są stałe - niech będzie 15ms.

Czyli pętla główna wygląda tak:

void loop()
{
 do_magic();
 delay(15);
}

Spróbuj zastanowić się, jak taka funkcja do_magic() wygląda i napisz co wymyśliłeś.

[marek1707]

Można do tego podejść bardzo ogólnie tak jak proponuje rozżalony es2, ale możesz spróbować zrobić to "po prostu". W sumie wyjdzie na to samo bo serwa będą chodzić razem, choć podejście jest trochę inne, może nie tak elastyczne.

Wyobraź sobie, że robisz jedną duża pętlę która ma w sobie jakiś delay. No niestety to wada kłująca w oczy, na szczęście można akurat to łatwo poprawić. No więc jest pętla z wbudowanym opóźnieniem powiedzmy 20ms czyli 50Hz. Każdy jej przebieg ma "wymyślić" aktualne położenie każdego serwa. Dla każdego z nich masz więc jakieś położenie aktualne, położenie docelowe i krok. Te trzy zmienne są stanem serwa. Za każdym przebiegiem robisz proste sumowanie modyfikujące położenie aktualne wg kroku i sprawdzasz czy serwo przekroczyło pozycję docelową. Jeśli tak, zerujesz mu krok i ono dalej już tylko stoi mimo kolejnych przebiegów pętli. Im dasz większy krok tym serwo będzie jechało szybciej. Pamiętaj, że modyfikacje jego położenia będą odbywać się tylko w takt całej pętli 20ms. Oczywiście po wykonaniu obliczeń dla wszystkich serw, pod koniec ciała pętli uaktualniasz ich rzeczywiste pozycje przez wywołania servo.write(). Dzięki temu co 20ms masz policzone i przesunięte każde serwo. Gdy już wszystkie się zatrzymały, możesz wyjść z pętli. To wydaje się dość proste, prawda?

Jeśli odpowiednio policzysz wielkość kroku każdego serwa wg jego zadanej drogi i wszystkie serwa wystartują w tym samym momencie to wszystkie zakończą pracę także w tej samej chwili mimo różnych prędkości. Zrobiłeś interpolator 🙂

Przykład:

Chcesz by w czasie 3 sekund trzy serwa zrobiły jednoczesne, płynne ruchy:

1. Od pozycji 20 do 70

2. Od pozycji 120 do 30

3. Od pozycji 0 do 180

Ponieważ w ciągu 3 sekund pętlę 20ms (czyli 50Hz) wykonasz 150 razy, liczysz trzy różne wielkości kroku:

1: (70-20)/150 = 0.333

2: (30-120)/150 = -0.6

3: (180-0)/150 = 1.2

Teraz wystarczy tylko ustawić pozycje początkowe:

servo_pos[0]=20.0

servo_pos[1]=120.0

servo_pos[2]=0.0

oraz wielkość kroku:

servo_step[0] = 0.333

servo_step[1] = -0.6

servo_step[2] = 1.2

i pozycje końcowe:

servo_target[0] = 70.0

servo_target[0] = 30.0

servo_target[0] = 180.0

teraz wystarczy odpalić główną pętlę modyfikującą servo_pos[] wg servo_step[] i uaktualniającą rzeczywiste położenia wywołaniami funkcji z biblioteki servo.

Czy to jakoś rozjaśnia? Rozwiązań tego problemu jest wiele, tu dałem przykład na zmiennych typu float, ale bez problemu zrobisz to na szybszych int-ach (mimo ułamków). Wtedy narzut obliczeniowy związany z arytmetyką będzie dużo mniejszy i pętla nie będzie tak "rozciągana" czasowo przez liczenie. Napisz jak poszło.

---------------------------

EDIT: Elvis, przepraszam, pisaliśmy razem a ja jak zwykle się rozwlekłem.. Nie widziałem że piszesz, ale zbieżność naszych tez jest jednak budująca 🙂

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

[Spwrt]

Dzięki za naprowadzenie, ale najpierw spróbuje zrobić metodą Elvisa, bo wydaje się prostsza, a ja nie jestem jakims kozakiem w programowaniu, wiec pojde na łatwizne. A zatem, wyszło mi coś takiego dla jednego serwa :

#include <Servo.h> //Biblioteka
Servo serwomechanizm_6;// Odwołanie do serwa nr 6
int pozycja_6 = 0;
void setup()
{
Serial.begin(9600);
serwomechanizm_6.attach(11); // serwo nr 6 podłączone do pinu 11 
}
int funkcja_zwracajaca_pozycje_serwa_6()
{
while(pozycja_6 <180)
{
pozycja_6++;
Serial.print(pozycja_6, DEC); // Dla zobrazowania czy pętla działa poprawnie
Serial.println("[pozycja_6]\t"); 
return pozycja_6;
}
}
void loop() {
pozycja_6 = funkcja_zwracajaca_pozycje_serwa_6();
serwomechanizm_6.write(pozycja_6);
delay(50);
}

Dla dwóch też sprawdzałem działa wszystko, dzięki bardzo !

[Elvis]

Prawdę mówiąc ta "moja metoda" to dokładnie to samo co od razu opisał marek1707 - tylko nie chciałem wszystkiego pisać za Ciebie, wolałem, żebyś małymi kroczkami odkrył rozwiązanie.

Kod który pokazałeś nie jest idealny. Jeśli działa to jest nawet ciekawe - w każdym razie coś jest nie tak z pętlą while - miała być jedna pętla, a są dwie!

int funkcja_zwracajaca_pozycje_serwa_6()
{
while(pozycja_6 <180)
{
return pozycja_6;
}
}

W sumie to nie jest pętla, bo return z niej wychodzi po pierwszym obiegu, ale może warto byłoby ten kod poprawić?

[ethanak]

Próbuję zrozumieć tę pętlę while... Chyba za mało piwa wypiłem. Może ktoś mi to wytłumaczyć? Może autor?

[Spwrt]

Gdzie w tym kodzie są dwie pętle while, Elvis ? Czy chodzi Ci o to, że niepotrzebna jest linijka :

pozycja_6++;

A jeśli chodzi o to jak działa ta pętla to powiem tak:

Najpierw zdefiniowałem zmienna int pozycja_6 = 0; czyli to jest pozycja poczatkowa dla serwa 6.Następnie napisalem funkcje która ma za zadanie dodawać pozycje co 1 stopien i odsylac ją w miejscu wywołania funkcji. Wywołujemy ją w loopie zaraz przed

 serwomechanizm_6.write(pozycja_6); 

aby serwo pobieralo wartosc bezposrednio po wykonaniu kazdej pętli.

Jesli chodzi o te dwie linijki

Serial.print(pozycja_6, DEC);
Serial.println("[pozycja_6]\t");

to dalem to tylko po to aby zobaczyc z monitorze portu szer. czy rzeczywiscie pętla działa, dlatego mozna to wyrzucic i tak też zrobiłem w docelowym programie.

[ethanak]

A co robi funkcja_zwracajaca_pozycje_6 (czy jakoś tak)? Pliz wyjaśnij...

[Spwrt]

Żartowniś z Ciebie 😃

[Elvis]

Spwrt, Arduino zamiast znanej z języka C/C++ funkcji main() używa loop() do zdefiniowania głównej części programu. Ta funkcja jest wykonywana w nieskończonej pętli - stąd pewnie jej nazwa. Masz więc w programie dwie pętle: pierwszą, której nie widać ale to ona wywołuje w kółko loop() oraz drugą utworzoną instrukcją while.

Podpowiem, że to co zrobiłeś odpowiada działaniu instrukcji warunkowej if. Proponuję doczytać czym różni się if od while - to powinno nieco rozjaśnić moje uwagi. Sporo początkujących myli if z while, więc nie ma co się mocno czepiać.

[ethanak]

Niestety nie...

To powiesz co zwraca czy zachowasz to w tajemnicy?

BTW takie czerwone literki warto czasami czytać...

[ Dodano: 25-04-2018, 14:50 ]

@Elvis: funkcja main jest zaimplentowana w Arduino:

int main()
{
  setup();
  for (;;) loop();
}

[Elvis]

ethanak, wiem - ale użytkownicy Arduino nie używają main bezpośrednio.

[ethanak]

Co nie zmienia faktu, że chciałbym poznać zdanie Autora na temat wartości zwracanej przez w/wym. funkcję😉

[Spwrt]

Elvis, serio nie wiem jak to poprawić tak wyglada cały program dodalem tylko jeden szczegół, a co dalej to nie wiem, póki co działa:

#include <Servo.h> //Biblioteka
Servo serwomechanizm_1;// Odwołanie do serwa nr 1
Servo serwomechanizm_2;// Odwołanie do serwa nr 2
Servo serwomechanizm_3;// Odwołanie do serwa nr 3
Servo serwomechanizm_4;// Odwołanie do serwa nr 4
Servo serwomechanizm_5;// Odwołanie do serwa nr 5
Servo serwomechanizm_6;// Odwołanie do serwa nr 6
int pozycja=0;
void setup()
{
Serial.begin(9600);
pinMode(2,INPUT_PULLUP); //Włącznik
pinMode(12,OUTPUT); // dioda (Arduino)
serwomechanizm_1.attach(3); // serwo nr 1 podłączone do pinu 3  
serwomechanizm_2.attach(5); // serwo nr 2 podłączone do pinu 5
serwomechanizm_3.attach(6); // serwo nr 3 podłączone do pinu 6
serwomechanizm_4.attach(9); // serwo nr 4 podłączone do pinu 9
serwomechanizm_5.attach(10); // serwo nr 5 podłączone do pinu 10
serwomechanizm_6.attach(11); // serwo nr 6 podłączone do pinu 11 
}

int funkcja_zwracajaca_pozycje_serwa(int pozycja, int pozycja_docelowa)
{
while(pozycja < pozycja_docelowa)
{
pozycja++;
return pozycja;
}
}

void loop()
{

digitalWrite(12, HIGH);
int odebraneDane_1 = analogRead(A0); // odczyt wartości z pinu A0
odebraneDane_1 = map(odebraneDane_1, 0, 1024, 0, 180); //Przeskalowanie odczytu z ADC na stopnie (0-180)

int odebraneDane_2 = analogRead(A1); // odczyt wartości z pinu A1
odebraneDane_2 = map(odebraneDane_2, 0, 1024, 23, 180); 

int odebraneDane_3 = analogRead(A2); // odczyt wartości z pinu A2
odebraneDane_3 = map(odebraneDane_3, 0, 1024, 23, 180); 

int odebraneDane_4 = analogRead(A3); // odczyt wartości z pinu A3
odebraneDane_4 = map(odebraneDane_4, 0, 1024, 0, 180);

int odebraneDane_5 = analogRead(A4); // odczyt wartości z pinu A4
odebraneDane_5 = map(odebraneDane_5, 0, 1024, 0, 180); 

int odebraneDane_6 = analogRead(A5); // odczyt wartości z pinu A5
odebraneDane_6 = map(odebraneDane_6, 0, 1024, 0, 180); 

if(digitalRead(2) == LOW)
{

//serwomechanizm_1.write(odebraneDane_1);
//serwomechanizm_2.write(odebraneDane_2);
//serwomechanizm_3.write(odebraneDane_3);
//serwomechanizm_4.write(odebraneDane_4);
 serwomechanizm_5.write(odebraneDane_5);
 serwomechanizm_6.write(odebraneDane_6);

Serial.print(odebraneDane_1, DEC);
Serial.print("[SERWO 1]\t");

Serial.print(odebraneDane_2, DEC);
Serial.print("[SERWO 2]\t");

Serial.print(odebraneDane_3, DEC); 
Serial.print("[SERWO 3]\t");

Serial.print(odebraneDane_4, DEC);
Serial.print("[SERWO 4]\t");

Serial.print(odebraneDane_5, DEC);
Serial.print("[SERWO 5]\t");

Serial.print(odebraneDane_6, DEC);
Serial.println("[SERWO 6]\t");

}
else 
{
pozycja = funkcja_zwracajaca_pozycje_serwa(pozycja,60);
serwomechanizm_6.write(pozycja);
delay(50);

pozycja = funkcja_zwracajaca_pozycje_serwa(pozycja, 100);
serwomechanizm_5.write(pozycja);
delay(50);
} 
}

[ Dodano: 25-04-2018, 15:34 ]

Funkcja zwraca wartość pozycji, która z każdym obiegiem pętli wzrasta, przez co serwo się rusza.