3.Энкодер. Разработка. Разработка алгоритма.


Содержание:
1. Введение. Исходные данные. Требования. Выбор типа датчика.
2. Выбор компонентов. Разработка конструкции энкодера.
3. Разработка алгоритма
4. Тестирование

3. Разработка алгоритма обработки информации

Магниты расположены на валу якоря коллекторного двигателя постоянного тока. Вал вращается с максимальной скоростью 90 об/сек по паспорту. Количество магнитов 6. На рисунке 4.1 показан график сигнала во времени, поступающий с датчиков Холла.


рисунок 4.1.

Опрос датчиков Холла осуществляется следующим образом:
1. Фиксируется изменение состояния датчиков (текущее состояние сравнивается с предыдущим).
2. В зависимости от полученного кода судим о направлении вращения вала.

Программа на отладочной плате Arduino UNO:

    z1=digitalRead(sensor1);
    z2=digitalRead(sensor2);
    if ((z1 != z1_old) || (z2 != z2_old))
    {
       if (((z1_old==0) && (z2==0) && (z1 == 1)) || ((z1_old==1) && (z2==1) && (z1 == 0)) || ((z2_old==0) && (z2==1) && (z1 == 1)) || ((z2_old==1) && (z2==0) && (z1 == 0)))
      FWD++;
       else if (((z1==0) && (z2_old==0) && (z2 == 1)) || ((z1==1) && (z2_old==1) && (z2 == 0)) || ((z1==1) && (z1_old==0) && (z2 == 1)) || ((z1==0) && (z1_old==1) && (z2 == 0)))
      REV++;
    angel=(FWD-REV)*12.000/25.000;

    }
    z1_old=z1;
    z2_old=z2;

Точность такого способа опроса составляет 0.5 градуса. Но при этом время опроса приблизительно равно 0.0001сек.

Программа поворота (PID регулятор) вала двигателя на заданный угол:

//PID
   error=turn-abs(angel);                              //Вычисление ошибки
   integral=integral+(error*dt);                       //Вычисление интеграла
   derivative=(previous_error-error)/dt;               //Вычисление дифференциала
   power=k_prop*error+k_int*integral+k_der*derivative; //Мощность, подаваемая на двигатель
   previous_error=error;

Скорость опроса в 0.1мсек достаточна.

Это были основные узлы программы.
А вот и вся программа целеком (просто чтобы показать где что на каком месте):
(библиотека Motorcontrol.h подает analogWrite() на соответствующие пины с задержкой между переключениями. Мы ее решили написать чтобы предохранить H-мост от выгорания)

#include <Motorcontrol.h>
MotorControl robot (3, 5);
int sensor1=2;
int sensor2=4;
int FWD=0, REV=0, ON=0, motor=0, power=0, turn=0, old_turn, i=0, warning;
boolean z1=0, z1_old=0, z2=0, z2_old=0, puls=0, old_puls=0, speed_0=0, speed_1=0;
float angel=0, angel_speed=0, old_angel=0, old_time=0, current_time=0, dt=0.000176, _dt, transition_process=0,
error,previous_error, integral, derivative, k_prop=0.8, k_int=0.006, k_der=1;                                           //Параметры PID регулятора

void setup ()
{
  pinMode (sensor1, INPUT);
  pinMode (sensor2, INPUT);
  pinMode (12, INPUT);
  pinMode (1, INPUT);
  Serial.begin(9600);
}

void loop()
{
  //Управление двигателем кнопкой
  puls=digitalRead(12);
  turn=analogRead(1);
  if ((old_puls != puls) && (puls==0))
  {
  motor++;
  if (motor==3)
  {
  robot.Stop();
  motor=0;
  ON=1;
  }
  }
  old_puls=puls;

  Serial.print(turn);
  Serial.println();


  //Поворот  на заданный угол
  if (motor==1)
  {
    transition_process=millis();
    while (speed_0 != 1) //Погрешности в 1 градус. Можно увеличить точность до 0.5 градуса
  {

    if (i == 2000) //Защиту здесь. Автоматическое выключение при недостатке мощности. 5000 циклов после начала.
    {
      if ( 0<= abs(angel_speed) <= 1)
      warning++;
      else warning=0;
      if (warning == 10)
      speed_0=1;
      i=0;
    }
    i++;

    z1=digitalRead(sensor1);
    z2=digitalRead(sensor2);
    if ((z1 != z1_old) || (z2 != z2_old))
    {
       if (((z1_old==0) && (z2==0) && (z1 == 1)) || ((z1_old==1) && (z2==1) && (z1 == 0)) || ((z2_old==0) && (z2==1) && (z1 == 1)) || ((z2_old==1) && (z2==0) && (z1 == 0)))
      FWD++;
       else if (((z1==0) && (z2_old==0) && (z2 == 1)) || ((z1==1) && (z2_old==1) && (z2 == 0)) || ((z1==1) && (z1_old==0) && (z2 == 1)) || ((z1==0) && (z1_old==1) && (z2 == 0)))
      REV++;
    angel=(FWD-REV)*12.000/25.000;

    }
    z1_old=z1;
    z2_old=z2;

    //вычисление периода опроса
    current_time=micros();
    _dt=(old_time-current_time)*1000;

      //PID
   error=turn-abs(angel);                              //Вычисление ошибки
   integral=integral+(error*dt);                       //Вычисление интеграла
   derivative=(previous_error-error)/dt;               //Вычисление дифференциала
   power=k_prop*error+k_int*integral+k_der*derivative; //Мощность, подаваемая на двигатель
   previous_error=error;

   //вычисление угловой скорости
   angel_speed=(angel-old_angel)/dt;
   old_angel=angel;

   old_time=current_time;




    //Управление двигателем

    if (power >= 255) power=255;              //Дополнительное фильтрование
      else if (power < 24) power=24;
    if ((error) < 0)
      robot.Forward(power,0);
    else
      robot.Backward(power,0);


    }
    motor=2;
    robot.Stop();
    transition_process=(current_time/1000-transition_process)/1000;
  }


  //Печать данных
   if (ON==1)
  {
  Serial.print(FWD);
  Serial.println();
  Serial.print(REV);
  Serial.println();
  Serial.print(angel);
  Serial.println();
  Serial.print(angel_speed);
  Serial.println();
  Serial.print(dt);
  Serial.println();
  Serial.println();
  Serial.print(error);
  Serial.println();
  Serial.print(derivative);
  Serial.println();
  Serial.print(integral);
  Serial.println();
  Serial.print(transition_process);
  Serial.println();
  Serial.println(_dt);
  Serial.println();
  delay(7000);
  ON=0;
  FWD=0;
  REV=0;
  puls=0;
  angel=0;
  current_time=0;
  angel_speed=0;
  power=0;
  turn=0;
  i=0;
  error=0;
  previous_error=0;
  speed_0=0;
  old_time=0;
  previous_error=0;
  transition_process=0;
  integral=0;
  derivative=0;
  warning=0;
  }

  }

//750 импульсов на 1 оборот колеса.
//12 импулльсов на 1 оборот ротора двигателя
//90*12=1080имп/сек
//минимальное время одного импульса 1/1080=0.93мсек.
//Примерно 1 мсек длительность 1 импульса при максимальной скорости.

Добавить комментарий

Arduino

Что такое Arduino?
Зачем мне Arduino?
Начало работы с Arduino
Для начинающих ардуинщиков
Радиодетали (точка входа для начинающих ардуинщиков)
Первые шаги с Arduino

Разделы

  1. Преимуществ нет, за исключением читабельности: тип bool обычно имеет размер 1 байт, как и uint8_t. Думаю, компилятор в обоих случаях…

  2. Добрый день! Я недавно начал изучать программирование под STM32 и ваши уроки просто бесценны! Хотел узнать зачем использовать переменную типа…

3D-печать AI Android Arduino Bluetooth CraftDuino DIY IDE iRobot Kinect LEGO OpenCV Open Source Python Raspberry Pi RoboCraft ROS swarm ИК автоматизация андроид балансировать бионика версия видео военный датчик дрон интерфейс камера кибервесна манипулятор машинное обучение наше нейронная сеть подводный пылесос работа распознавание робот робототехника светодиод сервомашинка собака управление ходить шаг за шагом шаговый двигатель шилд юмор

OpenCV
Робототехника
Будущее за бионическими роботами?
Нейронная сеть - введение