Магниты расположены на валу якоря коллекторного двигателя постоянного тока. Вал вращается с максимальной скоростью 90 об/сек по паспорту. Количество магнитов 6. На рисунке 4.1 показан график сигнала во времени, поступающий с датчиков Холла.
рисунок 4.1. Опрос датчиков Холла осуществляется следующим образом:
1. Фиксируется изменение состояния датчиков (текущее состояние сравнивается с предыдущим).
2. В зависимости от полученного кода судим о направлении вращения вала.
Точность такого способа опроса составляет 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 импульса при максимальной скорости.
Комментарии (0)
RSS свернуть / развернутьТолько зарегистрированные и авторизованные пользователи могут оставлять комментарии.