Простые примеры работы Servo, ESC, PWM 10 bit, pin mapping

Работаю с Arduino, в основном работаю стандартной библиотекой Servo, она сделана не плохо и отлично работает. И но недавнего времени даже не думал о переходе на AVR для работы с Servo и ESC.

Однако потребовалась запустить данный ESC - https://world.taobao.com/item/19320941472.htm?spm=a312a.7700714.0.0.wEKSlp#detail. Особенность данного ESC в том, что это обычный ESC перепрошитый для реверса. Именно с данным ESC обычная библиотека беспомощна, но об этом чуточку позже.

Обычные примеры.
Возможно кому-то будет полезно, данные примеры под Atmega328p на таймере T1.

Пример 1.
Управление 1 Servo или ESC на 1 канал таймера.
Таймер в режиме 14 Fast PWM (ICR1), пределитель 64, длину импульса записываем в регистр OCR1A.
Значение длины сигнала в ms = OCR1A*4, к примеру OCR1A =150, длина импульса 0,6ms.

Код (C++):

// Пример для Atmega328p на T1.

/// <summary>
/// Настройка при запуске Аrduino.
/// </summary>
void setup()
{
  // Подключаем 9 пин ардуино, B1 для Atmega328p.
  DDRB |=(1<<PB1);  

  // TCCR1A - регистр управления A.
  TCCR1A = 0;

  // TCCR1B - регистр управления B.
  TCCR1B = 0;

  // TCCR1A - регистр управления A.
  // TCCR1B - регистр управления B.
  // Биты WGM13 (4) , WGM12 (3) регистра TCCR1B и биты WGM11 (1) , WGM10 (0) регистра TCCR1A устанавливают режим работы таймера/счетчика T1:
  // 0000 - обычный режим
  // 0001 - коррекция фазы PWM, 8-бит
  // 0010 - коррекция фазы PWM, 9-бит
  // 0011 - коррекция фазы PWM, 10-бит
  // 0100 - режим счета импульсов (OCR1A) (сброс при совпадении)
  // 0101 - PWM, 8-бит
  // 0110 - PWM, 9-бит
  // 0111 - PWM, 10-бит
  // 1000 - коррекция фазы и частоты PWM (ICR1)
  // 1001 - коррекция фазы и частоты PWM (OCR1A)
  // 1010 - коррекция фазы PWM (ICR1)
  // 1011 - коррекция фазы и частоты PWM (OCR1A)
  // 1100 - режим счета импульсов (ICR1) (сброс при совпадении)
  // 1101 - резерв
  // 1110 - PWM (ICR1)
  // 1111 - PWM (OCR1A)

  // 1110 - PWM (ICR1)
  TCCR1A |= (1 << WGM11) | (0 << WGM10);
  TCCR1B |= (1 << WGM12) | (1 << WGM13);

  // ICR1 - регистр захвата (16 бит)
  ICR1 = 5000;

  // OCR1A - регистр сравнения A (16 бит)
  OCR1A = 150;

  // TCCR1A - регистр управления A.
  // Биты COM1A1 (7) и COM1A0 (6) влияют на то, какой сигнал появится на выводе OC1A (15 ножка) при совпадении с A (совпадение значения счетного регистра TCNT1 со значением регистра сравнения OCR1A):
  /* 1. Обычный режим
  00 - вывод OC1A не функционирует
  01 - изменение состояния вывода OC1A на противоположное при совпадении с A
  10 - сброс вывода OC1A в 0 при совпадении с A
  11 - установка вывода OC1A в 1 при совпадении с A
      2. Режим ШИМ
  00 - вывод OC1A не функционирует
  01 - если биты WGM13 - WGM10 установлены в (0000 - 1101), вывод OC1A не функционирует
  01 - если биты WGM13 - WGM10 установлены в 1110 или 1111, изменение состояния вывода OC0A на противоположное при совпадении с A
  10 - сброс вывода OC1A в 0 при совпадении с A, установка  вывода OC1A в 1 если регистр TCNT1 принимает значение 0x00 (неинверсный режим)
  11 - установка вывода OC1A в 1 при совпадении с A, установка  вывода OC1A в 0 если регистр TCNT1 принимает значение 0x00  (инверсный режим)
      3. Режим коррекции фазы ШИМ
  00 - вывод OC1A не функционирует
  01 - если биты WGM13 - WGM10 установлены в (0000 - 1100, 1010, 1100 - 1111), вывод OC1A не функционирует
  01 - если биты WGM13 - WGM10 установлены в 1101 или 1011, изменение состояния вывода OC1A на противоположное при совпадении с A
  10 - сброс вывода OC1A в 0 при совпадении с A во время увеличения значения счетчика, установка  вывода OC1A в 1  при совпадении с A во время уменьшения значения счетчика
  11 - установка вывода OC1A в 1 при совпадении с A во время увеличения значения счетчика, сброс  вывода OC1A в 0  при совпадении с A во время уменьшения значения счетчика
  */
  TCCR1A |= (1 << COM1A1) | (0 << COM1A0);

  // TCCR1B - регистр управления B.
  // Биты CS12 (2), CS11 (1), CS10 (0) регистра TCCR1B устанавливают режим тактирования и предделителя тактовой частоты таймера/счетчика T1:
  // 000 - таймер/счетчик T1 остановлен
  // 001 - тактовый генератор CLK
  // 010 - CLK/8
  // 011 - CLK/64
  // 100 - CLK/256
  // 101 - CLK/1024
  // 110 - внешний источник на выводе T1 (11 ножка) по спаду сигнала
  // 111 - внешний источник на выводе T1 (11 ножка) по возрастанию сигнала
  // 011 - CLK/64
  TCCR1B |= (0 << CS12) | (1 << CS11) | (1 << CS10);
}

/// <summary>
/// Основной цикл.
/// </summary>
void loop()
{

}
 

Данная идея была подсмотрена тут.

Автор Александр Писанец, но я отказался от прерываний.

 

Пример 2.
Управление до 8 Servo или ESC на 1 канал таймера.
Таймер в режиме 4 CTC (OCR1A), пределитель 64, длину импульса записываем в регистр OCR1A.
Значение длины сигнала в ms = OCR1A*4, к примеру OCR1A =150, длина импульса 0,6ms.

Код (C++):

// 4-верть сигнала в 50Hz, импульсы идут одие за другим.
#define SERVO_CYCLE 1250U

// Угол поворота сервы (1 тик таймера 0,004ms).
// 1 серва.
volatile uint16_t servoAngle1 = 150;
// 2 серва.
volatile uint16_t servoAngle2 = 300;
// 3 серва.
volatile uint16_t servoAngle3 = 450;
// 4 серва.
volatile uint16_t servoAngle4 = 600;
// Переменная номер шага для прерывния TIMER1_COMPA_vect.
volatile uint8_t servoTakt = 0;

/// <summary>
/// Настройка при запуске Аrduino.
/// </summary>
void setup()
{
  // Подключаем 9,10,11,12 пин ардуино, B1-B4 для Atmega328p.
  DDRB |=(1<<PB1)|(1<<PB2)|(1<<PB3)|(1<<PB4);    

  // TCCR1A - регистр управления A.
  TCCR1A = 0;

  // TCCR1B - регистр управления B.
  TCCR1B = 0;

  // TCCR1A - регистр управления A.
  // TCCR1B - регистр управления B.
  // Биты WGM13 (4) , WGM12 (3) регистра TCCR1B и биты WGM11 (1) , WGM10 (0) регистра TCCR1A устанавливают режим работы таймера/счетчика T1:
  // 0000 - обычный режим
  // 0001 - коррекция фазы PWM, 8-бит
  // 0010 - коррекция фазы PWM, 9-бит
  // 0011 - коррекция фазы PWM, 10-бит
  // 0100 - режим счета импульсов (OCR1A) (сброс при совпадении)
  // 0101 - PWM, 8-бит
  // 0110 - PWM, 9-бит
  // 0111 - PWM, 10-бит
  // 1000 - коррекция фазы и частоты PWM (ICR1)
  // 1001 - коррекция фазы и частоты PWM (OCR1A)
  // 1010 - коррекция фазы PWM (ICR1)
  // 1011 - коррекция фазы и частоты PWM (OCR1A)
  // 1100 - режим счета импульсов (ICR1) (сброс при совпадении)
  // 1101 - резерв
  // 1110 - PWM (ICR1)
  // 1111 - PWM (OCR1A)

  // 0100 - режим счета импульсов (OCR1A) (сброс при совпадении)
  TCCR1A |= (0 << WGM11) | (0 << WGM10);
  TCCR1B |= (1 << WGM12) | (0 << WGM13);

  // TCCR1A - регистр управления A.
  // Биты COM1A1 (7) и COM1A0 (6) влияют на то, какой сигнал появится на выводе OC1A (15 ножка) при совпадении с A (совпадение значения счетного регистра TCNT1 со значением регистра сравнения OCR1A):
  /* 1. Обычный режим
  00 - вывод OC1A не функционирует
  01 - изменение состояния вывода OC1A на противоположное при совпадении с A
  10 - сброс вывода OC1A в 0 при совпадении с A
  11 - установка вывода OC1A в 1 при совпадении с A
      2. Режим ШИМ
  00 - вывод OC1A не функционирует
  01 - если биты WGM13 - WGM10 установлены в (0000 - 1101), вывод OC1A не функционирует
  01 - если биты WGM13 - WGM10 установлены в 1110 или 1111, изменение состояния вывода OC0A на противоположное при совпадении с A
  10 - сброс вывода OC1A в 0 при совпадении с A, установка  вывода OC1A в 1 если регистр TCNT1 принимает значение 0x00 (неинверсный режим)
  11 - установка вывода OC1A в 1 при совпадении с A, установка  вывода OC1A в 0 если регистр TCNT1 принимает значение 0x00  (инверсный режим)
      3. Режим коррекции фазы ШИМ
  00 - вывод OC1A не функционирует
  01 - если биты WGM13 - WGM10 установлены в (0000 - 1100, 1010, 1100 - 1111), вывод OC1A не функционирует
  01 - если биты WGM13 - WGM10 установлены в 1101 или 1011, изменение состояния вывода OC1A на противоположное при совпадении с A
  10 - сброс вывода OC1A в 0 при совпадении с A во время увеличения значения счетчика, установка  вывода OC1A в 1  при совпадении с A во время уменьшения значения счетчика
  11 - установка вывода OC1A в 1 при совпадении с A во время увеличения значения счетчика, сброс  вывода OC1A в 0  при совпадении с A во время уменьшения значения счетчика
  */
  TCCR1A |= (0 << COM1A1) | (0 << COM1A0);

  // Разрешают прерывания при совпадении с A.
  TIMSK1 |= (1 << OCIE1A);

  // TCCR1B - регистр управления B.
  // Биты CS12 (2), CS11 (1), CS10 (0) регистра TCCR1B устанавливают режим тактирования и предделителя тактовой частоты таймера/счетчика T1:
  // 000 - таймер/счетчик T1 остановлен
  // 001 - тактовый генератор CLK
  // 010 - CLK/8
  // 011 - CLK/64
  // 100 - CLK/256
  // 101 - CLK/1024
  // 110 - внешний источник на выводе T1 (11 ножка) по спаду сигнала
  // 111 - внешний источник на выводе T1 (11 ножка) по возрастанию сигнала
  // 011 - CLK/64
  TCCR1B |= (0 << CS12) | (1 << CS11) | (1 << CS10);
}

/// <summary>
/// Основной цикл.
/// </summary>
void loop()
{
  // put your main code here, to run repeatedly:

}

// Прерывание при совпадении с регистром OCR1A.
ISR(TIMER1_COMPA_vect)
{
  // Шаг 0.
  if (servoTakt == 0)
  {
    PORTB |= (1 << PB1);
    OCR1A = servoAngle1;
  }
  // Шаг 1.
  if (servoTakt == 1)
  {
    PORTB &= ~(1 << PB1);
    OCR1A = SERVO_CYCLE - servoAngle1;
  }
  // Шаг 2.
  if (servoTakt == 2)
  {
    PORTB |= (1 << PB2);
    OCR1A = servoAngle2;
  }
  // Шаг 3.
  if (servoTakt == 3)
  {
    PORTB &= ~(1 << PB2);
    OCR1A = SERVO_CYCLE - servoAngle2;
  }
  // Шаг 4.
  if (servoTakt == 4)
  {
    PORTB |= (1 << PB3);
    OCR1A = servoAngle3;
  }
  // Шаг 5.
  if (servoTakt == 5)
  {
    PORTB &= ~(1 << PB3);
    OCR1A = SERVO_CYCLE - servoAngle3;
  }
  // Шаг 6.
  if (servoTakt == 6)
  {
    PORTB |= (1 << PB4);
    OCR1A = servoAngle4;
  }
  // Шаг 7.
  if (servoTakt == 7)
  {
    PORTB &= ~(1 << PB4);
    OCR1A = SERVO_CYCLE - servoAngle4;
  }

  // Увеличиваем шаг.
  servoTakt++;

  // Обнуляем шаг.
  if (servoTakt == 8)
  {
    servoTakt = 0;
  }
};

Эта идея была подсмотрена тут - http://www.customelectronics.ru/servo-control/, не понял кто автор, взята без изменений.

 

Теперь код для работы с данным китайским ESC, из-за которого пришлось разбираться с AVR. С частотой 50Hz, как и 60Hz запустить реверс мне не удалось, а вот на частоте 250Hz и 333Hz удалось. Реверс запускается через нейтральное положение, скачала на нейтральное положение, потом идем на реверс.

Код аналогичный первому примеру. С той лишь разницей, что частота 333Hz и пределитель 8 (сделано для более плавной регулировки импульса).

Управление 1 Servo или ESC на 1 канал таймера.
Таймер в режиме 14 Fast PWM (ICR1), пределитель 8, длину импульса записываем в регистр OCR1A.
Значение длины сигнала в ms = OCR1A/2, к примеру OCR1A =3000, длина импульса 1,5ms.

Код (C++):

// Пример для Atmega328p на T1.

void setup()
{
  Serial.begin(115200);

  // Подключаем 9 пин ардуино, B1 для Atmega328p.
  DDRB |=(1<<PB1);  

  // TCCR1A - регистр управления A.
  TCCR1A = 0;

  // TCCR1B - регистр управления B.
  TCCR1B = 0;

  // TCCR1A - регистр управления A.
  // TCCR1B - регистр управления B.
  // Биты WGM13 (4) , WGM12 (3) регистра TCCR1B и биты WGM11 (1) , WGM10 (0) регистра TCCR1A устанавливают режим работы таймера/счетчика T1:
  // 0000 - обычный режим
  // 0001 - коррекция фазы PWM, 8-бит
  // 0010 - коррекция фазы PWM, 9-бит
  // 0011 - коррекция фазы PWM, 10-бит
  // 0100 - режим счета импульсов (OCR1A) (сброс при совпадении)
  // 0101 - PWM, 8-бит
  // 0110 - PWM, 9-бит
  // 0111 - PWM, 10-бит
  // 1000 - коррекция фазы и частоты PWM (ICR1)
  // 1001 - коррекция фазы и частоты PWM (OCR1A)
  // 1010 - коррекция фазы PWM (ICR1)
  // 1011 - коррекция фазы и частоты PWM (OCR1A)
  // 1100 - режим счета импульсов (ICR1) (сброс при совпадении)
  // 1101 - резерв
  // 1110 - PWM (ICR1)
  // 1111 - PWM (OCR1A)

  // 1110 - PWM (ICR1)
  TCCR1A |= (1 << WGM11) | (0 << WGM10);
  TCCR1B |= (1 << WGM12) | (1 << WGM13);

  // ICR1 - регистр захвата (16 бит)
  ICR1 = 6000;

  // OCR1A - регистр сравнения A (16 бит)
  OCR1A = 3000;

  // TCCR1A - регистр управления A.
  // Биты COM1A1 (7) и COM1A0 (6) влияют на то, какой сигнал появится на выводе OC1A (15 ножка) при совпадении с A (совпадение значения счетного регистра TCNT1 со значением регистра сравнения OCR1A):
  /* 1. Обычный режим
  00 - вывод OC1A не функционирует
  01 - изменение состояния вывода OC1A на противоположное при совпадении с A
  10 - сброс вывода OC1A в 0 при совпадении с A
  11 - установка вывода OC1A в 1 при совпадении с A
      2. Режим ШИМ
  00 - вывод OC1A не функционирует
  01 - если биты WGM13 - WGM10 установлены в (0000 - 1101), вывод OC1A не функционирует
  01 - если биты WGM13 - WGM10 установлены в 1110 или 1111, изменение состояния вывода OC0A на противоположное при совпадении с A
  10 - сброс вывода OC1A в 0 при совпадении с A, установка  вывода OC1A в 1 если регистр TCNT1 принимает значение 0x00 (неинверсный режим)
  11 - установка вывода OC1A в 1 при совпадении с A, установка  вывода OC1A в 0 если регистр TCNT1 принимает значение 0x00  (инверсный режим)
      3. Режим коррекции фазы ШИМ
  00 - вывод OC1A не функционирует
  01 - если биты WGM13 - WGM10 установлены в (0000 - 1100, 1010, 1100 - 1111), вывод OC1A не функционирует
  01 - если биты WGM13 - WGM10 установлены в 1101 или 1011, изменение состояния вывода OC1A на противоположное при совпадении с A
  10 - сброс вывода OC1A в 0 при совпадении с A во время увеличения значения счетчика, установка  вывода OC1A в 1  при совпадении с A во время уменьшения значения счетчика
  11 - установка вывода OC1A в 1 при совпадении с A во время увеличения значения счетчика, сброс  вывода OC1A в 0  при совпадении с A во время уменьшения значения счетчика
  */
  TCCR1A |= (1 << COM1A1) | (0 << COM1A0);

  // TCCR1B - регистр управления B.
  // Биты CS12 (2), CS11 (1), CS10 (0) регистра TCCR1B устанавливают режим тактирования и предделителя тактовой частоты таймера/счетчика T1:
  // 000 - таймер/счетчик T1 остановлен
  // 001 - тактовый генератор CLK
  // 010 - CLK/8
  // 011 - CLK/64
  // 100 - CLK/256
  // 101 - CLK/1024
  // 110 - внешний источник на выводе T1 (11 ножка) по спаду сигнала
  // 111 - внешний источник на выводе T1 (11 ножка) по возрастанию сигнала
  // 011 - CLK/64
  TCCR1B |= (0 << CS12) | (1 << CS11) | (0 << CS10);
}

void loop()
{
  if (Serial.available())
  {
    int newValue = Serial.parseInt();
    cli();
    OCR1A = newValue;
    sei();
  }
}

В цикле добавлено управление через Serial, для проверки данного чудо ESC.
Всем удачи!

 

Сегодня попросили сделать 10 битный ШИМ в режиме коррекции фазы и частоты.
Возможно, пригодится..

Данный пример для Atmega2560 (Arduino Mega 2560 R3), для таймера T5. Но этот пример можно перенести и на Atmega328 и Atmega32u4, он подходит для всех 16 битных таймеров данных микропроцессоров.

Инициализация.

Код (C++):

void PwmInit(void)
{
  // TCCR5A - регистр управления A.
  TCCR5A = 0;

  // TCCR5B - регистр управления B.
  TCCR5B = 0;

  // TCCR5C - регистр управления C.
  TCCR5C = 0;

  // TCCR5A - регистр управления A.
  // TCCR5B - регистр управления B.
  // Биты WGM53 (4) , WGM52 (3) регистра TCCR5B и биты WGM51 (1) , WGM50 (0) регистра TCCR5A устанавливают режим работы таймера/счетчика T5:
  // 0000 - обычный режим
  // 0001 - коррекция фазы PWM, 8-бит
  // 0010 - коррекция фазы PWM, 9-бит
  // 0011 - коррекция фазы PWM, 10-бит
  // 0100 - режим счета импульсов (OCR5A) (сброс при совпадении)
  // 0101 - PWM, 8-бит
  // 0110 - PWM, 9-бит
  // 0111 - PWM, 10-бит
  // 1000 - коррекция фазы и частоты PWM (ICR5)
  // 1001 - коррекция фазы и частоты PWM (OCR5A)
  // 1010 - коррекция фазы PWM (ICR5)
  // 1011 - коррекция фазы и частоты PWM (OCR5A)
  // 1100 - режим счета импульсов (ICR5) (сброс при совпадении)
  // 1101 - резерв
  // 1110 - PWM (ICR5)
  // 1111 - PWM (OCR5A)

  // 0101 - PWM, 8-бит
  //TCCR5A |= (1 << WGM51) | (1 << WGM50);
  //TCCR5B |= (1 << WGM52) | (0 << WGM53);

  // 1000 - коррекция фазы и частоты PWM (ICR5)
  TCCR5A |= (0 << WGM51) | (0 << WGM50);
  TCCR5B |= (0 << WGM52) | (1 << WGM53);

  // ICR5 - регистр захвата (16 бит)
  ICR5 = 1023;

  // OCR5A - регистр сравнения A (16 бит)
  OCR5A = 0;

  // OCR5B - регистр сравнения B (16 бит)
  OCR5B = 0;

  // OCR5C - регистр сравнения C (16 бит)
  OCR5C = 0;

  // TCCR5A - регистр управления A.
  // Биты COM5A1 (7) и COM5A0 (6) влияют на то, какой сигнал появится на выводе OC5A (15 ножка) при совпадении с A (совпадение значения счетного регистра TCNT5 со значением регистра сравнения OCR5A):
  /* 1. Обычный режим
    00 - вывод OC5A не функционирует
    01 - изменение состояния вывода OC1A на противоположное при совпадении с A
    10 - сброс вывода OC5A в 0 при совпадении с A
    11 - установка вывода OC5A в 1 при совпадении с A
      2. Режим ШИМ
    00 - вывод OC1A не функционирует
    01 - если биты WGM53 - WGM50 установлены в (0000 - 1101), вывод OC5A не функционирует
    01 - если биты WGM53 - WGM50 установлены в 1110 или 1111, изменение состояния вывода OC5A на противоположное при совпадении с A
    10 - сброс вывода OC5A в 0 при совпадении с A, установка  вывода OC5A в 1 если регистр TCNT5 принимает значение 0x00 (неинверсный режим)
    11 - установка вывода OC5A в 1 при совпадении с A, установка  вывода OC5A в 0 если регистр TCNT5 принимает значение 0x00  (инверсный режим)
      3. Режим коррекции фазы ШИМ
    00 - вывод OC5A не функционирует
    01 - если биты WGM53 - WGM50 установлены в (0000 - 1100, 1010, 1100 - 1111), вывод OC5A не функционирует
    01 - если биты WGM53 - WGM50 установлены в 1101 или 1011, изменение состояния вывода OC5A на противоположное при совпадении с A
    10 - сброс вывода OC5A в 0 при совпадении с A во время увеличения значения счетчика, установка  вывода OC5A в 1  при совпадении с A во время уменьшения значения счетчика
    11 - установка вывода OC5A в 1 при совпадении с A во время увеличения значения счетчика, сброс  вывода OC5A в 0  при совпадении с A во время уменьшения значения счетчика
  */
  TCCR5A |= (1 << COM5A1) | (0 << COM5A0) | (1 << COM5B1) | (0 << COM5B0) | (1 << COM5C1) | (0 << COM5C0);

  // TCCR5B - регистр управления B.
  // Биты CS52 (2), CS51 (1), CS50 (0) регистра TCCR5B устанавливают режим тактирования и предделителя тактовой частоты таймера/счетчика T5:
  // 000 - таймер/счетчик T5 остановлен
  // 001 - тактовый генератор CLK
  // 010 - CLK/8
  // 011 - CLK/64
  // 100 - CLK/256
  // 101 - CLK/1024
  // 110 - внешний источник на выводе T5 (PL2 ножка) по спаду сигнала
  // 111 - внешний источник на выводе T5 (PL2 ножка) по возрастанию сигнала
  // 011 - CLK/8
  TCCR5B |= (0 << CS52) | (0 << CS51) | (1 << CS50);
}

Максимальная частота в таком режиме - 7813hz.

Устанавливаем значения.

Код (C++):

// OCR5A - регистр сравнения A (16 бит). Пин PL3 (46 пин).
OCR5A = 255;

// OCR5B - регистр сравнения B (16 бит). Пин PL4 (45 пин).
OCR5B = 511;

// OCR5C - регистр сравнения C (16 бит). Пин PL5 (44 пин).
OCR5C = 1023;

 

И сделал пин маппинг Arduino Mega 2560 R3, Arduino Nano.
Где-то видел у одного из форумчан, но не смог найти тему, чтобы разместить в одном месте.

Как это выглядит.


Сам файл Pin Mapping.xlsx в прищепке Pin Mapping.zip