- Фото робота
Причем выходной вал выходит у него с двух сторон, с одной стороны крепится колесо, а с другой стороны я приделал диск для энкодера с 20 прорезями.
- Фото энкодера
Микроконтроллер на управляющей платке стоит Atmega328, и прерывания с энкодеров заведены на PCINT12 и PCINT13, т.е. обработчик прерывания у них один. Поскольку прорезей на диске не так уж много, а обороты колеса небольшие, я считаю как восходящие фронты, так и падающие, итого у меня получается 40 импульсов энкодера за 1 оборот колеса. Но моторы то разные (хоть и одинаковые
), так что крутятся с разной скоростью, и тележка едет по дуге. Поэтому я решил закодить PID регулятор, который бы держал обороты колёс на одном уровне, даже под разной нагрузкой. Вот что у меня получилось- Код: Выделить всё • Развернуть
#include "pid.h"
// желаемая скорость моторов
uint16_t left_speed = 15;
uint16_t right_speed = 15;
// включение и выключение регулятора
uint8_t pid_enable;
static int16_t left_prev_error = 0;
static int16_t right_prev_error = 0;
static float left_integral = 0;
static float right_integral = 0;
static uint8_t overflows = 0;
void pid_init()
{
// настройка таймера для вызова прерываний
// предделитель 1024
TCCR2B |= _BV(CS22) | _BV(CS21) | _BV(CS20);
TIMSK2 |= _BV(TOIE2);
}
void pid_process(void)
{
int16_t l_pwm = pid_compute(encoder[LEFT], left_speed, &left_prev_error, &left_integral);
int16_t r_pwm = pid_compute(encoder[RIGHT]-3, right_speed, &right_prev_error, &right_integral);
motor_setspeed(MLeft, l_pwm);
motor_setspeed(MRight, r_pwm);
encoder[LEFT] = 0;
encoder[RIGHT] = 0;
}
int16_t pid_compute(int measured, int setpoint, int16_t *prev_error, float *integral)
{
int16_t error = setpoint - measured;
int16_t output;
float derivate;
*integral += (error * PID_INTERVAL);
if(*integral > MOTOR_MAX_SPEED) *integral = MOTOR_MAX_SPEED;
if(*integral < MOTOR_MIN_SPEED) *integral = MOTOR_MIN_SPEED;
derivate = (error - (*prev_error)) / (float)PID_INTERVAL;
output = (kP * error) + (kI * (*integral)) + (kD * derivate);
*prev_error = error;
if(output > MOTOR_MAX_SPEED) output = MOTOR_MAX_SPEED;
if(output < MOTOR_MIN_SPEED) output = MOTOR_MIN_SPEED;
return output;
}
ISR(TIMER2_OVF_vect)
{
if(overflows < 8){
++overflows;
} else {
// примерно 1/10 секунды прошла, запускаем пид регулятор
pid_process();
overflows = 0;
PORTC ^= _BV(Led);
}
}
Желаемая скорость моторов задана переменными left_speed и right_speed (в импульсах энкодера, т.е. скорость 15 соответствует 15 импульсам энкодера). Функция pid_process() обслуживает прерывания и вызывается примерно раз, в 261мс, после восьми пререполнений таймера 2. Частота контроллера 8МГц, предделитель таймера 1024. Используемые макросы объявлены в def.h, вот его часть
- Код: Выделить всё • Развернуть
/**********************************
* коэффициенты ПИД-регулятора *
**********************************/
#define kP 7
#define kI 0.08
#define kD 0.002
#define PID_INTERVAL 261
/*******************************
* настройки моторчиков *
*******************************/
#define MOTOR_MAX_SPEED 255
#define MOTOR_MIN_SPEED 0
Проблема в том, что я никак не могу подобрать правильные коэффициенты для регулятора, уже пробовал по всякому. И код вроде верный все перепроверил. А моторчики все равно крутятся неравномерно и с рывками, движения по прямой так получить и не удалось. Я ожидал, что когда колесо псовсем заблокировано (ну или на грани блокировки), регулятор будет увеличивать мощность моторов до максимума, но нет, я вчера осциллом смотрел, может 2/3 заполнение только. Если увеличивать пропорциональный коэффициент, то можно добиться мощности 100% при блокировке вала, только нестабильно это как-то все получается. В чем может быть причина? Может у меня испульсов с энкодера слишком мало? Или может какой-то другой способ поддержания заданных оборотов существует?