آموزش STM32 با توابع LL قسمت 36: راه‌اندازی و کنترل Servo Motor

به دستگاه‌هایی گفته می‌شود که می‌توان به‌وسیله آن‌ها موقعیت زاویه‌ای و یا خطی و همچنین سرعت و شتاب را کنترل کرد. Servo Motor درواقع یک موتور الکتریکی ساده است که به‌وسیله سروومکانیزم کنترل می‌شود. در قسمت‌های قبل و پس از معرفی موج PWM گفتیم که این موج برای کنترل این نوع موتور نیز استفاده می‌شود. در این قسمت می‌خواهیم یک Servo Motor را به‌وسیله میکروکنترلر راه‌اندازی و کنترل کنیم.

می‌دانیم که با استفاده از Servo Motor می‌توانیم موقعیت زاویه‌ای را کنترل کنیم. اما این عمل چگونه انجام می‌شود؟ ساختار سروو موتور چگونه است و از چه اجزایی تشکیل شده است؟ و در آخر نحوه محاسبه و تولید سیگنال کنترلی به چه صورت است؟

در ادامه سعی می‌کنیم به این سؤالات پاسخ دهیم و سروو موتور را با جزییات بیشتر معرفی کنیم. سپس سراغ ایجاد پروژه و کد نویسی خواهیم رفت.

اصول Servo Motor و کاربرد آن

در بسیاری از موقعیت‌ها، نیاز به موتور الکتریکی‌ای وجود دارد که امکان تغییر زاویه را به مقدار مشخص و دلخواهی برای ما فراهم کند. در این کاربردها به‌وسیله انواع خاصی از موتورهای الکتریکی و اجزاء دیگر فرمان الکتریکی لازم برای چرخش محور موتور، به میزان موردنظر اعمال می‌شود. درواقع این همان وظیفه‌ای است که می‌خواهیم برای ما انجام بدهد.

Servo Motor ها به‌طورمعمول از یک موتور DC ساده تشکیل‌شده‌اند که به‌وسیله مدار کنترل‌کننده و همچنین یک سروومکانیزم (یک سیستم کنترلی حلقه بسته) در زاویه موردنظر تنظیم می‌شود. سیستم توصیف‌شده امروزه، صنعت گسترده‌ای دارد علاوه بر این‌ها در موارد دیگری ازجمله ربات‌ها، ماشین‌ها و هواپیماهای اسباب‌بازی کنترلی، DVD و CD پلیرها و صدها وسیله روزمره دیگر کاربرد دارند.

اجزای Servo Motor و نحوه کارکرد آن

Servo Motor ها خود به دو دسته DC و AC تقسیم می‌شوند که همان‌طور که انتظار می‌رود بسته به نوع موتوری دارد که به سروومکانیزم متصل شده است. همان‌طور که اشاره شد، Servo Motor یک اکچوایتور خطی و یا زاویه‌ای است که به‌وسیله آن می‌توان موقعیت خطی و یا زاویه‌ای و همچنین سرعت و شتاب را به‌صورت دقیق کنترل کرد. هر کدام به‌طورمعمول شامل یک موتور متصل به یک سنسور فیدبک موقعیت می‌شود. علاوه بر آن برای کنترل موقعیت و فرمان دادن به موتور از یک ماژول کنترلر نیز استفاده می‌شود. سایر اجزای تشکیل دهنده یک Servo Motor نمونه و همچنین مراحل تغییر موقعیت آن را می‌توان در شکل زیر مشاهده کرد:

اشاره کردیم که سیگنال کنترلی ورودی برای فرمان دادن به Servo Motor و تنظیم آن در زاویه موردنظر، درواقع یک سیگنال PWM است. در دیتاشیت هر Servo Motor مشخصات این سیگنال ازجمله دوره تناوب آن و طول پالس مشخص‌شده برای هر زاویه، نوشته‌شده است. به طور معمول برای کنترل Servo Motor ها، ازجمله نمونه‌ای که در این پروژه استفاده می‌کنیم از یک موج PWM با فرکانس 50‌Hz (یعنی با تناوب 20‌ms) استفاده می‌شود. بازه زاویه قابل چرخش و همچنین طول پالس PWM تعیین‌شده برای هر زاویه، در Servo Motor های مختلف تفاوت دارد. به‌عنوان‌مثال برای Servo Motor به‌کاررفته در این پروژه، طول پالس متناظر با زاویه 0 درجه، 0.55‌ms و طول پالس متناظر با زاویه 180 درجه، 2.8‌ms است که به‌وسیله آزمایش و سعی و خطا تعیین‌شده‌اند.

بنابراین در هر Servo Motor، برای هر زاویه خاص یک طول پالس متناظر تعریف می‌شود که یک نمونه آن در شکل زیر مشخص‌شده است. در این شکل، برای اینکه در موقعیت‌های 0، 90 و 180 درجه قرار بگیرد، طول پالس‌های متناظر نشان داده‌شده است. برای سایر زوایا نیز با استفاده یک نگاشت در همین بازه می‌توان طول پالس مناسب را به مدار اعمال کرد.

همان‌طور که در شکل زیر نشان داده‌شده است، سیگنال کنترلی گفته‌شده به کنترل‌کننده درون Servo Motor منتقل می‌شود و سپس اختلاف زاویه موقعیت فعلی با زاویه ورودی (متناظر با سیگنال اعمال‌شده) محاسبه‌شده و فرمان الکتریکی مناسب به موتور اعمال می‌شود.

اکنون که با ساختار Servo Motor و نحوه کارکرد آن آشنا شدیم، می‌خواهیم به سراغ توسعه یک پروژه برای کنترل این ساختار برویم.

ایجاد پروژه

برای این پروژه، تنظیمات را دقیقاً مانند پروژه اولیه‌ی مربوط به PWM انجام می‌دهیم. بدین‌صورت که تنظیمات کلاک و دیباگ را مانند همیشه اعمال می‌کنیم و سپس Timer1 را در حالت PWM تنظیم می‌کنیم. فرکانس شکل موج PWM را نیز به‌وسیله مقدار تقسیم‌کننده فرکانس (PSC) و رجیستر ARR، روی 50‌Hz تنظیم می‌کنیم.

برای چاپ پیغام‌های خروجی موردنظر در ترمینال سریال می‌توانیم مانند گذشته واحد USART1 را نیز فعال کنیم. همچنین می‌توانیم یک پایه را نیز برای اتصال کلید و به‌منظور فرمان دادن به Servo Motor در حالت ورودی تنظیم کنیم.

در آخر پروژه را ایجاد کرده و به سراغ کد می‌رویم.

نوشتن کد پروژه

در این پروژه برای کنترل سروو، سه فایل servo_config.h، ‌servo.h و servo.c را ایجاد می‌کنیم. در فایل اول یعنی servo_config.h، اطلاعات مربوط به Servo Motor (از دیتا شیت یا آزمایش) را وارد می‌کنیم. و در توابع نوشته‌شده در servo.c از این اطلاعات استفاده خواهد شد. پس ابتدا به سراغ نوشتن فایل servo_config.h می‌رویم.

در این فایل، ابتدا کتابخانه‌های موردنیاز برای نوشتن توابع کنترلی را اضافه کرده و سپس ثابت‌های مربوط به Servo Motor را تعریف می‌کنیم؛

/********************/
/********///includes: 

#include "stm32f1xx_ll_utils.h"
#include "stm32f1xx_ll_tim.h"
/********************/
/********///definitions:
//servo's angle range in Degrees
#define Angle_Range 180
// Minimum pulse width in microsecends that corresponds to servo's 0 degree postion
#define Min_Pulse_Width 550 
// Maxmum pulse width in microsecends that corresponds to servo's 180 degree postion
#define Max_Pulse_Width 2800 
// Range of pulse width in microsecends that corresponds to servo's angle range
#define Pulse_Width_Range (Max_Pulse_Width - Min_Pulse_Width)
//Appropriate PWM wave period in microsecends
#define PWM_period 20000

سپس در فایل servo.h فایل‌های موردنیاز را اضافه و توابعی که می‌خواهیم در servo.c تعریف کنیم را اعلان می‌کنیم.

/********************/
/********///includes: 

#include <stdbool.h>
#include "servo_config.h"

/********************/
/********///Functions: 

void servo_init(void);

bool servo_moveto(int);

void servo_sweep(int, int);

در آخر و در فایل servo.c، ابتدا فایل servo.h را اضافه کرده و سپس توابع را تعریف می‌کنیم. اولین تابع یعنی servo_init، وظیفه راه‌اندازی وسایل جانبی موردنیاز servo (یا همان تایمر) را بر عهده دارد.

 #include "servo.h"

 /* Function for initializing servo */
void servo_init(void)
{
LL_TIM_EnableCounter(TIM1); //Enable Timer1's counter
LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1); //Enable channel1 of Timer1(PWM)
LL_TIM_EnableAllOutputs(TIM1); //Enable Timer1's outputs
}

همان‌طور که در بدنه تابع دیده می‌شود، مشابه پروژه PWM، برای راه‌اندازی servo_init تنها نیاز داریم که تایمر 1 (یا یک تایمر دیگر) را در حالت PWM راه‌اندازی کنیم. در تابع دوم یعنی servo_moveto، عمل تغییر زاویه به زاویه ورودی موردنظر انجام می‌شود. بدنه این تابع را به‌صورت زیر می‌نویسیم:

 /* Function for moving servo to desired position */
bool servo_moveto(int degree)
{
if((0 > degree) || (Angle_Range < degree))
{
return false;
}

uint32_t ARR = LL_TIM_GetAutoReload(TIM1) + 1;
uint32_t CRR = (((( (float) degree / Angle_Range) * Pulse_Width_Range) + Min_Pulse_Width) / PWM_period) * ARR;
LL_TIM_OC_SetCompareCH1(TIM1, CRR); //Set Duty cycle 
return true;
}

در تابع آخر نیز دو ورودی که زاویه شروع و زاویه پایان هستند گرفته می‌شوند و سپس Servo Motor، بازه میان این دو زاویه را sweep می‌کند. بدنه این تابع به‌صورت زیر و با استفاده از تابع servo_moveto نوشته می‌شود:

/* Function for sweeping from angle specified with begin_pos to the angle end_pos */ 
void servo_sweep(int begin_pos, int end_pos)
{ 
if (begin_pos < 0)
{
begin_pos = 0;
}

if (begin_pos > 180)
{
begin_pos = 180;
}

if (end_pos < 0)
{
end_pos = 0;
}

if (end_pos > 180)
{
end_pos = 180;
}

if (end_pos < begin_pos)
{
servo_moveto(begin_pos);
return;
}


for (int i = end_pos; i > begin_pos; i--)
{
servo_moveto(i); 
Delay_msec(30);
}
for (int i = begin_pos; i < end_pos; i++)
{
servo_moveto(i);
Delay_msec(30);
}
}

اکنون‌که نوشتن فایل‌های موردنیاز پروژه به اتمام رسیده است، می‌توانیم از توابع تعریف‌شده، در فایل اصلی برنامه، یعنی main.c، استفاده کنیم. اما قبل از آن، هدرفایل servo.h را به این فایل اضافه می‌کنیم.

#include "servo.h"

در قدم بعدی، قبل از حلقه while(1) تابع servo_init را فراخوانی می‌کنیم تا راه‌اندازی انجام شود؛

 /* USER CODE BEGIN 2 */
servo_init();
/* USER CODE END 2 */

اکنون می‌توانیم به Servo Motor فرمان دهیم که در موقعیت مورد نظر ما قرار گیرد. یا اینکه با استفاده از تابع servo_sweep زاویه خاصی را جاروب کنیم.

 servo_sweep(36, 150);

همچنین درصورتی‌که یک پایه را به‌عنوان پایه ورودی تعریف کرده باشیم می‌توانیم با نوشتن کد زیر در صورت زدن یک کلید، به Servo Motor فرمان دهیم که زاویه 180 درجه تا 0 درجه را جاروب کند و مجدداً به موقعیت 180 درجه بازگردد (این کد برای حالت ساده Dummy Project کاربرد دارد)؛

 if(((LL_GPIO_ReadInputPort(GPIOB)) & (1<<12)) == 0) 
{
for (int i = 180; i > 0; i--)
{
servo_moveto(i); 
LL_mDelay(1);
}
for (int i = 0; i < 180; i++)
{
servo_moveto(i);
LL_mDelay(20);
}
}

در صورت اجرای صحیح تمامی مراحل (به‌خصوص واردکردن صحیح طول‌موج‌های مربوط به هر زاویه) می‌توانیم انتظار داشته باشیم که Servo Motor در موقعیت‌های واردشده قرار بگیرد و حرکت کند.