//#define DRIVER_STEP_TIME 10
#include <GyverStepper.h>;
#include <LiquidCrystal_I2C.h>

#define dirPin 2
#define stepPin 3
#define stopButton 10

#define speedCLK 4  //порт CLK энкодера скорости шаговика шнеков
#define speedDT 5   //порт DT энкодера скорости шаговика шнеков
#define speedSW 6  //порт SW энкодера управления ножом

#define accelCLK 7 //порт CLK энкодера управления ножом
#define accelDT 8  //порт DT энкодера управления ножом
#define accelSW 9  //порт SW энкодера управления ножом

int maxSpeed = 300;
int maxAccel = 10;
int minAccel = 1;

int stepPul = 1600;

int speed =120;
int oldSpeed = speed+1;
int accel = 5;
int oldAccel = accel+1;
int rotation = 5000;
int cycle = 10000;

int motorMod = 1; //1 - SPEED, 2 - ROTATE 3 - FOllOW POS

bool speedLastClk = HIGH; 
bool accelLastClk = HIGH;

bool direct = HIGH; //HIGH - по часовой, LOW - против
bool rotationMode = LOW; //HIGH - регулировка работы, LOW - регулировка цикла
bool stopFunc = HIGH; //HIGH - стоп, LOW - работа
bool stopButtonFunc = HIGH; //то же самое, но кнопка

long position = 0;
long oldPosition = position+1;
int posIncrem=360;
int incVar = 10;

int angle =720;
float cycleSpeedDeg = 4*angle/square(rotation/1000);

uint32_t zaWarudo = 0;

uint32_t encoderTime = -5000;
uint32_t cycleTime = -10000;
uint32_t stoppingTime = millis();

LiquidCrystal_I2C lcd(0x27, 16, 2);//обозначение ЖК дисплея

GStepper<STEPPER2WIRE> motor(stepPul, stepPin, dirPin);

void setup() {
Serial.begin(115200);
lcd.init();
lcd.backlight();

pinMode(dirPin, OUTPUT);
pinMode(stepPin, OUTPUT);

pinMode(speedCLK, INPUT);
pinMode(speedDT, INPUT);
pinMode(speedSW, INPUT_PULLUP);

pinMode(accelCLK, INPUT);
pinMode(accelDT, INPUT);
pinMode(accelSW, INPUT_PULLUP);

pinMode(stopButton, INPUT);

motor.setRunMode(KEEP_SPEED);
motor.setMaxSpeedDeg(6*maxSpeed);
motor.setAcceleration(360*accel);

lcd.setCursor(0, 0); //вывод 1 строки на ЖК дисплее
lcd.print("ACCEL DIR  SPEED");
lcd.setCursor(2, 1);
lcd.print(accel);

lcd.setCursor(7, 1);
lcd.print("+");

lcd.setCursor(12, 1);
lcd.print(speed);

}

void loop() {

if (digitalRead(accelSW) == LOW && digitalRead(stopButton)==false && millis() - encoderTime >= 1000 && stopFunc == HIGH) {
  motorMod = motorMod+1;
  if (motorMod >=4){motorMod = 1;};

  if (motorMod == 1){
    lcd.clear();
    motor.setRunMode(KEEP_SPEED);
    lcd.setCursor(0, 0);
    lcd.print("ACCEL DIR  SPEE");
    lcd.print("D");
    lcd.setCursor(2, 1);
    lcd.print(accel);
    lcd.setCursor(7, 1);
    if(direct == HIGH){lcd.print("+");}
    else {lcd.print("-");};
    lcd.setCursor(12, 1);   
    lcd.print(speed);
    motor.setMaxSpeedDeg(6*maxSpeed);
    motor.setAccelerationDeg(360*accel);
    encoderTime = millis();    
    }

  if (motorMod == 2){
    motor.brake();
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("ANGLE ROT");
    if (rotationMode == HIGH){lcd.write(127); lcd.print(" ");}
    if (rotationMode == LOW){lcd.print(" "); lcd.write(126);};
    lcd.print("CYCLE");    
    lcd.setCursor(0, 1);
    lcd.print(angle);
    lcd.setCursor(6, 1);
    lcd.print(float(rotation/1000.0),1);
    lcd.setCursor(12, 1);
    lcd.print(float(cycle/1000.0),1);
    motor.setAccelerationDeg(cycleSpeedDeg);
    motor.setMaxSpeedDeg(60*maxSpeed);
    motor.setCurrentDeg(0);
    motor.setRunMode(FOLLOW_POS);
    encoderTime = millis();    
    }    

  if (motorMod == 3){
    lcd.clear();
    position = 0;
    oldPosition = position;
    motor.setCurrentDeg(position);
    motor.setTargetDeg(position, ABSOLUTE);
    lcd.setCursor(0, 0);
    lcd.clear();
    lcd.print("INCR  TARGET POS");
    lcd.setCursor(1, 1);
    lcd.print(int(posIncrem));
    lcd.setCursor(8, 1);
    lcd.print(int(position));
    motor.setAccelerationDeg(360*accel);
    motor.setMaxSpeedDeg(6*speed);
    encoderTime = millis();    
    };
};



if(motorMod == 1){//режим скорости

  //нажатие на энкодер, выбор направления вращения
  if (digitalRead(speedSW) == LOW && millis() - encoderTime >= 2500){
    direct = !direct;
    lcd.setCursor(7, 1);
    if (direct == HIGH){lcd.print("+");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};}
    else { lcd.print("-");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};};
    if (stopButtonFunc == LOW){      
      if (direct == HIGH ) {motor.setSpeedDeg(6*speed, SMOOTH);}
      else{motor.setSpeedDeg(-6*speed, SMOOTH);};};
    encoderTime = millis();
  };

  if (stopFunc == LOW) {
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();};

  //опрос энкодера скорости
  int speedNewClk = digitalRead(speedCLK);
  if (speedNewClk != speedLastClk){
    speedLastClk = speedNewClk;
    int speedValueDT = digitalRead(speedDT);
    if (speedNewClk == LOW && speedValueDT == HIGH) {speed+=5;} //при движении энкодера по часовой
    if (speedNewClk == LOW && speedValueDT == LOW) {speed-=5;};//при движении энкодера против часовой
    speed = constrain(speed, 15, maxSpeed);
    lcd.setCursor(12, 1);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(speed);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
  }

  //опрос энкодера ускорения
  int accelNewClk = digitalRead(accelCLK);
  if (accelNewClk != accelLastClk){
   accelLastClk = accelNewClk;
    int accelValueDT = digitalRead(accelDT);
   if (accelNewClk == LOW && accelValueDT == HIGH) {accel+=1;} //при движении крутилки по часовой
   if (accelNewClk == LOW && accelValueDT == LOW) {accel-=1;};//при движении крутилки против часовой
   accel = constrain(accel, minAccel, maxAccel);
   lcd.setCursor(2, 1);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(accel);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
   lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
   }

  if (stopFunc == LOW) {
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();};

  //назначение новых скоростей через промежуток в 2,5 секунды
  if (speed != oldSpeed || oldAccel != accel){
   if (digitalRead(stopButton)==true && millis() - encoderTime >= 2500){
    if (speed != oldSpeed){
      oldSpeed = speed;
      if (direct == HIGH ) {motor.setSpeedDeg(6*speed, SMOOTH);}
      else{motor.setSpeedDeg(-6*speed, SMOOTH);};}
    if (oldAccel != accel){
      oldAccel = accel;
      motor.setAcceleration(float(stepPul*accel));};
    if (stopButtonFunc == HIGH){stopButtonFunc = LOW;};
    if (stopFunc == HIGH){stopFunc = LOW;};
    encoderTime = millis();};};

  //останов
  if (stopButtonFunc==HIGH && stopFunc == LOW && millis() - stoppingTime >= zaWarudo) {stopFunc = HIGH;};

  if (stopFunc == LOW) {
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();};

  //расчёт времени для останова
  if (digitalRead(stopButton)==false && stopButtonFunc==LOW && millis() - encoderTime >= 1000) {
    zaWarudo = abs(((speed/accel)*16.67));
    oldSpeed = speed+20;
    oldAccel = accel+20;
    motor.stop();
    stopButtonFunc=HIGH;
    stoppingTime = millis();};

  if (stopFunc == LOW) {
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();};
}



if(motorMod == 2){
  //переключение с времени цикла на время поворота
  if (digitalRead(speedSW) == LOW && millis() - encoderTime >= 2500){
    rotationMode =!rotationMode;
    lcd.setCursor(9, 0);
    if (rotationMode == HIGH){lcd.write(127); lcd.print(" ");}
    if (rotationMode == LOW){lcd.print(" "); lcd.write(126);};
  encoderTime = millis();
  }

    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
  
  //изменение времени цикла/поворота
  int speedNewClk = digitalRead(speedCLK);
  if (speedNewClk != speedLastClk) {
  speedLastClk = speedNewClk;
  int speedValueDT = digitalRead(speedDT);
    if(rotationMode == LOW){
      if (speedNewClk == LOW && speedValueDT == HIGH) {cycle+=500;} //при движении энкодера по часовой
      if (speedNewClk == LOW && speedValueDT == LOW) {cycle-=500;};//при движении энкодера против часовой
      cycle = constrain(cycle, 8000, 30000);
      lcd.setCursor(12, 1);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
      lcd.print(float(cycle/1000.0),1);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
      lcd.print("  ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
      if(rotation>=cycle-1000){
        rotation = constrain(rotation, 5000, cycle-3000);
        lcd.setCursor(6, 1);   
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
        lcd.print(float(rotation/1000.0),1);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
        lcd.print("  ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
        cycleSpeedDeg = 4*angle/square(rotation/1000);
        motor.setAccelerationDeg(cycleSpeedDeg);};}

    if(rotationMode == HIGH){
      if (speedNewClk == LOW && speedValueDT == HIGH) {rotation+=500;} //при движении крутилки по часовой
      if (speedNewClk == LOW && speedValueDT == LOW) {rotation-=500;};//при движении крутилки против часовой
      rotation = constrain(rotation, 5000, cycle-3000);
      lcd.setCursor(6, 1);   
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
      lcd.print(float(rotation/1000.0),1);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
      lcd.print("  ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
      cycleSpeedDeg = 4*angle/square(rotation/1000);   
      motor.setAccelerationDeg(cycleSpeedDeg);};
  }

    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();

  //изменения угла поворота
  int accelNewClk = digitalRead(accelCLK);
  if (accelNewClk != accelLastClk){
    accelLastClk = accelNewClk;
    int accelValueDT = digitalRead(accelDT);
    if (accelNewClk == LOW && accelValueDT == HIGH) {angle+=incVar;} //при движении крутилки по часовой
    if (accelNewClk == LOW && accelValueDT == LOW) {angle-=incVar;};//при движении крутилки против часовой
    angle = constrain(angle, incVar, 3600);
    lcd.setCursor(0, 1);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(angle);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    if(angle<=100){lcd.print(" ");};
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    if(angle<=10){lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    };
    cycleSpeedDeg = (2*angle)/(square(rotation/1000));
    motor.setAccelerationDeg(2*cycleSpeedDeg);};

    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();

  //запуск по таймеру
  if (digitalRead(stopButton)==true && millis() - cycleTime >= cycle){
    if(stopButtonFunc == HIGH){stopButtonFunc = LOW;};
    
    if (direct == HIGH) {motor.setTargetDeg(angle, RELATIVE);}
    else{motor.setTargetDeg(-angle, RELATIVE);};
    cycleTime = millis();
  };    

    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
  
  //останов
  if (digitalRead(stopButton)==false && stopButtonFunc == LOW) {
    motor.stop();
    stopButtonFunc = HIGH;
    encoderTime = millis();  
  };
}



if(motorMod == 3){
  //обнуление позиции
  if (stopButtonFunc == HIGH && digitalRead(speedSW) == LOW && millis() - encoderTime >= 2500){
    position = 0;
    motor.setCurrentDeg(position);
    lcd.setCursor(8, 1);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(int(position));
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print("       ");
  };

    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();

  //установка угла поворота
  int speedNewClk = digitalRead(speedCLK); //условие срабатывания энкодера для изменения паузы между махами
  if (speedNewClk != speedLastClk) {
    speedLastClk = speedNewClk;
    int speedValueDT = digitalRead(speedDT);
    if (speedNewClk == LOW && speedValueDT == HIGH) {position=position+posIncrem;} //при движении крутилки по часовой - увеличить время маха на 0,05 с
    if (speedNewClk == LOW && speedValueDT == LOW) {position=position-posIncrem;}; //при движении крутилки против часовой - уменьшить время маха на 0,05 с
    lcd.setCursor(8, 1);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(position);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
  };

    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();

  //выставление инкремента угла
  int accelNewClk = digitalRead(accelCLK); //условие срабатывания энкодера для изменения паузы между махами
  if (accelNewClk != accelLastClk){
    accelLastClk = accelNewClk;
    int accelValueDT = digitalRead(accelDT);
    if (accelNewClk == LOW && accelValueDT == HIGH) {posIncrem=posIncrem+incVar;} //при движении крутилки по часовой - увеличить время маха на 0,05 с
    if (accelNewClk == LOW && accelValueDT == LOW) {posIncrem=posIncrem-incVar;}; //при движении крутилки против часовой - уменьшить время маха на 0,05 с
    posIncrem = constrain(posIncrem, incVar, 3600);
    lcd.setCursor(0, 1);
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    if (posIncrem>=1000){lcd.print(posIncrem);}
    else{lcd.print(" ");
      lcd.print(posIncrem);}
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
    lcd.print(" ");
      if (stopFunc == LOW) {motor.tick();motor.tick();motor.tick();motor.tick();motor.tick();};
  };

    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();

  //перемещение в координату
  if (digitalRead(stopButton)==true && oldPosition != position){
    oldPosition = position;
    motor.setTargetDeg(position, ABSOLUTE);
    if(stopButtonFunc == HIGH){stopButtonFunc = LOW;};
    }
 
  if (stopFunc == HIGH && digitalRead(stopButton)==true && motor.getCurrent() != position){
    motor.setTargetDeg(position, ABSOLUTE);
    if(stopButtonFunc == HIGH){stopButtonFunc = LOW;}; 
    if(stopFunc == HIGH){stopFunc = LOW;};     
  };

    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();
    motor.tick();

  //останов
  if (digitalRead(stopButton)==false && stopButtonFunc == LOW) {
    motor.stop();
    if(stopButtonFunc == LOW){stopButtonFunc = HIGH;};
    if(stopFunc == LOW){stopFunc = HIGH;}; 
    encoderTime = millis();  
    };
}
}