Мой опыт управления длиной актуатора постоянного тока
arduino encoding interrupts

Мой опыт управления длиной актуатора постоянного тока

16 декабря 2022 г.

Все началось с того, что мы заказали актуатор с энкодером из Китая для проекта. Но что-то пошло не так, и через месяц мы получили актуаторы без энкодера.

Нужно было срочно доделывать код проекта, но управлять приводами без энкодеров было бы невозможно. Для проекта нужно знать длину вылета штока, а для этого нужно знать количество оборотов двигателя. Поэтому мы решили потратить один день и сделать свой энкодер для приводов. Сначала думали поставить многооборотный потенциометр.

Но поворотов может быть очень много, а такого энкодера у нас под рукой не было.

Затем мы захотели использовать такой кодировщик, как EC11,

но у таких энкодеров очень маленький ресурс работы. И в итоге выбрали вариант: из двух цифровых датчиков Холла SS443A и магнитов.

Датчик Холла SS443A выдает логический ВЫСОКИЙ уровень на одной полярности магнитов и логический НИЗКИЙ уровень на другой полярности и сохраняет это значение даже при удалении магнита. Если кратко: Берем два датчика холла и ставим их под углом 45 градусов друг к другу. К оси мотора приклеиваем два магнита, с разными полюсами, и при вращении мотора каждый датчик включается и выключается по очереди.

Если каждый датчик включался и выключался по одному разу, то был один оборот двигателя. С двумя датчиками мы можем считать четверть оборота.

А зная последовательность включения и выключения датчиков, мы можем определить направление вращения.

Мы разобрали привод, чтобы найти место для установки магнитов.

Места там не было, а магниты приклеить не получалось, так как все было замаслено жиром. Тогда мы решили выкинуть оригинальную крышку актуатора, в которую будет встроена шестеренка с магнитами и два датчика холла. Шестерня будет ведомой, и мы поместим шестерню на отдельную ось. И шестерня будет крутиться без нагрузки от ведущей шестерни двигателя. Мы взяли два магнита диаметром 5мм и толщиной 4мм. В SolidWorks с помощью плагина «GearTrax» нарисовали шестерню нужного размера и нарисовали крышку корпуса привода с 6-контактным разъемом GX16.

Мы распечатали корпус на 3D-принтере из пластика PETG и с 3-й попытки получили работающий механизм. На все это вместе с 3D-печатью ушло около 11 часов.

Осталось только написать код для расчета длины удлинения стержня. Для теста мы взяли плату Arduino MEGA 2560; есть шесть контактов с аппаратными прерываниями. Можно сделать код без перерывов, но если хотя бы один импульс (четверть оборота) будет пропущен, то погрешность будет 1,6мм, а это уже много для нашей задачи. Тесты показали, что без перерывов ошибка может накапливаться до больших значений при активном использовании. Поэтому мы используем два контакта прерывания для каждого привода. 1 контакт = 1 датчик Холла.

Код, в итоге, получился довольно простым, но я решил оставить его здесь как шпаргалку на будущее, а может еще кому пригодится:

 const byte M1_HALL1_pin = 20; //connect pintout Hall sensor 1
const byte M1_HALL2_pin = 21;   //connect pintout Hall sensor 2

const byte M1_HALL1_pin_interupt = 3; //Interrupt pin3 match Pin20 Arduino Mega
const byte M1_HALL2_pin_interupt = 2; //Interrupt pin2 match Pin21 Arduino Mega

volatile byte M1_HALL_cur_state = B00; //Current state hall sensors. If Hall1 and Hall2 is LOW then variable value is B00
volatile byte M1_HALL_last_state = B00; //Last state hall sensors. If Hall1 is HIGH and Hall2 is LOW then variable value is B10

volatile long countPulsesM1 = 0; //Counter

/*
Arrays dirUpM1 and  dirDownM1 have directions of rotation.
*/
byte dirUpM1[] = {1,3,0,2};     //01 11 00 10
byte dirDownM1[] = {2,0,3,1};   //10 00 11 01

void setup(void){
  attachInterrupt(M1_HALL1_pin_interupt, countRotationInteruptsM1, CHANGE);
  attachInterrupt(M1_HALL2_pin_interupt, countRotationInteruptsM1, CHANGE);

  //Be sure to pullup the pins to avoid noises
  pinMode(M1_HALL1_pin, INPUT_PULLUP); 
  pinMode(M1_HALL2_pin, INPUT_PULLUP); 

  Serial.begin(9600);
}

void loop(void){
  Serial.println(countPulsesM1);
}

void countRotationInteruptsM1(){
  //Present cur status of hall sensor as binary number
  M1_HALL_cur_state = digitalRead(M1_HALL1_pin);  //Read HALL1 state (for example: if HALL1 is HIGH then value is  B01)
  M1_HALL_cur_state = M1_HALL_cur_state << 1; //Shift the bit to make room for the second value (for example: value is  B10)
  M1_HALL_cur_state += digitalRead(M1_HALL2_pin); //Read HALL2 state and ADD it to HALL1 value (for example: if HALL2 is HIGH too, then value is B11)

    //If in an array dirUpM1 of sequences, the previous value is followed by the current, means the actuator moves up
    if(dirUpM1[M1_HALL_cur_state] == M1_HALL_last_state){
        countPulsesM1++;
    //else  the actuator moves up
    } else if(dirDownM1[M1_HALL_cur_state] == M1_HALL_last_state){
        countPulsesM1--;
        if(countPulsesM1 < 0){
            countPulsesM1 = 0;
        }
    }
}

Массивы dirUpM1 и dirDownM1 имеют направления вращения. По мере вращения шестерни датчики Холла будут включаться и выключаться один за другим.

Например:

  • сначала HALL1 и HALL2 имеют НИЗКИЙ уровень. Это можно записать как двоичный код B00
  • тогда HALL1 – НИЗКИЙ, а HALL2 – ВЫСОКИЙ. Это можно записать как двоичный код B01
  • тогда HALL1 и HALL2 имеют значение HIGH. Это можно записать как двоичный код B11
  • тогда HALL1 – ВЫСОКИЙ, а HALL2 – НИЗКИЙ. Это можно записать как двоичный код B10
  • а затем цикл повторяется

Это можно представить в виде комбинации: 00 - 01 01 - 11 11 - 10 10–00

Если вы преобразуете двоичные числа в десятичные, это будет выглядеть так: 0 - 1 1 - 3 3 - 2 2–0

Теперь представим его в виде массива:

обрД[0] = 1;

обрД[1] = 3;

обрД[3] = 2;

обрД[2] = 0;

И наконец: byte dirUpM1[] = {1,3,0,2};

Если вдруг изменится подключение датчиков холла, массив с направлением можно «обучить»:

void setDerectionUp(){  
  if(M1_HALL_cur_state != M1_HALL_last_state){
    dirUpM1[M1_HALL_last_state] = M1_HALL_cur_state;
    M1_HALL_last_state=M1_HALL_cur_state;
  }
  if(M2_HALL_cur_state != M2_HALL_last_state){
    dirUpM2[M2_HALL_last_state] = M2_HALL_cur_state;
    M2_HALL_last_state=M2_HALL_cur_state;
  }
}

ПС: Заводской актуатор с энкодером стоит: $41. Актуатор без энкодера + самодельный энкодер стоит: 26$ + 4$ = 30$

Оригинал

Recent Post

Categories

PREVIOUS ARTICLE
NEXT ARTICLE