Задача: организовать считывание положения джойстика KY-023, по оси X и Y. На основе считывания привести в движение сервомоторы SG90-9G.

В статье Радионабор 433МГц. Джойстик с радиопередатчиком была рассмотрена программа для attiny45. Микроконтроллер управлял такой конструкцией:

Сервоприводы управлялись удаленно, но по видео ниже видно, что движения резкие и очень тяжело подвести кронштейн к нужной позиции.

В этот раз нужно сделать доработку управления сервоприводами более плавную. Для упрощения удаленное управление исключается. Джойстик напрямую будет соединен с микроконтроллером, который управляет сервоприводами.

Схема подключения:

И как схема выглядит на макетной плате:

Теперь программная реализация.

Сервомотор управляется импульсом от 0.6 мс до 2.4 мс, частота следования импульсов - 20 мс. Импульс - 1.5 мс - это центральная позиция, 0.6 мс - это минус 90 градусов поворота, 2.4 мс - это 90 градусов поворота.

1-й способ реализации программы

В данной реализации программы микроконтроллер отслеживает значение потенциометров Х и Y, с помощью встроенных АЦП на ножках PB3 и PB4. Используется таймер 0 и его прерывания по переполнению.

Сам текст программы: по ссылке на github

Пояснения:

#define ADCTR 4

это число раз перечитывания значений с АЦП, т.к оно можетбыть не стабильным, то значение считывается 4 раза и результат усредняется.

uint16_t readADC(uint8_t adc_num){
    ADCSRA = _BV(ADEN) | _BV(ADPS1) | _BV(ADPS0);

Включение АЦП и перевод его в режим Devision factor = 8.

Далее инетерсный момент в этих числах:

#define MAGICK_NUMBER 38 //Number of ticks in 1ms
#define DOWNV 25
#define MAXIV 87

Что это такое - объяснение ниже. В данном случае активирован режим срабатывания прерывания по переполнению таймера ISR(TIM0_OVF_vect), деления на таймере никакого нет:

    TCCR0A = 0;
    TCCR0B = _BV(CS00);
    TCNT0 = 0;
    TIMSK0 = _BV(TOIE0);

т.е при работе контроллера на частоте 9600000 Гц, прерывание будет возникать:

\(\frac{9600000}{256}=37500\)

37500 прерываний в секунду, т.е 0.00002666 секунд интервал между прерываниями. Значит за одну милиссекунду произойдет

\(\frac{0.001}{0.0000266666}=38\)

38 прерываний. А вот и #define MAGICK_NUMBER 38, за 0.6 мс произойдет 22 прерывания (#define DOWNV 25, я добавил три тика, чтоб двигатель сильно не бился в крайние позиции) и за 2.4 мс произойдет 90 прерываний (#define MAXIV 87, так же уьрал 3 тика, по той же причине что описал выше).

Здесь происходит преобразование yTrn_tmp = Ry_coord * (MAXIV-DOWNV) / 1024; считанных с АЦП значений в масштаб импульса(т.е в число тиков).

\(NEW_VAL=DOWNV+\frac{(MAXIV-DOWNV)*ADC}{1024}=\frac{87-25}{1024}*ADC+22\)

87-25 * ADC при любом ADC поместится в uint16_t.

Пример работы на видео ниже.

Недостатки:

  • т.к положение джойстика указывает в какую позицию поврнуть сервомоторы, то любая флуктуация напряжения питания заставляет дрожать двигатели.(попытка избежать флуктуаций - это повторно перечитывание значений АЦП и усреднение, а так же процедура addelem, которая заносит новое значение в массив, выталкивая старое и усредняет текущее значение на основе старых данных)
  • невозможность зафиксировать двигатели в одной позиции(в алгоритме предусмотрено нажатие кнопки, которое дает 5 секунд. чтоб установить позицию двигателей и после чего координата перестает меняться, т.е замораживается)

2-й способ реализации программы

Здесь я пытался убрать недостаток 1 и 2 предыдущего метода.

Сам текст программы: по ссылке на github

В программе так же присутсвуют магические числа, описанные ранее, только число попыток перечитывания АЦП существенно увеличилось до 16. И появились новые:

#define ADC_C_LOW 500
#define ADC_C_HIGH 524
#define ANGLE_MAGIC 4608
#define ADC_MID 512

В данной реализации задается не координата положения рычага серводвигателя, а угол на который нужно повернуть серву, при возвращении джойстика в нулевую позицию двигатель просто останавливается, а не возвращается в старую позицию, как в прошлом методе. Это дало возможность избежать использования дополнительной кнопки, что было не удобно.

ADC_C_LOW, ADC_C_HIGH, ADC_MID теперь задают середину, т.к здесь нам важно знать - куда вращать мотор, влево или вправо. Вот процедура, которая вычисляет эти величины:

void getRotation(uint16_t coord, uint8_t *step, uint8_t *direct){
    uint16_t tmp = 0;
    if (coord < ADC_C_LOW){
        *direct = 1;
        tmp = ((ADC_MID-coord)*((MAXIV-DOWNV)/2))/ANGLE_MAGIC;
        *step = (uint8_t)tmp;
        } else if (coord > ADC_C_HIGH) {
        *direct = 2;
        tmp = ((coord - ADC_MID)*((MAXIV-DOWNV)/2))/ANGLE_MAGIC;
        *step = (uint8_t)tmp;
        } else {
        *direct = 0;
        *step = 0;
    }
    return;
}

step - говорит на сколько тиков изменить текущее значение длины импульса, а direct - задает куда изменять, увеличивать или уменьшать. Откуда взялись эти формулы?

Т.к у нас 37500 прерываний в секунду, и 38 срабатываний прерывания(тиков) в 1 миллисекунду и разница между 2.4 мс - 0.6 мс = 1.8 мс, а это 68 тиков. Т.к угол поворота двигателя 180 градусов - 68 тиков. В каждую сторону по 90 градусов, т.е - 34 тика.

Был выбран угол максимального поворота равный 10 градусам. Почему? Т.к мы можем изменить длину импульса на 1 тик, а это 90/34=2.9 граудса. Т.е 10/2.9=3, т.е при 10 гралусах поворота мы будем иметь три скорости поворота: 3, 6 и 9 градусов.

Если выбрать меньше 10 - это значит уменьшить число скоростей.

Пусть 10 - MAX_ANGLE, тогда угол вычисляется как:

\(ANGLE=\frac{MAX_ANGLE}{512}*(512-ADC)\)

или ADC-512, если ADC больше 512.

Переводградусов в тики для шага:

\(X=\frac{ANGLE*(MAXIV-DOWNV)/2}{90}=\frac{MAX_ANGLE*(512-ADC)*(MAXIV-DOWNV)/2}{90*512}=\frac{10*62*(512-ADC)}{46080}=\frac{62*(512-ADC)}{4608}\)

Вот и магическое число #define ANGLE_MAGIC 4608, все вычисления вмещаются в uint16_t.

Недостатки

  • видно ступенчатое перемещение, что затрудняет более точный поврот, т.к 3 градуса минимальная ширина поворота - это много

3-й способ реализации программы

Устранение недостатка предыдущего метода.

Сам текст программы: по ссылке на github

В программе прерывание было заменено на ISR(TIM0_COMPA_vect) - прерывание по совпадению таймера с регистром. В регистр загружено значение OCR0A = 97, т.е 9600000/(97-1)=100000, получаем 100000 прерываний в секунду. Теперь на одну миллисекунду приходится 100 тиков.

#define MAGICK_NUMBER 100 //Number of ticks in 1ms
#define DOWNV 60
#define MAXIV 240
#define ADCTR 4

Новые магические числа из-за увеличения числа тиков.

По методу выше, выведены магические числа 5 и 256.

#define ANGLE_MAGIC_A 256
#define ANGLE_MAGIC_B 5

для формулы:

tmp = ((coord - ADC_MID)*ANGLE_MAGIC_B)/ANGLE_MAGIC_A;

\(X=\frac{ANGLE**(MAXIV-DOWNV)/2}{90}=\frac{MAX_ANGLE*(MAXIV-DOWNV)/2*(512-ADC)}{90*512}=\frac{10*90*(512-ADC)}{90*512}=\frac{5*(512-ADC)}{256}\)

Вот и искомые коэффициенты. Все вычисления вмещаются в uint16_t. Теперь результат лучше.

Видео демонстрации:

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

Следующая запись Предыдущая запись