Управление сервоприводом

Довелось немного поработать с сервоприводами, решил поделиться информацией.

Думаю любой человек представляет себе, что такое электродвигатель, нет? — тогда вспомните какой нибудь вентилятор. Какая характерная черта? Правильно, подали напряжение он крутится, сняли напряжение — не крутится. Сервопривод, это тоже движок, но в отличие от других, на сколько скажешь ему повернуться, на столько он и повернется и остановится. Пока держится управляющий сигнал, сервопривод будет фиксировать свое положение. Можете его хоть руками покрутить, он все равно вернется в заданное положение.

Угол на который поворачивается серва, задается шириной импульса. Стоит уточнить небольшую тонкость, сервоприводы бывают разные. Бывают такие, которые крутятся постоянно в определенную сторону, при этом ширина импульса влияет только на скорость поворота. Бывают многооборотистые. Те о которых речь пойдет дальше, на сайте производителя имеют явную маркировку, в которой указан угол поворота. Поэтому учтите если серва, не имеет явной маркировки, то может оказаться так, что она тупо постоянно вращается. Не путайте, надписи 0.20 sec/60° означают скорость вращения, они никак не связаны с максимальным углом поворота.

Перейдем к теории. Представляем себе микроконтроллер с подключенным к АЦП входу резистором R и некий движок, который крутится по ШИМ сигналу PWM. Допустим уровень сигнала АЦП напрямую связан с ШИМ выходом, тогда когда мы будем крутить резистор, то скорость будет меняться, когда напряжение АЦП станет равным 0, движок остановится.

Теперь рассмотрим вариант 2. Ручка резистора насажена на вал двигателя, таким образом, что когда двигатель вращается, он изменяет сопротивление резистора, следовательно и напряжение, которое подается на вход АЦП. При этом, если имеется еще один источник сигнала, то микроконтроллер сравнивает напряжение на входах и если оно больше, то крутит в одну сторону, если меньше, то в другую. Рано или поздно напряжения уровняются и движок остановится. Поэтому серва включает в себя все что нарисовано: резистор, микроконтроллер, двигатель. Внешний сигнал естественно подавать должны мы, чтобы управлять.

Типовые кишки выглядят так:

На фотке видно что резистор и моторчик соединяется через кучу шестеренок, поэтому если полезете внутрь будьте готовы что на вас высыпется все это добро. Вид снизу

Чаще всего ширина импульса колеблется в диапазоне от 1100мкс до 1900мкс, при периоде 20мс, но цифры могут отличаться, причем достаточно сильно. Пример из даташита:
Control System: +Pulse Width Control 1520usec Neutral
Required Pulse: 3-5 Volt Peak to Peak Square Wave
Operating Voltage: 4.8 Volts
Operating Speed (6V): 0.20sec/60 degrees at no load
Operating Angle: 45 Deg. one side pulse traveling 400usec
Continuous Rotation Modifiable: No
Direction: Counter Clockwise/Pulse Traveling 1520-1900usec

Отсюда четко видно, что средняя точка 1520мкс, чтобы повернуть такую серву на 45градусов, уже нужно подать импульс 1900мкс, соответственно остальные углы рассчитываются пропорцией. Чтобы повернуть на -45 градусов нужно подать импульс 1100мкс. Т.е. диапазон 90град. Еще видно, что под Continuous Rotation сей девайс не заточен, что хорошо.

Перейдем к практике. Есть поциент Futaba S3152, которым нужно покрутить туды — сюды.

Также можно это дело потестить в протеусе. Обычное подключение по трем проводам красный +5В, черный — земля, белый — управляющий.

В последних версиях CAVR, в Codewizard появилось много ништяков, например можно вбить цифры в попугаях и увидеть период и импульс в секундах. Собственно нам важен режим fast pwm top ICR. Примечателен этот режим тем, что ICR задает период, а OCR ширину импульса.

Период вычисляется очень просто:
ICR = (Частота таймера/50Hz)-1

Тогда нужную ширину импульса можно легко вычислить по пропорции:
20ms = ICR
?ms = OCR

В итоге можно переписать так:
OCR = (x*ICR)/20; где x это необходимая длительность импульса. Например, нужна длительность импульса в 1мс, значит OCR= (1*9C3)/20=0x7C.

Собственно и все. Теперь исходим из того, что нейтральная точка = 1524мкс или OCR1 = (1.524*9C3)/20 = 0xBE и зависимости от тогу куда нам нужно повернуть пересчитываем OCR. Простенький пример, поворачиваем на -45, затем 0 и потом +45.

#include <mega8.h>
#include <delay.h>
 
void main(void)
{
// Function: Bit7=In Bit6=In Bit5=In Bit4=In Bit3=Out Bit2=In Bit1=Out Bit0=In 
DDRB=(0<<DDB7) | (0<<DDB6) | (0<<DDB5) | (0<<DDB4) | (1<<DDB3) | (0<<DDB2) | (1<<DDB1) | (0<<DDB0);
// State: Bit7=T Bit6=T Bit5=T Bit4=T Bit3=0 Bit2=T Bit1=0 Bit0=T 
PORTB=(0<<PORTB7) | (0<<PORTB6) | (0<<PORTB5) | (0<<PORTB4) | (0<<PORTB3) | (0<<PORTB2) | (0<<PORTB1) | (0<<PORTB0);
 
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 125,000 kHz
// Mode: Fast PWM top=ICR1
// OC1A output: Non-Inverted PWM
// OC1B output: Disconnected
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer Period: 20 ms
// Output Pulse(s):
// OC1A Period: 20 ms Width: 0,9924 ms
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=(1<<COM1A1) | (0<<COM1A0) | (0<<COM1B1) | (0<<COM1B0) | (1<<WGM11) | (0<<WGM10);
TCCR1B=(0<<ICNC1) | (0<<ICES1) | (1<<WGM13) | (1<<WGM12) | (0<<CS12) | (1<<CS11) | (1<<CS10);
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x09;
ICR1L=0xC3;
OCR1AH=0x00;
OCR1AL=0x7C;
OCR1BH=0x00;
OCR1BL=0x00;
 
// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=(0<<OCIE2) | (0<<TOIE2) | (0<<TICIE1) | (0<<OCIE1A) | (0<<OCIE1B) | (0<<TOIE1) | (0<<TOIE0);
 
while (1)
      { 
        //-45
        OCR1A = (1100*0x9C3)/20000;
        delay_ms(1000);
        //0
        OCR1A = (1524*0x9C3)/20000;
        delay_ms(1000);
        //+45
        OCR1A = (1900*0x9C3)/20000;
        delay_ms(1000);
 
      }
}

Получилось так:

Для stm32 приведу пример настройки, которая помойму даже проще. Пример для stm32f103, нога PA1, тактовая 72МГц.

//Настройка ножки PA1 на альтернативную функцию
GPIO_InitTypeDef PORT_SETUP; 
 PORT_SETUP.GPIO_Mode = GPIO_Mode_AF_PP;
PORT_SETUP.GPIO_Pin = GPIO_Pin_1; 
PORT_SETUP.GPIO_Speed = GPIO_Speed_50MHz; 
GPIO_Init(GPIOA, &PORT_SETUP);
 
//настройка таймера
TIM_TimeBaseInitTypeDef TIM_SETUP; 
TIM_SETUP.TIM_CounterMode = TIM_CounterMode_Up;   
TIM_SETUP.TIM_Period = 4096;     
TIM_SETUP.TIM_Prescaler = 351;  // 72мгц/4096/351=50hz  
TIM_TimeBaseInit(TIM2, &TIM_SETUP);
 
//настройка ШИМ
TIM_OCInitTypeDef PWM_SETUP;  
//PWM_SETUP.TIM_Pulse = 200;    //4096 = 20ms 200 ~ 1ms
PWM_SETUP.TIM_Pulse = 0;   
PWM_SETUP.TIM_OCMode = TIM_OCMode_PWM2; 
PWM_SETUP.TIM_OutputState =TIM_OutputState_Enable; 
PWM_SETUP.TIM_OCPolarity = TIM_OCPolarity_Low; 
TIM_OC2Init(TIM2, &PWM_SETUP);
TIM_Cmd(TIM2, ENABLE);
 
while(1)
{
....
//где то внутри основного цикла меняем угол поворота переменной result
TIM2->CCR2 = (4096 * result)/20000;
}

И пара бонусных моментов. Особо крутые сервоприводы могут программироваться, качаешь отдельную софтинку, покупаешь спецпрограмматор и втыкаешься в те же 3 провода и можно регулировать параметры. Фактически прошивайка работает по юарту.

Еще один момент, это крепление нагрузки. Пластиковые штуки, которые крепятся на вал называются качалками.