Содержание:
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 импульса при максимальной скорости.