Что нам стоит ЧПУ привод построить

Не так давно я озаботился вопросом, как бы сделать привод для систем ЧПУ на коллекторном двигателе, да еще и с обратной связью, и что бы работал по Modbus RTU.

В распоряжении у меня имелись, Arduino UNO, Драйвер на L298N, коллекторный двигатель от привода сиденья автомобиля марки AEG на 12В, щелевой фотопрерыватель 2 шт.

С начала я спаял платку с 2-мя фотопрерывателями, вот по такой схеме.



В готовом виде это всё выглядит так:



Суть данной некрасивой вещи — спичка пересекает с начала первый прерыватель, потом второй, так микроконтроллер понимает в какую сторону вращается мотор.

Все соединения выглядят так:

d2 — первый фотопрерыватель;
d3 — второй фотопрерыватель;
d5 — выход шим для вращения влево;
d6 — выход шим для вращения вправо;

Всё было соединено и я засел за написание кода, в итоги муки мои родили следующее:

#include <ModbusRtu.h>
#define ID 1
//Задаём ведомому адрес, последовательный порт, выход управления TX
Modbus slave(ID, 0, 0);
int8_t state = 0;   //состояние соединения
uint16_t au16data[11];// массив данных modbus

#include <PID_v1.h> //биилиотека и настройки ПИД регулятора
double Setpoint1, Input1, Output1;
double Setpoint2, Input2, Output2;
double Kp = 1, Ki = 2.5, Kd = 0.5;
PID myPID1(&Input1, &Output1, &Setpoint1, Kp, Ki, Kd, DIRECT);    //1-й вращает в одну сторону
PID myPID2(&Input2, &Output2, &Setpoint2, Kp, Ki, Kd, REVERSE); //2-й вращает в другую

int LeftPWM = 5;  //шим вращения в одну сторону
int RightPWM = 6; //шим вращения в другую сторону
volatile byte EncA, EncB = 0; //переменные для работы с прерываниями
volatile int Position = 0; //текущая позиция мотора
int SetPosition = 0; //в какую позицию поставить мотор

void setup()
{
  slave.begin(9600);
  pinMode (LeftPWM, OUTPUT);
  pinMode (RightPWM, OUTPUT);
  attachInterrupt(0, ChangePosition1, FALLING); // привязываем 0-е(pin2) прерывание к функции ChangePosition1()
  attachInterrupt(1, ChangePosition2, FALLING); // привязываем 1-е(pin3) прерывание к функции ChangePosition2()
  Setpoint1 = 0;
  Setpoint2 = 0;
  digitalWrite (LeftPWM, LOW);
  digitalWrite (RightPWM, LOW);
}
//обработка прерывания 0
void ChangePosition1() //кто долго мучался с энкодером вот вам простой код для понимания!
{
  EncA = 1;
  if (EncA == 1 && EncB == 1)
  {
    EncA = 0;
    EncB = 0;
    Position++;
  }
}
//обработка прерывания 1
void ChangePosition2()
{
  EncB = 1;
  if (EncA == 1 && EncB == 1)
  {
    EncA = 0;
    EncB = 0;
    Position--;
  }
}
void loop()
{
  //Работа с ModBus
  // обработка сообщений
  state = slave.poll(au16data, 11);
  //Запись в регистры
  au16data[2] = Position;//текущее положение
  //Чтение из регистров
  SetPosition = au16data[3];//установить положение
  //Запись служебных регистров
  au16data[8] = slave.getInCnt();  //входящих пакетов
  au16data[9] = slave.getOutCnt(); //исходящих пакетов
  au16data[10] = slave.getErrCnt();//ошибок

  if (SetPosition == Position) //если нужная позиция равна текущей
  {
    myPID1.SetMode(MANUAL);
    myPID2.SetMode(MANUAL);
    Output1 = 0;
    Output2 = 0;
    digitalWrite (LeftPWM, LOW);
    digitalWrite (RightPWM, LOW);
  }
  if (SetPosition < Position)
  {
    myPID2.SetMode(AUTOMATIC);
    Setpoint2 = SetPosition;
    Input2 = Position;
    myPID2.Compute();
    analogWrite (LeftPWM, Output2);
    digitalWrite (RightPWM, LOW);
  }
  if (SetPosition > Position)
  {
    myPID1.SetMode(AUTOMATIC);
    Setpoint1 = SetPosition;
    Input1 = Position;
    myPID1.Compute();
    analogWrite (RightPWM, Output1);
    digitalWrite (LeftPWM, LOW);
  }
}

А теперь дело за частью на компьютере. Что бы это все двигать была выбрана SinplLight SCADA, парни дают её самодельщикам бесплатно. Итак — вот так выглядят настройки в скаде.







Интерфейс был сделан такой:



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