Я пытаюсь получить точный контроль над скоростью робота на базе Rover 5. Он имеет четыре двигателя с ШИМ-управлением и 4 оптических квадратурных энкодера. Я использую 4-канальный контроллер двигателя с шасси Rover 5 . Я использую arduino Nano для управления. Я могу считывать выходные данные кодировщика INT и изменять ШИМ в зависимости от ширины импульса для управления скоростью. Но в результате я получаю сильные колебания на выходе управления. Это заставляет робота двигаться ступенчато, так как ШИМ постоянно меняется. Мне нужен алгоритм, который может свести к минимуму этот звон и обеспечить плавное движение робота. Вот мой фрагмент кода Arduino.
void setup() {
Serial.begin(9600);
init_motors();
init_encoders();
req_speed[0] = 20;
req_speed[1] = 20;
req_speed[2] = 20;
req_speed[3] = 20;
}
void loop() {
update_encoders();
update_motors();
}
void update_motors() {
int i, err;
unsigned long req_width;
if(micros() - mtime > 2999) {
mtime = micros();
for(i=0; i<4; i++) {
digitalWrite(pins_dir[i], req_speed[i]>0);
if(mtime - change_time[i] > 50000ul && req_speed[i] != 0) {
cur_pwm[i] += 5;
}
if(req_speed[i] > 0)
cur_err[i] = req_speed[i]*10 - cur_speed[i];
else
cur_err[i] = (-req_speed[i]*10) - cur_speed[i];
if(cur_err[i] > 0 && cur_pwm[i] < 255) {
cur_pwm[i]++;
} else if(cur_err[i] < 0 && cur_pwm[i] > 0) {
cur_pwm[i]--;
}
analogWrite(pins_pwm[i], cur_pwm[i]);
}
}
}
void update_encoders() {
int i;
unsigned long w;
enc_new = PINC & B00001111;
unsigned long etime = micros();
for (i=0; i<4; i++) {
if((enc_old & (1 << i)) < (enc_new & (1 << i)))
{
w = (unsigned long)(((etime - change_time[i])));
pulse_width[i] = (w + pulse_width_h1[i] + pulse_width_h2[i])/3;
pulse_width_h2[i] = pulse_width_h1[i];
pulse_width_h1[i] = pulse_width[i];
change_time[i]=etime;
pulse_count[i]++;
cur_speed[i] = (3200000ul / pulse_width[i]);
}
}
enc_old=enc_new;
}
Здесь req_speed находится в диапазоне от -100 до 100, где знак указывает направление. Пожалуйста, рассматривайте все неопределенные переменные как глобальные. Я экспериментально измерил, что когда двигатель работает на полной скорости, ширина импульса составляет около 3200 мкс.
Выходы INT энкодеров (исключающее ИЛИ A и B) подключены к A0 через A3. ШИМ двигателя подключен к D3, D5, D6, D9.
Ардуино недостаточно быстро, чтобы догнать 4 энкодера? PinChangeInts лучше опроса?
Пожалуйста, позвольте мне предложить какие-либо улучшения в этом коде и посоветовать мне, что мне здесь не хватает.
Мне кажется, что ваш цикл управления по существу:
if speed is too slow:
increase PWM duty cycle one unit
if speed is too fast:
descreate PWM duty cycle one unit
Как вы заметили, это не работает так хорошо. Ваш контур управления циклически превышает целевую скорость.
Каноническим решением такого рода проблем является ПИД-регулятор . Концепция, по сути, та же самая: измерьте вещь и сравните ее с целью, чтобы вычислить ошибку. Затем отрегулируйте что-нибудь (в вашем случае рабочий цикл ШИМ) в зависимости от ошибки.
Однако ПИД-регулятор имеет три условия ошибки:
Для каждого из этих условий контроллер запрограммировал некоторое усиление. Затем он умножает каждый член на соответствующий коэффициент усиления, чтобы рассчитать, насколько должен измениться управляющий вход (рабочий цикл ШИМ). При правильной настройке это позволяет вам получить петлю обратной связи, которая сначала увеличивает рабочий цикл, а затем, когда ваше транспортное средство приближается к целевой скорости, плавно снижает скорость, так что вы получаете минимальный перерегулирование.
Я действительно думаю, что вы должны смотреть на прерывания, а не на опрос, по крайней мере. Я также делаю аналогичный проект энкодера (хотя и не для робота), и я решил потратить несколько долларов на отдельный чип для подсчета импульсов, которые я затем могу считывать с аналоговых контактов через ЦАП.
Это имеет несколько преимуществ:
Чип, который мне порекомендовали, находится здесь
Используя, например, аналоговый вывод Arduino -> мульти/демультиплексор -> ЦАП -> выделенный чип интерфейса кодировщика, вы можете освободить МНОГО контактов Arduino / получить гораздо больше кодировщиков;)
Возможно, вам придется замедлить реакцию ШИМ, когда вы измерите, что двигатель не работает с правильной скоростью. Я предполагаю, что вы вычисляете целевой рабочий цикл ШИМ с учетом ошибки. Это называется контролем скорости, и это то, что люди традиционно называют дифференциальным элементом в трехчленном регуляторе. По сути, простой способ добиться этого - решить, куда вам нужно двигаться, но ограничить количество в секунду, на которое вы изменяете рабочий цикл ШИМ.
В любом случае, это мнение аналогового инженера, а кто-то пишет код только в экстренных случаях!
Пунит Сони
Фил Фрост
Пунит Сони
Пол Салливан