/*
  WOKWI simulation of rotary stand for photogrametry (on build).
  Firmware 1.7

  Jaime J. Diaz T.
  October 10 2022

  Rotary table moved by DC motor with optoencoder;
  1 motor turn = 32 encoder ticks. [// byte motorTurnTicks = 32;]
  1 motor turn = 1 degree (worm gear coupled). [int tableTurnTicks = 11520; // motorTurnTicks x 360º]

  Pourpose made power shield has:
  12VDC barrel jack and polarity protection.
  7805 to power all.
  BC517 transistors with LED for driving motor and camera trigger relay.
  7414 Schmitt to square optoencoder signal.
  Pinheader connector for motor and encoder on rotating stand.
  Pinheader connector for I2C 2004 LCD and rotary encoder selector.

  Camera trigger output isolated by relay, has LED pilot.

  Parameters selected thru rotary encoder, values shown on I2C LCD 20x4. (0x27)
  >N. Stops (photos) per 360º turn [2-100; default 50], also shown degrees between stops.
  >Pause since motor stops until camera trigger pulse [100-1000; default 250ms], alows oscilations dampening.
  >Pulse camera trigger lenght [100-250; default 200ms], to adapt diferent relay / camera needs.
  >Photo: Give camera time to do it [1-10; default 5 seconds], adapt to camera needs.
  >START; once selected, the programed sequence begins, parameter change is locked, only STOP accepted.
  At end of one rotary stand revolution, STOPs and enables parameter change.
  If sequence stopped, this point becomes new 0º reference.
  Also displayed stop count and actual position in degrees.

  Optoencoder serviced with interrupts.
  Selector encoder serviced by pooling.

  Arduino pins used [ #define ]:
  +5V
  GND
  A4 - SDA I2C [default]
  A5 - SCK I2C [default]
  D3 - optoencoder A pulse [interrupt]
  //D4 - optoencoder B pulse [interrupt] // > tested, no improvement
  //D5 - Brake transistor output // > tested, no improvement
  D6 - Motor transistor output
  D7 - selector encoder CLK
  D8 - selector encoder DT
  D9 - selector encoder SW
  D13[LED] - Camera trigger transistor output

  Motor and Camera Relay need drive transistors to boost current.
  Optoencoder signal squared with Schmitt trigger. [ unnecesary? ]

*/

#include <LiquidCrystal_I2C.h>

#define firmware "1.7"

//motor encoder PIN definition
#define encoder_PinA  2
//#define encoder_PinB  3

//motor encoder interrupt variables
volatile int encoder_Pos = 0;
//volatile boolean encoder_A = 0; // only needed for quad resolution
//volatile boolean encoder_B = 0;
volatile boolean RotationDetected; // need volatile for Interrupts

//motor control pin definition
#define motor_PIN 5
//#define brake_PIN 4

//actual motor position variables
int position = 0;
float pos = 0;
byte StopCount = 0;
// byte motorTurnTicks = 32;
int tableTurnTicks = 11520; // motorTurnTicks x 360º

//camera trigger pin definition
#define camera_TRIG 13

//camera timekeeper
unsigned long now; // store millis(), without rollover check @ 50 days

//selector encoder PIN definition
#define ENCODER_CLK 6
#define ENCODER_DT  7
#define ENCODER_SW  8

//selector functions variables
bool start = false;
uint16_t numstops = 50; // give a default 2 - 100 step 2
uint16_t pause = 250;  // give a default 100 - 1000 step 50
uint8_t pulse = 200;  // give a default 100 - 250 step 10
int cameraJobTime = 5000;  //default camera time to take photo (ms) step 1000
int motorEncoderTicsPerDivision = 230;  // (32 ticks per degree x 360º / 50 numstops) = 230.4

long int modeLastChanged = 0;
bool selectorChanged = false;
bool clk = HIGH;
bool prevClk = HIGH;

typedef enum
{
  SET_START,
  SET_NUMSTOPS,
  SET_PAUSE,
  SET_PULSE,
  SET_PHOTO,
} Mode;
Mode mode = SET_NUMSTOPS;

// Create lcd object 20x4 @ I2C 0x27
LiquidCrystal_I2C lcd(0x27, 20, 4);


void setup ()
{
  //Serial.begin(115200); //only if Serial.print() debugging

  // >> configure selector encoder pins
  pinMode(ENCODER_CLK, INPUT_PULLUP);
  pinMode(ENCODER_DT, INPUT_PULLUP);
  pinMode(ENCODER_SW, INPUT_PULLUP);

  // >> configure motor encoder pins
  pinMode(encoder_PinA, INPUT);
  //turn on pullup resistor
  //digitalWrite(encoder_PinA, HIGH); //ONLY FOR SOME ENCODER!!!!
  //pinMode(encoder_PinB, INPUT);
  //turn on pullup resistor
  //digitalWrite(encoder_PinB, HIGH); //ONLY FOR SOME ENCODER!!!!
  //encoder_A = (boolean)digitalRead(encoder_PinA); //initial value of channel A (only needed for quad resolution)
  //encoder_B = (boolean)digitalRead(encoder_PinB); //initial value of channel B (checked just before run)// > tested, no improvement

  // encoder A channel on interrupt 0 (Arduino's pin 2)
  attachInterrupt(digitalPinToInterrupt(encoder_PinA), doMotorEncoderA, RISING);
  //attachInterrupt(0, doMotorEncoderA, RISING);
  // encoder B channel pin on interrupt 1 (Arduino's pin 3)
  //attachInterrupt(1, doMotorEncoderB, CHANGE);

  // >> configure motor control pin
  pinMode(motor_PIN, OUTPUT);
  //pinMode(brake_PIN, OUTPUT);

  // >> configure camera control pin
  pinMode(camera_TRIG, OUTPUT);
  
  // >> configure lcd
  lcd.init();                      // initialize the LCD dsplay

  lcd.setCursor(1, 0);
  lcd.print("Rotating Stand");
  lcd.setCursor(1, 1);
  lcd.print("Firmware ");
  lcd.setCursor(11,1);
  lcd.print(firmware);

  lcd.backlight();

  //  3 blinks of backlight - ready
  for (int i = 0; i < 3; i++)
  {
    lcd.noBacklight();
    delay(250);
    lcd.backlight();
    delay(250);
  }
  delay(1000);

  // display labels
  lcd.setCursor(1, 0);
  lcd.print("START ");
  lcd.setCursor(1, 1);
  lcd.print("N. Stops: ");
  lcd.setCursor(1, 2);
  lcd.print("Pause ms: ");
  lcd.setCursor(1, 3);
  lcd.print("Pulse ms: ");
  lcd.setCursor(7, 0);
  lcd.print("Now  ");
  lcd.write(126);
  lcd.print("0");
  lcd.write(223);

  //  display default values
  updateDisplay();
}

void loop ()
{
  // configure job
  selector();

  // do the job !!!
  if (start)
   {
    motor();
   }

  // update position displayed for unmotorized turns
  if (RotationDetected)
   {
    displayPosition();
    RotationDetected = false;
   }

}


//_______________FUNCTIONS_________________________________________________

//________________________________________________________________________
// >> Motor functions
//________________________________________________________________________
void motor()
{
  // >> move motor to next stop,
  //    pause to damp vibration,
  //    trigger photo,
  //    wait cameraJobTime,
  //    and continue to next function on loop()
  
  float nextStop = motorEncoderTicsPerDivision * ( StopCount + 1 ); // next stop position
  bool giro = true;// enable rotation
  //digitalWrite(brake_PIN, HIGH);// > tested, no improvement
  //encoder_B = (boolean)digitalRead(encoder_PinB); // so optoencoder keeps counting upwards 
  digitalWrite(motor_PIN, HIGH);
  
  while (start && giro)   //   move motor to next stop,
  {
    position = encoder_Pos; // position now
    if (position >= nextStop)
    {
      displayPosition();
      giro = false;
    }
    // update position only if changed
    if (RotationDetected)
    {
      //Keep updating till next pause
      displayPosition();
      RotationDetected = false;
    }
    // check STOP
    checkSelectorEncoder();
  }
  
  digitalWrite(motor_PIN, LOW);
  //digitalWrite(brake_PIN, LOW);// > tested, no improvement
  if (start)
  { StopCount = StopCount + 1;
    displayStopCount();
    Pause();   //    pause to damp vibration,
    digitalWrite(camera_TRIG, HIGH);
    displayPosition();
    Pulse();   //    trigger photo,
    digitalWrite(camera_TRIG, LOW);
    displayPosition();
    CameraJobTime();    //   give the camera time to do it
  }
  // if job interrupted, consider this position new zero and reset counters
  if (!start && encoder_Pos < tableTurnTicks)
  {
    displayPosition();
    StopCount = 0;
    displayStopCount();
    encoder_Pos = 0;
  }
  // stop after one turn completed and reset counters
  if (encoder_Pos >= tableTurnTicks)
  {
    start = false;
    // reset StopCount
    StopCount = 0;
    displayStopCount();
    // display STOP
    updateDisplay();
    // reset encoder_Pos
    encoder_Pos = encoder_Pos - tableTurnTicks; // keep counting from same zero ? keep counter less than one turn
    //encoder_Pos = 0; // take this position new zero ?
  }
}

//________________________________________________________________________
// >> Camera time functions
//_______________________________
void Pause()
{
  now = millis();
  while (now + pause > millis())
  {
    checkSelectorEncoder();
  }
}

//_______________________________
void Pulse()
{
delay(pulse);
checkSelectorEncoder();
}

//_______________________________
void CameraJobTime()
{
  now = millis();
  lcd.setCursor(15, 2);
  lcd.print("Photo");
  int second = 0;
  while (now + cameraJobTime > millis())
  {
    checkSelectorEncoder();
    int time = ( millis() - now ) / 1000; // seconds elapsed
    if ( second == time) // update count only if second changed
    {
      second ++;
      lcd.setCursor(16, 3);
      lcd.print("    ");
      lcd.setCursor(16, 3);
      lcd.print( cameraJobTime / 1000 - time ); // display seconds remaining
    }
  }
  lcd.setCursor(15, 2);
  lcd.print("     ");
  lcd.setCursor(16, 3);
  lcd.print("    ");
}


//________________________________________________________________________
// >> Display functions
//_______________________________
// update actual position displayed in degrees
void displayPosition()
{
  // positive position (CW)
  if (encoder_Pos > 0)
  {
    position = encoder_Pos % tableTurnTicks;
    pos = float (position) / 32;
  }
  /*
  // negative position (CCW) [unnecesary? keep as guard for hand rotation]
  else
  {
    position = (65536 - encoder_Pos);
    position = position % tableTurnTicks;
    pos = float (position) / 32;
    pos = 360 - pos;
    if (pos == 360)
    {
      pos = 0;
    }
  }
  */
  updatePositionDisplay();// display update
}

//_______________________________
void updatePositionDisplay()
{
  lcd.setCursor(13, 0);
  lcd.print("       ");
  lcd.setCursor(13, 0);
  lcd.print(pos);
  lcd.write(223);
}

//_______________________________
void displayStopCount()
{
  lcd.setCursor(10, 0);
  lcd.print("  ");
  if (StopCount < 10)
  {
    lcd.setCursor(11, 0);
  }
  else
  {
    lcd.setCursor(10, 0);
  }
  lcd.print(StopCount % 100);
}

//_______________________________
void updateDisplay()
{
  lcd.setCursor(10, 1);
  lcd.print("   ");
  lcd.setCursor(10, 1);
  lcd.print(numstops);
  lcd.setCursor(13, 1);
  lcd.write(126);
  lcd.print(360 / float (numstops));
  if (numstops > 3)
  {
    lcd.write(223);
  }
  lcd.print(" ");
  if (mode == SET_NUMSTOPS)
  {
    lcd.setCursor(0, 0);
    lcd.print(" ");
    lcd.setCursor(0, 1);
    lcd.print(">");
  }

  lcd.setCursor(10, 2);
  lcd.print("    ");
  lcd.setCursor(10, 2);
  lcd.print(pause);
  if (mode == SET_PAUSE)
  {
    lcd.setCursor(0, 1);
    lcd.print(" ");
    lcd.setCursor(0, 2);
    lcd.print(">");
  }

  lcd.setCursor(10, 3);
  lcd.print("   ");
  lcd.setCursor(10, 3);
  lcd.print(pulse);
  if (mode == SET_PULSE)
  {
    lcd.setCursor(0, 2);
    lcd.print(" ");
    lcd.setCursor(0, 3);
    lcd.print(">");
  }

  if (!start)
  {
    lcd.setCursor(15, 2);
    lcd.print("Photo");
    lcd.setCursor(16, 3);
    lcd.print("    ");
    lcd.setCursor(16, 3);
    lcd.print( cameraJobTime / 1000 );
    lcd.print("s");
    if (mode == SET_PHOTO)
    {
      lcd.setCursor(0, 3);
      lcd.print(" ");
      lcd.setCursor(14, 2);
      lcd.print(">");
    }
  }

  if (start)
  {
    lcd.setCursor(15, 2);
    lcd.print("     ");
    lcd.setCursor(16, 3);
    lcd.print("    ");
    lcd.setCursor(1, 0);
    lcd.print("STOP ");
  }
  else
  {
    lcd.setCursor(1, 0);
    lcd.print("START");
  }
  if (mode == SET_START)
  {
    lcd.setCursor(14, 2);
    lcd.print(" ");
    lcd.setCursor(0, 0);
    lcd.print(">");
  }
}


//________________________________________________________________________
// >> Selector functions
//________________________________________________________________________
void selector()
{
  checkSelectorEncoder();
  if (selectorChanged)
  {
    selectorChanged = false;
    updateDisplay();
  }
}

//_______________________________
void checkSelectorEncoder()
{
  if (digitalRead(ENCODER_SW) == LOW && millis() - modeLastChanged > 300)
  {
    modeLastChanged = millis();
    nextMode();
    selectorChanged = true;
  }

  clk = digitalRead(ENCODER_CLK);
  if (clk != prevClk && clk == LOW)
  {
    int dt = digitalRead(ENCODER_DT);
    int increment = dt == HIGH ? 1 : -1;
    updateValue(increment);
    selectorChanged = true;
  }
  prevClk = clk;
}

//_______________________________
void nextMode()
{
  if (!start)
  {
    switch (mode)
    {
      case SET_NUMSTOPS:
        mode = SET_PAUSE;
        break;

      case SET_PAUSE:
        mode = SET_PULSE;
        break;

      case SET_PULSE:
        mode = SET_PHOTO;
        break;

      case SET_PHOTO:
        mode = SET_START;
        break;

      case SET_START:
        mode = SET_NUMSTOPS;
        break;
    }
  }
}

//_______________________________
void updateValue(int increment)
{
  switch (mode)
  {
    case SET_NUMSTOPS:
      numstops = constrain(numstops + 2 * increment, 2, 100);
      motorEncoderTicsPerDivision = tableTurnTicks / numstops;
      break;

    case SET_PAUSE:
      pause = constrain(pause + 50 * increment, 100, 1000);
      break;

    case SET_PULSE:
      pulse = constrain(pulse + 10 * increment, 100, 250);
      break;

    case SET_PHOTO:
      cameraJobTime = constrain(cameraJobTime + 1000 * increment, 1000, 10000);
      break;

    case SET_START:
      start = constrain(start + increment, 0, 1);
      break;
  }
}

//________________________________________________________________________
// >> Optoencoder interrupt functions
//________________________________________________________________________
// >> Interrupt routine counts motor ticks if CLK (A) goes from LOW to HIGH
/* >> Checks DT (B) to know rotation direction // > tested, no improvement
//you may easily modify the code to get quad resolution
//but be sure Arduino can keep the rhythm*/
//________________________________________________________________________
void doMotorEncoderA()
{
  encoder_Pos++;
  /*encoder_B ? encoder_Pos-- :  encoder_Pos++;*/
  RotationDetected = true;
}
/*
void doMotorEncoderB()// > tested, no improvement
{
  encoder_B = !encoder_B;
}
*/