// Автор: kimoncar
//
// Код написан с использованием скетча Lebois Racing:
// https://www.lebois-racing.fr/en
// Для Uno/Nano (ATmega328P)
// Совместимо с FlyPT Mover, SimTools и SimHub

// ПОДКЛЮЧЕНИЕ:
// Серво1: STEP 7, DIR 6
// Серво2: STEP 5, DIR 4
// Серво3: STEP 3, DIR 2
#define StepPin1  7 
#define DirPin1   6

#define StepPin2  5
#define DirPin2   4

#define StepPin3  3
#define DirPin3   2

#define RelayPin  8 // HIGH = ВКЛ, LOW = ВЫКЛ

#define pulseDelay 2      // скорость двигателя: чем меньше, тем быстрее. Не ниже 2 или он не будет работать.
#define directionDelay 10

byte buffer           = 0 ;    // временная переменная для байта из Serial
byte buffercount      = 0 ;    // счётчик позиции внутри принятого пакета
byte commandbuffer[6] = {0};   // буфер команд: 3 оси × по 2 байта

unsigned m1Target = 0,   m2Target = 0,   m3Target = 0; 
unsigned m1Position = 0, m2Position = 0, m3Position = 0;
int  dir1 = -1, dir2 = -1, dir3 = -1;         // направление для каждого мотора
byte dirChange = 0;
bool noData = true;
bool servoEnabled = false;

void setup() {
  Serial.begin(250000);    // скорость связи с SimTools/SimHub/FlyPT Mover

  pinMode(RelayPin, OUTPUT);
  disableServo();

  pinMode(StepPin1, OUTPUT);
  pinMode(DirPin1, OUTPUT);
  digitalWrite(DirPin1, HIGH);

  pinMode(StepPin2, OUTPUT);
  pinMode(DirPin2, OUTPUT);
  digitalWrite(DirPin2, HIGH);

  pinMode(StepPin3, OUTPUT);
  pinMode(DirPin3, OUTPUT);
  digitalWrite(DirPin3, HIGH);
}

void loop() {
  SerialReader();                     // читаем данные по Serial
  moveMotor();                        // двигаем моторы
}

void SerialReader() {       // Выход FlyPT: T<Axis1><Axis2><Axis3>, каждая ось — 16 бит (2 байта), 8N1
  while (Serial.available())
  {
    if (buffercount == 0)
    {
      buffer = Serial.read();

      if (buffer != 'T') {
        if(buffer =='A'){enableServo();}
        if(buffer =='C'){disableServo();}     
                      
        buffercount = 0; // стартовый байт не 'T': обрабатываем команды A/C и ждём новый пакет
      } else {
        buffercount = 1;
      }
    }
    else   //  if(buffercount>=1)
    {
      buffer = Serial.read();
      commandbuffer[buffercount-1] = buffer; // первый байт после 'T' попадает в commandbuffer[0]
      buffercount++;
      if (buffercount >= 7) { // 'T' + 6 байт данных = 7
          m1Target = commandbuffer[0] * 256 + commandbuffer[1];      // 8-битная передача, данные 16 бит → объединяем 2 байта
          m2Target = commandbuffer[2] * 256 + commandbuffer[3];
          m3Target = commandbuffer[4] * 256 + commandbuffer[5];
          buffercount = 0;
        break;
      }
    }
  }
}


void moveMotor() {
  directionManager(); // выставляем направления моторов
  singleStep();       // делаем по одному шагу на нужных моторах
}

void directionManager() {
  if ((m1Target > m1Position) && (dir1 == -1)) {
    PORTD &= B10111111;  // DIR1 пин 6 = LOW
    dir1 = 1;
    dirChange = 1;
  }

  if ((m1Target < m1Position ) && (dir1 == 1)) {
    PORTD |= B01000000;
    dir1 = -1;
    dirChange = 1;
  }

  if ((m2Target > m2Position) && (dir2 == -1)) {
    PORTD &= B11101111;  // DIR2 пин 4 = LOW
    dir2 = 1;
    dirChange = 1;
  }
  
  if ((m2Target < m2Position ) && (dir2 == 1)) {
    PORTD |= B00010000;
    dir2 = -1;
    dirChange = 1;
  }

  if ((m3Target > m3Position) && (dir3 == -1)) {
    PORTD &= B11111011;  // DIR3 пин 2 = LOW
    dir3 = 1;
    dirChange = 1;
  }
  
  if ((m3Target < m3Position ) && (dir3 == 1)) {
    PORTD |= B00000100;
    dir3 = -1;
    dirChange = 1;
  }
  
  if (dirChange == 1) {
    delayMicroseconds(directionDelay);
    dirChange = 0;
  }
}

void singleStep() {
  byte stepMask = 0;

  if (m1Target != m1Position) {
    stepMask |= B10000000;  // STEP1 (пин 7)
    m1Position += dir1;
  }
  
  if (m2Target != m2Position) {
    stepMask |= B00100000;  // STEP2 (пин 5)
    m2Position += dir2;
  }
    
  if (m3Target != m3Position) {
    stepMask |= B00001000;  // STEP3 (пин 3)
    m3Position += dir3;
  }
    
  if(stepMask == 0){return;}

  PORTD |= stepMask;  // одновременно поднимаем все нужные STEP в HIGH
  delayMicroseconds(pulseDelay);
  PORTD &= ~stepMask; // одновременно опускаем все STEP в LOW
}

void enableServo(){
    digitalWrite(RelayPin, HIGH); // реле активно по уровню HIGH
    servoEnabled=true;
}
void disableServo(){
    digitalWrite(RelayPin, LOW);
    servoEnabled = false;
}