/*
   Dual stepper control using encoder..
   5.1.2023 ~q
*/

#include <LiquidCrystal_I2C.h>
#include <ezButton.h>
#include <AccelStepper.h>
//screen
LiquidCrystal_I2C lcd(0x27, 16, 2);
//encoder
#define BTN_UP   13
#define BTN_DOWN 12
#define BTN_SEL  11
//encoder 2
#define BTN2_UP   8
#define BTN2_DOWN 9
#define BTN2_SEL  10

#define STEPS_PER_REV 200
#define DEG_STEP 360 / STEPS_PER_REV
#define ZERO_OFFSET 2

//encoder tracking
int encoder1Val = 0; // 0 down, 1 up
int encoder2Val = 0; // 0 down , 1 up
int encoder1Last =  0;//remember last values
int encoder2Last =  0;//..

unsigned long bValue = 0;//counter for button presses
unsigned long bValue2 = 0;//counter for button presses

//timer and interval for encoder debouncing
unsigned long lastSample = 0;
int intervalSample = 50;
//select button on encoder..
ezButton button(BTN_SEL);
ezButton button2(BTN2_SEL);

int lastClk = HIGH;
int lastClk2 = HIGH;

//starting pos for motors in steps..
int Motor1Start = 2;
int Motor2Start = 12;
int Motor1Pos = 2;
int Motor2Pos = Motor2Start;
// Define the stepper motor and the pins that is connected to
AccelStepper stepper1(1, 3, 2); // (Typeof driver: with 2 pins, STEP, DIR)
AccelStepper stepper2(1, 5, 4);


void setup() {
  lcd.init();
  lcd.backlight();
  lcd.setCursor(1, 0);
  pinMode(BTN_UP, INPUT_PULLUP);
  pinMode(BTN_DOWN, INPUT_PULLUP);
  pinMode(BTN_SEL, INPUT_PULLUP);
  pinMode(BTN2_UP, INPUT_PULLUP);
  pinMode(BTN2_DOWN, INPUT_PULLUP);
  pinMode(BTN2_SEL, INPUT_PULLUP);
  button.setDebounceTime(50);//50 ms debounce
  button.setCountMode(COUNT_RISING);
  button2.setDebounceTime(50);//50 ms debounce
  button2.setCountMode(COUNT_RISING);

  stepper1.setMaxSpeed(100); // Set maximum speed value for the stepper
  stepper1.setAcceleration(100); // Set acceleration value for the stepper
  stepper1.setCurrentPosition(0); // Set the current position to 0 steps

  stepper2.setMaxSpeed(100);
  stepper2.setAcceleration(100);
  stepper2.setCurrentPosition(0);

  stepper1.moveTo(Motor1Start);
  stepper2.moveTo(Motor2Start);

  lcd.setCursor(0, 0);
  lcd.print("Motor 1 :");
  lcd.setCursor(0, 1);
  lcd.print("Motor 2 :");
  lcd.setCursor(10, 0);
  lcd.print((Motor1Pos - ZERO_OFFSET) * DEG_STEP);
  lcd.setCursor(10, 1);
  lcd.print((Motor2Pos - ZERO_OFFSET) * DEG_STEP);

}


void loop() {
  checkEncoders();
  stepper1.run();
  stepper2.run();
}



void checkEncoders()
{
  button.loop(); // MUST call the loop() function first
  button2.loop();
  bool gotClick = false;
  bool gotClick2 = false;

  unsigned long currMillis = millis();
  if (currMillis - lastSample >= intervalSample)
  {

    unsigned long count =  button.getCount();
    if (count != bValue)
    {
      bValue = count;
      Motor1Pos = Motor1Start;
      stepper1.runToNewPosition(Motor1Pos);
      lcd.setCursor(10, 0);
      lcd.print("  ");
      lcd.setCursor(10, 0);
      lcd.print((Motor1Pos - ZERO_OFFSET) * DEG_STEP);
    }

    count =  button2.getCount();
    if (count != bValue2)
    {
      bValue2 = count;
      Motor2Pos = Motor2Start;
      stepper2.runToNewPosition(Motor2Pos);
      lcd.setCursor(10, 1);
      lcd.print("  ");
      lcd.setCursor(10, 1);
      lcd.print((Motor2Pos - ZERO_OFFSET) * DEG_STEP);
    }

    //encoder 1 movement tracking..
    int newClk = digitalRead(BTN_UP);
    if (newClk != encoder1Last) {
      // There was a change on the CLK pin
      encoder1Last = newClk;

      int dtValue = digitalRead(BTN_DOWN);
      if (newClk == LOW && dtValue == HIGH) {
        encoder1Val = 1;
        gotClick = true;
      }
      if (newClk == LOW && dtValue == LOW) {
        encoder1Val = 0;
        gotClick = true;
      }
    }

    //encoder 2 movement tracking..
    int newClk2 = digitalRead(BTN2_UP);
    if (newClk2 != encoder2Last) {
      // There was a change on the CLK pin
      encoder2Last = newClk2;

      int dtValue2 = digitalRead(BTN2_DOWN);
      if (newClk2 == LOW && dtValue2 == HIGH) {
        encoder2Val = 1;
        gotClick2 = true;
      }
      if (newClk2 == LOW && dtValue2 == LOW) {
        encoder2Val = 0;
        gotClick2 = true;
      }
    }
  }




  if (gotClick)
  {
    if (encoder1Val == 0)
    {
      //down
      Motor1Pos--;
    } else if (encoder1Val > 0)
    {
      //up
      Motor1Pos++;
    }
    stepper1.moveTo(Motor1Pos);
    lcd.setCursor(10, 0);
    lcd.print("  ");
    lcd.setCursor(10, 0);
    lcd.print((Motor1Pos - ZERO_OFFSET) * DEG_STEP);
  }

  if (gotClick2)
  {
    if (encoder2Val == 0)
    {
      //down
      Motor2Pos--;
    } else if (encoder2Val > 0)
    {
      //up
      Motor2Pos++;
    }
    stepper2.moveTo(Motor2Pos);
    lcd.setCursor(10, 1);
    lcd.print("  ");
    lcd.setCursor(10, 1);
    lcd.print((Motor2Pos - ZERO_OFFSET) * DEG_STEP);
  }
  
}