Замена контролера сервы на Ардуино.

КонстантинБ
Offline
Зарегистрирован: 02.11.2017

Всем привет.

Пытаюсь сделать контролер сервы из Ардуино.

Спалил одну, запас есть, но интересно сделать контролер самому.

Выпаял родной контролер, подключил к Ардуино энкодер и моторчик через драйвер L298n. Схема подключения на всякий случай:

Код:

#define PIN_ENA 6      // Вывод управления скоростью вращения мотора №1
#define PIN_IN1 5      // Вывод управления направлением вращения мотора №1
#define PIN_IN2 4      // Вывод управления направлением вращения мотора №1
#define PIN_POT A7     // Чтение данных с энкодера
//#define PIN_ANGLEIN A6 // Получение угла с основного контролера

int StartAngle;
int PrevInput = -1;

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

  pinMode(PIN_POT, INPUT);
  pinMode(PIN_ENA, OUTPUT);
  pinMode(PIN_IN1, OUTPUT);
  pinMode(PIN_IN2, OUTPUT);
  //pinMode(PIN_ANGLEIN A6, INPUT);
}

void loop()
{
  int angleInput = /*map(analogRead(PIN_ANGLEIN), 0, 255, 0, 180)*/ 90;
  int angleEncoder = map(analogRead(PIN_POT), 0, 1023, 0, 180);

  if (angleInput != PrevInput)
  {
    PrevInput = angleInput;
    StartAngle = angleEncoder;
  }

  //--------------------------------------------

  if (angleInput - angleEncoder > 2) //увеличение угла
  {
    analogWrite(PIN_ENA, 90);
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, HIGH);
  }

 //--------------------------------------------

  if (angleEncoder - angleInput > 2) //уменьшение угла
  {
    analogWrite(PIN_ENA, 90);
    digitalWrite(PIN_IN1, HIGH);
    digitalWrite(PIN_IN2, LOW);
  }

 //--------------------------------------------

  if (angleInput == angleEncoder - 1 || angleInput == angleEncoder || angleInput == angleEncoder + 1)
  {
    digitalWrite(PIN_IN1, HIGH);//LOW
    digitalWrite(PIN_IN2, HIGH);//LOW
  }
}

Строки

pinMode(PIN_ANGLEIN A6, INPUT);

int angleInput = /*map(analogRead(PIN_ANGLEIN), 0, 255, 0, 180)*/ 90;

подразумевают получение сигнала от основного контролера, но пока закомментированы, а угол задаю в скетче, в данном случае 90 градусов.

Углы (переменные):

angleInput - требуемый угол (получаем с основного контролера)

PrevInput - предыдущий требуемый угол

StartAngle - угол с которого начинается движения (угол энкодера в начале движения)

angleEncoder - угол энкодера
 
 
Проблема такая:
Даже на минимальной скорости вал проезжает заданную позицию, возвращается назад, снова проезжает, снова назад и так бесконечно. Получаю колебания +-10 градусов возле заданного угла. Пытался делать противоход, т.е. при "перелете" запускал моторчик в другую сторону- не помает.
 
Вопросы:
Как этот момент реализован в сервомоторе?
Почему серва из любого угла на максимальной скорости приезжает, достаточно точно, в любой угол и без всяких колебаний останавливается в заданной точке? 
rkit
Offline
Зарегистрирован: 23.11.2016

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

Например http://www.cds.caltech.edu/~murray/amwiki/index.php/Main_Page

КонстантинБ
Offline
Зарегистрирован: 02.11.2017

rkit пишет:

PI регулятор. 

Большое спасибо за ответ.

Нашел статью http://robofob.ru/materials/articles/pages/Karpov_mobline1.pdf , очень доходчиво разжевано про ПИД-регулятор.

Разобрали каждую строчку, что делает, зачем делает. Вроде разобрались с интерпретацией результатов (ШИМ от 100 до 255, ниже 100 мотор не крутится, только пищит). Используем только пропорциональную и интегральную составляющие. return up + ui.

Код:

#define PIN_ENA 6      // Вывод управления скоростью вращения мотора №1
#define PIN_IN1 5      // Вывод управления направлением вращения мотора №1
#define PIN_IN2 4      // Вывод управления направлением вращения мотора №1
#define PIN_POT A7     // Чтение данных с энкодера
#define PIN_ANGLEIN A6 // Получение угла с основного контролера
#define iMin -0.2 // Минимальное значение интегратора
#define iMax 0.2 // Максимальное значение интегратора

// Параметры пропорционального звена
float kp = 2; // Коэффициент пропорционального звена

// Параметры интегратора
float ki = 0.001; // Коэффициент интегрального звена
float iSum = 0; // Сумма ошибок (значение, накопленное в интеграторе)

// Параметры дифференциатора
float kd = 1; // Коэффициент дифференциального звена
float old_y = 0; // Предыдущее значение сигнала

float PIDctl(float error, float y)
{
  float up, ui, ud;

  // Пропорциональная компонента
  up = kp * error;

  // Интегральная компонента

  iSum = iSum + error; // Накапливаем (суммируем)

  if (iSum < iMin) iSum = iMin; // Проверяем граничные значение
  if (iSum > iMax) iSum = iMax;

  ui = ki * iSum;

  // Дифференциальная компонента
  ud = kd * (y - old_y);
  old_y = y;

  //return up + ui + ud;
  return up + ui;
  //return up;
}

void setup()
{
  Serial.begin(9600);
  Serial.println("PID, Encoder");
  pinMode(PIN_POT, INPUT);
  pinMode(PIN_ENA, OUTPUT);
  pinMode(PIN_IN1, OUTPUT);
  pinMode(PIN_IN2, OUTPUT);
  pinMode(PIN_ANGLEIN, INPUT);
}

void loop()
{
  int angleInput = /*map(analogRead(PIN_ANGLEIN), 0, 255, 0, 180)*/ 170; //задаем угол в ручную
  int angleEncoder = map(analogRead(PIN_POT), 0, 1023, 0, 180);

  float PID = PIDctl(angleInput - angleEncoder, angleEncoder);

  if (PID < 0)
  {
    digitalWrite(PIN_IN1, HIGH);
    digitalWrite(PIN_IN2, LOW);
  }
  else if (PID > 0)
  {
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, HIGH);
  }
  else
  {
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, LOW);
  }
  
  Serial.print(PID);
  Serial.print(",");
  Serial.println(angleEncoder);

  analogWrite(PIN_ENA, map(abs(PID), 0, 1800, 100, 255));// U // при ШИМ меньше 100 мотор не крутится.
}

Но проблема с колебаниями осталась. Какие бы значения Kp и Ki не выставляли, происходят колебания вокруг заданной токчи ровно в 4 градуса, 2 в + и 2 в -.

Немного изменили концовку:

void loop()
{
  int angleInput = /*map(analogRead(PIN_ANGLEIN), 0, 255, 0, 180)*/ 170;
  int angleEncoder = map(analogRead(PIN_POT), 0, 1023, 0, 180);

  float PID = PIDctl(angleInput - angleEncoder, angleEncoder);

  if (PID < 0)
  {
    digitalWrite(PIN_IN1, HIGH);
    digitalWrite(PIN_IN2, LOW);
  }
  else if (PID > 0)
  {
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, HIGH);
  }

  Serial.print(PID);
  Serial.print(",");
  Serial.println(angleEncoder);

  analogWrite(PIN_ENA, map(abs(PID), 0, 1800, 100, 255));//U

  if (angleEncoder == angleInput)
  {
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, LOW);
    delay (5000);
  }
}

и добавили 5 сек задержку. Серва попадает в заданное положение - включается задержка. 5 сек проходит - серва отпрыгивает на два градуса, то в право, то в лево поочередно и снова возвращается в заданный угол.

Надеюсь понятно из скрина.
Причем я серву руками останавливаю в заданной точке, должна бы остановиться, а нет. Отпускаю - начинаются колебания. Т.е. она из заданной точки выпрыгивает и вокруг неё прыгает.
 
Подскажите в чем дело?
rkit
Offline
Зарегистрирован: 23.11.2016

КонстантинБ пишет:

  if (PID < 0)
  {
    digitalWrite(PIN_IN1, HIGH);
    digitalWrite(PIN_IN2, LOW);
  }
  else if (PID > 0)
  {
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, HIGH);
  }
 

А что это такое? Перечитай-ка еще раз.

КонстантинБ
Offline
Зарегистрирован: 02.11.2017

rkit пишет:

А что это такое? Перечитай-ка еще раз.

Результат функции ПИД контрол выводится с + или с -. В зависимости от знака крутим моторчик в ту или иную сторону. А модулем от ПИД через мар задаем ШИМ на пин ENA. Ведь на ENA мы не можем минусовое значение подать.

КонстантинБ
Offline
Зарегистрирован: 02.11.2017

rkit пишет:

А что это такое? Перечитай-ка еще раз.

Вот выдержка из статьи по ссылке:

Будем интерпретировать сигнал u(t) следующим образом: если u(t)<0, то будем притормаживать левый двигатель на величину, пропорциональную модулю u(t); если u(t)>0, то будем притормаживать правый двигатель на величину, пропорциональную u(t). Естественно, что если u(t)=0, то тележка едет прямо (никакие двигатели не притормаживаются).

Ну вот это я и делаю.

rkit
Offline
Зарегистрирован: 23.11.2016

КонстантинБ пишет:

на величину, пропорциональную модулю u(t);

подумай еще

КонстантинБ
Offline
Зарегистрирован: 02.11.2017

rkit пишет:

подумай еще

Блин, ну модуль это абсолютная величина. У минус 6 и у плюс 6 модуль будет шесть.

Пропорциональность я реализую через МАР.

U это управляющее воздействие.

Вот схема подключения мотора.

IN1, IN2 определяют направление движения, на пин ENA подаем ШИМ и он может быть только положительный

Другого не думается. Подскажите.

leks
Offline
Зарегистрирован: 22.10.2017

Нифига как сложно оказывается из ардуино и мотора сделать серву !

Пид какой то больше 0, меньше, а при 0 чего делать?

rkit
Offline
Зарегистрирован: 23.11.2016

Не увидел вывода шим, извиняюсь. Разрешение по углу слишком маленькое. Работай в диапазоне 0-1023, а не 0-180.

КонстантинБ
Offline
Зарегистрирован: 02.11.2017

rkit пишет:

Работай в диапазоне 0-1023, а не 0-180.

Вот здесь вообще не понял. От 0 до 180 изменяются градусы. У сервы ход 180 градусов.

b707
Offline
Зарегистрирован: 26.05.2017

КонстантинБ пишет:

Вот здесь вообще не понял. От 0 до 180 изменяются градусы. У сервы ход 180 градусов.

для контроллера нет никаких градусов, он берет сигнал с аналогового входа, с диапазоном 0-1023. А преобразуете этот диапазон к "градусам" 0-180 - вы сами в коде программы, таким образом теряя в точности в 6 раз

Вот эту строчку надо выкинуть

int angleEncoder = map(analogRead(PIN_POT), 0, 1023, 0, 180);

и работать напрямую со значениями analogRead(PIN_POT)

 

КонстантинБ
Offline
Зарегистрирован: 02.11.2017

b707 пишет:

и работать напрямую со значениями analogRead(PIN_POT)

Работает два МК. Один основной (управляет сервами), второй в роли контролера сервомотора.

Основной подает ШИМ сигнал в формате 0-255 как на настоящую серву. Второй, тот что контролер сервы, соответственно принимает 0-255. Вот заготовка по это. Временно задаю в ручную (ноль градусов).

  int angleInput = /*map(analogRead(PIN_ANGLEIN), 0, 255, 0, 180)*/ 0;

 Энкодер выдает 0-1023. Мне надо сравнивать эти значения. Все равно преобразование где то придется делать.

А потом еще на мотор передавать  0-255 (100-255).

Если я уберу строку

int angleEncoder = map(analogRead(PIN_POT), 0, 1023, 0, 180);

мне придется эту строку

int angleInput = /*map(analogRead(PIN_ANGLEIN), 0, 255, 0, 180)*/ 0;

переделать в такую

  int angleInput = /*map(analogRead(PIN_ANGLEIN), 0, 1023, 0, 180)*/ 0;

а я не могу так сделать потому, что с основного МК получаю значение в виде 0-255.

b707
Offline
Зарегистрирован: 26.05.2017

КонстантинБ пишет:

Все равно преобразование где то придется делать.

да. в любом случае что-то придется преобразовывать, весь вопрос - что выбрать.

Чем больше размерность данных на ПИД регуляторе - тем более "гладко" он работает. Поэтому 0-1023 лучше. чем  0-180.

 

КонстантинБ
Offline
Зарегистрирован: 02.11.2017

Спасибо, попробую.

Izvekoff
Offline
Зарегистрирован: 02.03.2020

leks пишет:

Нифига как сложно оказывается из ардуино и мотора сделать серву !

Пид какой то больше 0, меньше, а при 0 чего делать?

не просто же так за цифровые сервы такие деньги просят )))

КонстантинБ
Offline
Зарегистрирован: 02.11.2017
#define PIN_ENA 6
#define PIN_IN1 5
#define PIN_IN2 4
#define PIN_POT A7
#define PIN_ANGLEIN A6 // Получение угла с основного контролера
#define iMin -0.2
#define iMax 0.2

float kp = 1;

float ki = 0.001;
float iSum = 0;

float kd = 1;
float old_y = 0;

float PIDctl(float error, float y)
{
  float up, ui, ud;

  up = kp * error;

  iSum = iSum + error;

  if (iSum < iMin) iSum = iMin;
  if (iSum > iMax) iSum = iMax;

  ui = ki * iSum;

  ud = kd * (y - old_y);
  old_y = y;

  return up + ui;
}

void setup()
{
  Serial.begin(9600);
  Serial.println("PID, Encoder");
  pinMode(PIN_POT, INPUT);
  pinMode(PIN_ENA, OUTPUT);
  pinMode(PIN_IN1, OUTPUT);
  pinMode(PIN_IN2, OUTPUT);
  pinMode(PIN_ANGLEIN, INPUT);
}

void loop()
{
  //int angleInput = map(analogRead(PIN_ANGLEIN), 0, 255, 0, 1023);//заготовка под внешний сигнал
  int angleInput = 512;//ввод в ручную.512 это 90 градусов в формате 0 1023

  int angleEncoder = analogRead(PIN_POT);//читаю данные с экодера в формате 0 1023

  float PID = PIDctl(angleInput - angleEncoder, angleEncoder);

  if (PID < 0)
  {
    digitalWrite(PIN_IN1, HIGH);
    digitalWrite(PIN_IN2, LOW);
  }
  else if (PID > 0)
  {
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, HIGH);
  }

  Serial.print(PID);
  Serial.print(", ");
  Serial.println(map(angleEncoder, 0 , 1023, 0, 180));//перевожу значение с энкодера в градусы чтоб читалось удобнее

  analogWrite(PIN_ENA, map(abs(PID), 0, 1023, 100, 255));//перевожу значение ПИД в ШИМ

  if (angleEncoder == angleInput)
  {
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, LOW);

  }
}

Вычисления в ПИ-регуляторе в диапазоне 0 - 1023.

Перевожу в градусы только в монитор порта.

При значениях Кп = 0,1; 1; 2; 5; 10 выходной вал колеблется возле заданной точки в диапазоне 3-4 градуса.

Причем серва часто попадает в заданное положение (на скрине видно), но не останавливается в нем. Более того, я её руками поворачиваю в заданный угол, как только отпускаю вал поворачивает и начинаются колебания.

Толи лыжи не едут, толи одно из двух.

Люди добрые, ПАМАЖИТЕ!!!

leks
Offline
Зарегистрирован: 22.10.2017

Можно для начала попробовать без ПИДа. Брал мотор-редуктор, прикручивал переменный резистор, драйвер L9110S и гонял по условиям <=  или >= заданного. В принципе понравилось, но 65 Р за готовую серву проще оказалось в итоге.

КонстантинБ
Offline
Зарегистрирован: 02.11.2017

leks пишет:

Можно для начала попробовать без ПИДа. 

Я в первом посте описывал без ПИДа. Только управление идет не с переменного резистора а с внешнего контролера, т.к. серва в составе проекта который управляется МК (тот самый внешний МК).

Серва не останавливается в заданном положении.

b707
Offline
Зарегистрирован: 26.05.2017

КонстантинБ пишет:

Серва не останавливается в заданном положении.

Так сначала надо выяснить, почему так происходит,  где у вас в коде ошибка, прежде чем ПИД прикручивать

Вы, надеюсь, понимаете, что если даже при прямом регулировании серва не останавливается - значит ошибка есть?

leks
Offline
Зарегистрирован: 22.10.2017

Переменный резистор, жестко связанный с выходным валом сервы даёт контроллеру чёткое положение вала - "идеальная обратная связь". Иными словами как с энкодером мк узнаёт где находится сейчас вал на самом деле? Конечно ресурс такой связи ограничен, но мне кажется она легче в понимании. 

sadman41
Offline
Зарегистрирован: 19.10.2016

Существуют абсолютные энкодеры, например...

leks
Offline
Зарегистрирован: 22.10.2017

Блин, точно. Читал уже, но забыл.Вот что значит ни разу не использовать )

rkit
Offline
Зарегистрирован: 23.11.2016

КонстантинБ пишет:

Толи лыжи не едут, толи одно из двух.

Люди добрые, ПАМАЖИТЕ!!!

Да сказано сразу было, что без теории это дохлый номер. Одна статейка это не теория. Дальше нужно смотреть на помехи и увеличение пропускной полосы всей системы.

vosara
vosara аватар
Offline
Зарегистрирован: 08.02.2014

Может добавить else

if (PID < 0)
  {
    digitalWrite(PIN_IN1, HIGH);
    digitalWrite(PIN_IN2, LOW);
  }
  else if (PID > 0)
  {
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, HIGH);
  }
  //пробуйте добавить это (если 0 то стоим)
  else 
  {
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, LOW);
  }

 

КонстантинБ
Offline
Зарегистрирован: 02.11.2017

vosara пишет:

Может добавить else

Да оно собственно так и было. 

В коде из последнего сообщения сообщения, вынесли случай равенства и делали паузу. Пауза срабатывает отлично. Когда отправлял код на форум паузу убрал, а else назад не перенес.

Я увеличил скорость и разрядность ШИМ до 15.6 кГц, 10bit, в положении 0 серва стала останавливаться, в положении 180 продолжаются колебания.

Кстати микросхема в серве KC9102, инфы про неё в инете не нашел, только маленький даташит на китайском языке.

Забил, может позже еще подумаю.