Serwomechanizm modelarski

Artykuł opisuje sterowanie serwomechanizmem modelarskim. W materiale postaram się przedstawić budowę i zasadę działania serwa oraz implementację sterowania na zestawie Nucelo-L476RG. Zajmiemy się dwoma typami serwomechanizmów: FEETECH FS90 typu Micro o zakresie obrotu 90° oraz PowerHD AR-3603HB (praca ciągła).

Budowa i zasada działania

Serwomechanizm modelarski jest układem napędowym z wbudowanym sprzężeniem zwrotnym. Jak sama nazwa wskazuje, serwo jest powszechnie stosowane w modelarstwie – zdalnie sterowanych samolotach, samochodach czy łodziach do kontrolowania ruchu lotek, przepustnicy lub steru. Uniwersalność serwomechanizm zawdzięcza temu, że jest kompletnym napędem – nie wymaga sterownika ani enkodera, a do jego uruchomienia potrzebne jest tylko napięcie zasilające i sygnał sterujący. Dzieje się tak dlatego, że wszystko, czego napęd potrzebuje, znajduje się we wnętrzu obudowy.

• 

Każdy serwomechanizm składa się z:

• silnika prądu stałego
• przekładni
• potencjometru
• układu elektronicznego do sterowania silnikiem
• przewodu ze standardowym złączem 3-pinowym (najpopularniejszy jest wtyk JR)
• orczyków (elementów ułatwiających montaż)

Cechą charakterystyczną złącza stosowanego w serwach jest to, że mają one przewód zasilania (+) umieszczony na środku 3-pinowego konektora, dzięki czemu odwrotne podłączenie nie spowoduje uszkodzenia.

Praca serwomechanizmu oparta jest na schemacie działania układu ze sprzężeniem zwrotnym. Sygnał podany na serwomechanizm przekazywany jest do układu sterującego. Układ generuje sygnał dla silnika przekazywany na wał wyjściowy za pomocą przekładni. Na wyjściu przekładni zamocowany jest potencjometr, który daje modułowi sterującemu informację zwrotną.

• 

Na rynku dostępne są serwomechanizmy w różnych rozmiarach i parametrach. Podstawowe wielkości to:

• micro – nieduże serwa o wadze do 10 g i wymiarach ok. 23 x 12 x 22 mm, stosowane w mniejszych modelach i do elementów nie wymagających dużego momentu obrotowego np. w szybowcach
• medium – serwa o wadze ok. 25 g i wymiarach ok. 30 x 12 x 30 mm, stosowane w średniej wielkości konstrukcjach
• standard – serwa o wadze ok. 50 g i wymiarach ok. 40 x 20 x 40 mm, stosowane w większych modelach, manipulatorach czy robotach kroczących

Serwomechanizmy możemy również podzielić ze względu na rodzaj przekładni:

• plastikowa – najmniej trwały typ przekładni, stosowane w mniejszych modelach, gdzie nie są wymagane duże momenty obrotowe
• metalowa – najbardziej wytrzymałe i bezawaryjne, mają małe luzy i dość wolno się zużywają
• CFRP – przekładnie z kompozytu wzmacnianego włóknami węglowymi, cechują się większym momentem obrotowym niż przekładnie plastikowe, jednak są dość kruche i mogą ulegać uszkodzeniu np. przy uderzeniu

Pod względem rodzaju sterowania wyróżnia się dwa typy serwomechanizmów: analogowe i cyfrowe. Oba typy serw otrzymują identyczny sygnał sterujący PWM o częstotliwości 50 Hz, jednak różni się sposób sterowania silnikiem przez układ elektroniczny. W serwomechanizmach analogowych sygnał sterujący przetwarzany jest na napięcie. Układ elektroniczny porównuje je z napięciem otrzymanym jako informacja zwrotna z potencjometru i generuje sygnał sterujący silnikiem. W serwach analogowych sygnał ten ma częstotliwość 50 Hz. W serwomechanizmach cyfrowych za sterowanie silnikiem odpowiada mikrokontroler, który porównuje wartość cyfrową sygnału sterującego z odczytem z potencjometru i generuje sygnał sterujący silnikiem o częstotliwości 300 Hz. Dzięki temu, że sygnał ma większą częstotliwość, przerwy między stanem wysokim podanym na silnik są mniejsze. Sprawia to, że serwo cyfrowe ma znacznie szybszą reakcję i jest precyzyjniejsze.

Jak już wspomniałem, serwomechanizm jest sterowany za pomocą sygnału PWM. PWM (Pulse Width Modulation) to sygnał prostokątny, w którym zmienna jest szerokość generowanego impulsu. W serwomechanizmach sygnał PWM powinien mieć częstotliwość 50 Hz (czyli okres 20 ms), a wypełnienie mieścić się w zakresie od 1 do 2 ms (czyli od 5 do 10 %). Wartość ta jest przybliżona i każdy serwomechanizm może mieć nieznaczne odchylenie.

• 

Serwomechanizmy mogą mieć różne zakresy działania. Oznaczone się one kątem, o jaki może się obrócić wał wyjściowy względem położenia bazowego. I tak możemy mieć m.in. serwo z kątem obrotu 90°, 180°, czy 270°. Istnieją też serwa bez ograniczenia kąta obrotu i są oznaczane jako 360° (praca ciągła). Wszystkie są sterowane sygnałem 50 Hz o zakresie wypełnienia 5-10%, a w zależności od zakresu ruchu poszczególne wypełnienia będą odpowiadały innym położeniom.

Implementacja

Sterowanie serwomechanizmem zaimplementujemy dla mikrokontrolera STM32L476RG na zestawie Nucleo-L476RG w środowisku STM32CubeIDE.

Tworzymy zatem nowy projekt wybierając „File->New->STM32 Project”. Przechodzimy przez wstępną konfigurację projektu i zabieramy się za konfigurację wyjść mikrokontrolera. Ja wygenerowałem projekt z domyślną konfiguracją dla płytki Nucleo, dlatego część pinów mam już skonfigurowane. Do sterowania serwem będziemy potrzebowali tylko jednego sygnału i wykorzystamy do tego pin PB10 jako wyjście PWM (TIMER 2, kanał 3). W oknie konfiguracji układów peryferyjnych po lewej stronie wybieramy zatem zakładkę „Timers->TIM2” i w polu „Channel 3” wybieramy „PWM Generation CH3„. Możemy też przypisać etykietę (klikając prawym przyciskiem na pin PB10).

• 

Teraz przejdziemy do konfiguracji samego układu Timer-a. Po wybraniu trybu pracy kanału 3 jako PWM, otworzy nam się okno Configuration, gdzie mamy dostępną konfiguracje rejestrów licznika w przedstawioną w przystępny sposób. W przypadku mikrokontrolera STM32L476RG (dla innych układów mogą nieznacznie się różnić) mamy możliwość skonfigurowania następujących ustawień:

• Counter Settings – ogólnie ustawienia timer-a:
  • Prescaler – dzielnik zegara, wartość 16-bitowa, należy pamiętać, że wpisujemy tutaj wartość, jaką chcemy osiągnąć pomniejszoną o 1, czyli np. dla 100 wpisujemy 99 (wynika to z faktu, że w momencie kasowania zawartości rejestru przeskakuje nam dodatkowy takt zegara, bo liczmy od 0 – szczegóły w „Reference Manual” na stronie 1077)
  • Counter Mode – tryb zliczania: w górę (Up) lub w dół (Down)
  • Counter Period (AutoReload Register) – wartość, do jakiej (w tybie Up) lub od jakiej (w trybie Down) nasz licznik będzie liczył
  • Internal Clock Division – dodatkowy dzielnik zegara używany przez filtry
  • Auto-Reload Preload – bit decydujący, czy zawartość rejestru ARR, gdzie przechowywana jest wartość do której zlicza licznik, ma być buforowana (wpisana do rejestru na stałe, czy tylko w przypadku zdarzenia od zakończenia liczenia)
• Trigger Output (TRGO) – konfiguracja wyjścia wyzwalającego dla innych liczników
• Clear Input – konfiguracja resetowania wyjścia PWM na dodatkowe zdarzenia
• PWM Generation Channel 3 – ustawienia kanału 3:
  • Mode – tryb PWM (tryb 1, tryb 2, tryb asymetryczny lub łączony). W trybie 1 stan wysoki na wyjściu jest utrzymywany do momentu, aż licznik osiągnie wartość wpisaną jako wypełnienie (Pulse) w rejestrze CCRx, potem następuje przełączenie na stan niski aż do osiągnięcia przez licznik wartości końcowej, czyli Counter Period. W trybie 2 licznik działa odwrotnie, najpierw wyjście przyjmuje stan niski, a po doliczeniu do wartości Pulse przełącza wyjście w stan wysoki.
  • Pulse – 32-bitowy licznik, wartość przy jakiej następuje zmiana stanu na wyjściu PWM, określa początkową wartość wypełnienia
  • Output Compare Preload – określa czy zmiana wypełnienia w trakcie liczenia jest możliwa (disable), czy nie (enable)
  • Fast Mode – trybki szybki, minimalizuje opóźnienie poprzez pominięcie porównywania licznika. Szczególnie przydatny w trybie One Pulse Mode
  • CH Polarity – polaryzacja wyjścia, działa podobnie jak tryb 1 i tryb 2, przy wartości High stan wysoki na wyjściu jest utrzymywany do momentu, aż licznik osiągnie wartość wpisaną jako wypełnienie (Pulse) w rejestrze CCRx, potem następuje przełączenie na stan niski aż do osiągnięcia przez licznik wartości końcowej, czyli Counter Period. W polaryzacji Low licznik działa odwrotnie, najpierw wyjście przyjmuje stan niski, a po doliczeniu do wartości Pulse przełącza wyjście w stan wysoki.

Jak wspominałem wcześniej, do sterowania serwomechanizmem powinniśmy wykorzystywać sygnał PWM o częstotliwości 50 Hz. Aby skonfigurować nasz licznik, musimy odpowiednio ustawić wartości: Prescaler i Counter Period. Częstotliwość sygnału PWM na podstawie tych wartości oraz częstotliwości taktowania Timer-a oblicza się z poniższego wzoru:

PWM_Freq = Timer_Freq / (Prescaler * Counter Period)

Częstotliwość taktowania Timer-a 2, którego używamy w projekcie, możemy odczytać z zakładki Clock Configuration. Aby tego dokonać, potrzebujemy informacji o tym, do jakiej szyny podłączony jest Timer 2. Możemy to sprawdzić w dokumentacji (Datasheet) mikrokontrolera na stronie 17.

• 

Jak możemy zauważyć, Timer 2 podłączony jest do szyny APB1. W zakładce Clock Configuration można odczytać, że jeżeli zegar główny (HCLK) ma ustawioną maksymalną częstotliwość dostępną dla tego mikrokontrolera, czyli 80 MHz, to szyna (a zatem również nasz licznik) jest taktowana z częstotliwością 80 MHz.

• 

Wartość Counter Period określa naszą rozdzielczość sterowania serwem. Aby łatwiej było to odnieść do zakresu sterowania podanego w milisekundach (1-2), w polu Prescaler ustawimy wartość 80, dzięki czemu wartość Counter Period będzie liczona w mikrosekundach. Chcąc zatem, aby sygnał PWM miał częstotliwość 50 Hz, Counter Period powinien mieć wartość:

80 000 000/(50 * 80) = 20000

Pozostały nam do skonfigurowania pozostałe istotne parametry licznika. Chcemy aby nasz licznik zliczał w górę (Counter Mode -> Up), wykorzystamy tryb 1 PWM, bez trybu Fast oraz z polaryzacją High. Jak opisywałem wcześniej, wartość Pulse określa wypełnienie PWM. Tutaj konfigurujemy tak naprawdę wartość początkową, ponieważ wypełnienie będziemy modyfikować za każdym razem, gdy będziemy chcieli zmienić położenie serwa. Możemy ustawić ją zatem jako 1000 (graniczne położenie). Pozostałe parametry nie są dla nas istotne i możemy je zostawić z wartością domyślną. Pełna konfiguracja Timer-a 2 oraz kanału 3 wygląda w następujący sposób.

• 

Przy tak skonfigurowanych peryferiach możemy wygenerować projekt („Project->Generate Code” lub „Alt+K„) i przejść do napisania kodu programu. Na początku stworzymy dwa pliki do obsługi serwomechanizmu: servo.c (w folderze Core->Src) oraz servo.h (w folderze Core->Inc), które będą stanowiły naszą bibliotekę.

W pliku servo.h dodamy definicje stałych potrzebnych do obliczeń kąta obrotu i prędkości serwa. Wartości SERVO_MAX_MS oraz SERVO_MIN_US mogą się nieznacznie różnić w zależności od rodzaju serwa i należy je dobrać indywidualnie. Jeżeli używasz serwa o innych zakresie obrotu, w stałej SERVO_MAX_ANGLE możesz umieścić swoją wartość.

#define SERVO_MAX_US		2000
#define SERVO_MIN_US		1000

#define SERVO_MAX_ANGLE		90
#define SERVO_MIN_ANGLE		0

#define	SERVO_MAX_SPEED		100

void servo_init(TIM_HandleTypeDef *, uint32_t);
void servo_set_angle(uint8_t);
void servo_set_speed(int);

Następnie w pliku servo.c tworzymy funkcję inicjalizującą pracę serwa oraz dodajemy niezbędne zmienne, które będą przechowywały informację o używanym timerze i kanale.

TIM_HandleTypeDef *pwm_tim;
uint32_t pwm_channel;

void servo_init(TIM_HandleTypeDef *tim, uint32_t channel)
{
	pwm_tim = tim;
	pwm_channel = channel;

	__HAL_TIM_SET_COMPARE(pwm_tim, pwm_channel, SERVO_MIN_MS);
	HAL_TIM_PWM_Start(pwm_tim, pwm_channel);
}

Teraz możemy przejść do funkcji sterującej serwem. Na początku warto dodać zabezpieczenie, które nie pozwoli na wywołanie obrotu o kąt większy niż możliwości użytego napędu. Następnie obliczamy wartość wypełnienia na podstawie zadanego kata obrotu i ustawiamy parametr Pulse Timer-a.

void servo_set_angle(uint8_t angle)
{
	if(angle < SERVO_MIN_ANGLE)
		angle = SERVO_MIN_ANGLE;
	else if(angle > SERVO_MAX_ANGLE)
		angle = SERVO_MAX_ANGLE;

	uint32_t pwm_duty_us;

	pwm_duty_us = SERVO_MIN_US + (angle * (SERVO_MAX_US - SERVO_MIN_US))/SERVO_MAX_ANGLE;

	__HAL_TIM_SET_COMPARE(pwm_tim, pwm_channel, pwm_duty_us);
}

Analogicznie tworzymy funkcję do obsługi serwa przeznaczonego do pracy ciągłej.

void servo_set_speed(int speed)
{
	if(speed < -SERVO_MAX_SPEED)
		speed = -SERVO_MAX_SPEED;
	else if(speed > SERVO_MAX_SPEED)
		speed = SERVO_MAX_SPEED;

	uint32_t pwm_duty_us;

	pwm_duty_us = (SERVO_MAX_US + SERVO_MIN_US)/2 + (speed * (SERVO_MAX_US - SERVO_MIN_US)/2)/SERVO_MAX_SPEED;

	__HAL_TIM_SET_COMPARE(pwm_tim, pwm_channel, pwm_duty_us);
}

W funkcji main() wywołujemy inicjalizację oraz zmianę zadanej wartości kąta (lub prędkości) na podstawie czteroelementowej tablicy. Zmiana będzie następowała z odstępem 2 sekund. Żeby przetestować serwo do pracy ciągłej, wystarczy odkomentować funkcję servo_set_speed(), a zakomentować servo_set_angle().

/* USER CODE BEGIN 2 */
  servo_init(&htim2, TIM_CHANNEL_3);

  int speed_table[] = {50, 20, -20, 10};
  uint8_t angle_table[] = {0, 90, 45, 15};

  int i = 0;
  uint32_t time_tick = HAL_GetTick();
  uint32_t max_time = 2000;
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
	  if((HAL_GetTick() - time_tick) > max_time)
	  {
		  time_tick = HAL_GetTick();

		  //servo_set_speed(speed_table[i++]);
		  servo_set_angle(angle_table[i++]);

		  if(i >= 4)
			  i = 0;
	  }
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */

Należy pamiętać także o dodaniu informacji o pliku servo.h w main.h.

/* USER CODE BEGIN Includes */
#include "servo.h"
/* USER CODE END Includes */

Przed uruchomieniem projektu powinniśmy jeszcze połączyć sterownik z zestawem Nucleo i podłączyć serwo. Robimy to według poniższego schematu.

• 

Teraz możemy skompilować kod (Project->Build Project) i go uruchomić (Run->Run). Efekt działania został przedstawiony na filmie poniżej.

Podsumowanie

W poradniku zapoznaliśmy się z budową i zasadą działania serwomechanizmu modelarskiego. Łatwość sterowania i brak dodatkowych elementów sprawia, że serwo jest jednym z najczęściej stosowanych napędów w robotyce. Używane są do sterowania robotów kołowych, kroczących, konstrukcji dwunożnych i manipulatorów. Mam nadzieję, że artykuł pozwoli Ci poznać i odkryć możliwości tego typu napędów i zastosować go w swoich projektach.

Do pobrania

Repozytorium GitHub