Управление шаговым двигателем заданным углом поворота

Песочница. Раздел для новичков
1

Добрый день! Не могу сообразить, подскажите пожалуйста. Нужно повторять угол поворота шаговым двигателем NEMA 14 за MPU5060, с датчика я получаю значения либо float 0.00 - 360.00 либо int целые. Но как преобразовать эти значения в управление двигателем ?

Я так понимаю:
создать переменную которая будет хранить текущий угол двигателя (изначально любой)
потом:
если значение датчика больше чем значение мотора сейчас
то значение датчика отнять значение мотора (выяснить насколько больше)
результат поделить на 1.8 (угол за шаг) и получим необходимое количество шагов
так же направление вращения если > то 1 по часовой, если < то против часовой…

это как пример, в противоположную сторону так же.

Это пример кода, в качестве датчика переменник, мотор все время просто по часовой крутится

#define DIR         2
#define STEP        3
int   angle_pot;
float angle_motor = 180;

void setup() 
{
  pinMode(DIR, OUTPUT);
  pinMode(STEP, OUTPUT);
  Serial.begin(9600);
}

void loop() 
{
   angle_pot = analogRead(A0);
   angle_pot = map(angle_pot, 0, 1023, 0, 360);

   if (angle_pot > angle_motor)
   {
     digitalWrite(DIR, 1);
     int num_step = (angle_pot - angle_motor) / 1.8;
     Serial.println(num_step);
     angle_motor = angle_pot;
   }

   if (angle_pot < angle_motor)
   {
     digitalWrite(DIR, 0);
     int num_step = (angle_motor - angle_pot) / 1.8;
     Serial.println(num_step);
     angle_motor = angle_pot;
   }
}
2

ну хорошо бы еще angle_pot в сериал вывести и результат нам показать.

3

Количество шагов на оборот известно, текущее положение двигателя известно(как минимум при старте), текущий угол наклона датчика известен.
Чего тебе ещё надо ?

4 
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#define DIR         2
#define STEP        4 
#define MICRO_STEP  1 // дробление шага
uint32_t timerMPU;    // таймер опроса датчика мс
#define PERIOD_MPU 10 // частота опроса датчика мс
int angle_motor = 180;
uint32_t timer_motor;
int speed; // частота импульсов двигателя

MPU6050 mpu;
uint8_t fifoBuffer[45];         // буфер
float ypr[3];                   // [0] - Z ось

void setup() 
{
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  mpu.dmpInitialize();
  mpu.setDMPEnabled(true);
}

void loop() 
{
  if (millis() - timerMPU >= PERIOD_MPU)
  {
    if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) 
    {
      Quaternion q;
      VectorFloat gravity;
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
      ypr[0] += 3.14;                            // перевод в положительные числа
      ypr[0] = ypr[0] * 57296 / 1000;            // перевод из радиан в градусы
      if (ypr[0] < 0.0) ypr[0] = 0.0;            // фильтр отрицательных значений при переходе 360-0
      //Serial.println((int)ypr[0]);
    }
  }
// ---------- ОБРАБОТКА ДВИГАТЕЛЯ ---------- 

  
  if ((int)ypr[0] > angle_motor)              // если угол датчика больше угла мотора
  { 
    digitalWrite(DIR, 1);                     // направление по часовой 
    int num_step = (int)ypr[0] - angle_motor; // насколько больше
    num_step /= 1.8;                          // градусы в шаги
    if (num_step > 0)
    {
      if (millis() - timer_motor >= speed)
      {
        timer_motor = millis();
        digitalWrite(STEP, !digitalRead(STEP));
        num_step --;
      }
    }
    angle_motor = (int)ypr[0];
  }

   if ((int)ypr[0] < angle_motor)
  {
    digitalWrite(DIR, 0);
    int num_step = angle_motor - (int)ypr[0];
    num_step /= 1.8;
    if (num_step > 0)
    {
     if (millis() - timer_motor >= speed)
      {
        timer_motor = millis();
        digitalWrite(STEP, !digitalRead(STEP));
        num_step --;
      }
    }
    angle_motor = (int)ypr[0];
  }
  

  speed = analogRead(A0);
  speed = map(speed, 0, 1023, 0, 20);

  Serial.print("Датчик - "); Serial.println((int)ypr[0]);
  Serial.print("Мотор  - "); Serial.println(angle_motor);
}

Последняя версия, как минимум проблема что в 40-й строке в порт значения с датчика выводятся отлично, а если выводить в конце после “ОБРАБОТКА ДВИГАТЕЛЯ” то значения практически в порте не меняются, одни и те же