// Drill press knee lift software
// Version 12
// 20th May 2024
// author:routerCNC
// up and down buttons
// speed control
// stepper motor runs
// limit stops
// stepper brake
// drill type (setup)
// selected option displayed on LCD
// mm/sec
// step function
// acceleration

// Libraries
#include <LiquidCrystal_I2C.h>       // LCD library
LiquidCrystal_I2C lcd(0x27, 16, 2);  // set the LCD address to 0x3F for a 16 chars and 2 line display
#include <EEPROM.h>                  // not implemented, but could allow changed variables to be stored in ROM when powered off

// Assign motor control INPUTS pins
int eStopPin = 2;            // high = OK, low = estop (safe way around)
int UpperLimitPin = 3;       // table reached upper travel limit so stop
int LowerLimitPin = 4;       // table reached lower travel limit so stop
int ButtonUpPin = 6;         // up button
int ButtonDownPin = 7;       // down button
int SpeedControlPin = A0;    // speed potentiometer channel
int DrillKneeContPin = 11;   // pin if knee continuous movement selected (3-way switch)
int DrillKneeDrillPin = 12;  // pin if drill cycle selected (3-way switch)

// Assign motor control OUTPUT pins
int MotorDirectionPin = 8;  // HIGH = forward, LOW = reverse
int MotorEnablePin = 9;     // HIGH = ?, LOW = ? (output depends on e-stop switch on pin 2)
int MotorStepPin = 10;      // Pulse train to control motor speed via stepper driver
int BrakeEnablePin = 5;     // HIGH = brake released, LOW = brake on

// Assign display and keypad pins
int i2c_SDA_Pin = A4;  // data pin
int i2c_SLK_Pin = A5;  // clock pin
int KeypadPin = A1;    // analog pin to read key pad inputs from user

// Control variables for actions
int ReadPressedUp = 0;     // up button, 0 = not pressed, 1 = pressed
int ReadPressedDown = 0;   // down button, 0 = not pressed, 1 = pressed
int ReadSpeedPot = 0;      // motor speed potentiometer reading (0-1022)
int ReadKeyPad = 0;        // various analog readings depending on key pressed (0-1022)
int ReadUpperLimit = 0;    // upper limit switch, 0 = OK, 1 = hit
int ReadLowerLimit = 0;    // lower limit switch, 0 = OK, 1 = hit
int ReadEstop = 0;         // e-stop condition.  if triggered change state of enable pin on stepper driver
int ReadKneeContPin = 0;   // 3-way selector, 0 = not pressed, 1 = pressed
int ReadKneeDrillPin = 0;  // 3-way selector, 0 = not pressed, 1 = pressed
int DrillMode = 0;         // for normal operation this holds the outcome of drill mode selection from ReadKneeContPin and ReadKneeDrillPin (0=cont,1=step,2=drill)

//// Drilling control variables (for step, cont, and drill)
// STEP: up = +0.1mm step value, down = -0.1mm step value (min = 0.1, max = 5)
float DrillControl_StepDepth = 5;
float DrillControl_StepSpeed = 0.05;  // scale delay based on feedrate
// DRILL CYCLE: up = +0.25mm total depth, down = -0.25mm total depth (min 0.25, max 20)
float DrillControl_Cycledepth = 0.5;  // default canned cycle drilling depth 0.5mm


// Pulse control variables
int PWM_Width = 5;                   // width of pulse in microseconds (default 10us)

// Ballscrew motion variables
float BallscrewPitch = 5;                                                                // ballscrew pitch in mm (ballscrew linear movement per ballnut revolution)
float MotorPulleyTeeth = 24;                                                             // teeth count on motor pulley (motor end)
float DrivePulleyTeeth = 36;                                                             // teeth count on main drive pulley (ballnut end)
float StepperPulseRev = 1600;                                                            // stepper pulses per revolution (= stepper pulse per rev (200) X micro stepping (8) )
float MotorPulleyRatio = DrivePulleyTeeth / MotorPulleyTeeth;                            // how many motor turns for one ballnut turn
float MotorPulsesPerMillimeter = (StepperPulseRev * MotorPulleyRatio) / BallscrewPitch;  // How many pulse required per mm of ballscrew travel
float MotorTimeDelayMicro = 0;                                                           // How long to wait before sending a pulse (depends on speed required) [time in microseconds]
float ReadCurrentTime = 0;
float ReadOldTime = 0;

// Motor speed variables for continuous running
int MotorMaxSpeed = 100;             // max motor speed in mm/sec
int MotorMinSpeed = 1;               // min motor speed in mm/sec
float MotorAcceleration = 500;       // acceleration rate of stepper motor (smaller value = quicker acceleration, larger value = slower acceleration)
float OutputSpeedPotMap = 0;         // mapped speed value (arduino units from pot into mm/sec)
float MotorAccelerationCounter = 0;  // acceleration counter for stepper motor

// Motor variables for step movement
int StepPulseCounter = 0;  // pulse counter when stepping

//*****************************************************************
// set up
//*****************************************************************
void setup() {

  // LCD display initialise
  lcd.init();       // initialise LCD
  lcd.clear();      // clear display
  lcd.backlight();  // Make sure backlight is on

  // open serial port
  Serial.begin(9600);

  // Configure input control pins
  pinMode(ButtonUpPin, INPUT_PULLUP);
  pinMode(ButtonDownPin, INPUT_PULLUP);
  pinMode(UpperLimitPin, INPUT_PULLUP);
  pinMode(LowerLimitPin, INPUT_PULLUP);
  pinMode(eStopPin, INPUT_PULLUP);
  pinMode(SpeedControlPin, INPUT);
  pinMode(DrillKneeContPin, INPUT_PULLUP);
  pinMode(DrillKneeDrillPin, INPUT_PULLUP);

  // Configure output motor control pins
  pinMode(MotorStepPin, OUTPUT);
  pinMode(MotorDirectionPin, OUTPUT);
  pinMode(MotorEnablePin, OUTPUT);
  pinMode(BrakeEnablePin, OUTPUT);

  // Configure keypad
  pinMode(KeypadPin, INPUT);

  // Reset motor to zero, no direction
  digitalWrite(MotorStepPin, LOW);
  digitalWrite(MotorDirectionPin, LOW);
  digitalWrite(MotorEnablePin, LOW);

  // setup PWM pin frequency (not used in the end)
  TCCR1B = TCCR1B & B11111000 | B00000011;  // for PWM frequency of 490.20 Hz (The DEFAULT)
  //TCCR1B = TCCR1B & B11111000 | B00000010;  // for PWM frequency of 3921.16 Hz
  //TCCR1B = TCCR1B & B11111000 | B00000001;  // set timer 1 divisor to 1 for PWM frequency of 31372.55 Hz

  // Display welcome message
  lcd.setCursor(0, 0);
  lcd.print("Loading . . .");
  delay(800);
  lcd.clear();
}

//******************************************************************
// MAIN LOOP
//******************************************************************
void loop() {

  // ***************************************************************
  // read user button status and limits
  // ***************************************************************
  // Read upper and lower limits
  ReadUpperLimit = digitalRead(UpperLimitPin);
  ReadLowerLimit = digitalRead(LowerLimitPin);

  // Read user inputs
  ReadPressedUp = digitalRead(ButtonUpPin);
  ReadPressedDown = digitalRead(ButtonDownPin);
  ReadSpeedPot = analogRead(SpeedControlPin);
  ReadKeyPad = analogRead(KeypadPin);
  ReadEstop = digitalRead(eStopPin);
  ReadKneeContPin = digitalRead(DrillKneeContPin);
  ReadKneeDrillPin = digitalRead(DrillKneeDrillPin);

  // *****************************************************************
  // determine drilling mode (cont = 0, step = 1 or canned cycle = 2)
  // *****************************************************************
  if (ReadKneeContPin == LOW) {
    DrillMode = 0;  // continuous 0

  } else if (ReadKneeDrillPin == LOW) {
    DrillMode = 2;  // canned drill cycle 2

  } else {
    DrillMode = 1;  // step movements 1
  }

  //*******************************************************************
  //engage stepper brake to prevent movement
  //*******************************************************************
  digitalWrite(BrakeEnablePin, HIGH);

  //*******************************************************************
  // Check if E_STOP pressed, if not continue normally
  //*******************************************************************
  if (ReadEstop == HIGH) {
    // e-Stop button pressed.  Disable stepper driver.  No key presses accepted.
    digitalWrite(MotorEnablePin, HIGH);
    // display message
    lcd.setCursor(0, 0);
    lcd.print("E-STOP       ");
    Serial.println("eSTOP");
  } else {
    // e-stop button not pressed.  Enable stepper driver.
    digitalWrite(MotorEnablePin, LOW);
    //**********************************************************
    // configure motor speed based on user potentiometer setting
    //**********************************************************
    OutputSpeedPotMap = map(ReadSpeedPot, 1023, 1, MotorMaxSpeed, MotorMinSpeed);    // arduino units max pot, arduino units min pot, mm/sec MAX, mm/sec MIN
    MotorTimeDelayMicro = 1000000 / (OutputSpeedPotMap * MotorPulsesPerMillimeter);  // microsecond time delay between pulses

    // *******************************************************
    // if CONT mode 0
    // *******************************************************
    if (DrillMode == 0) {
      // display function
      lcd.setCursor(0, 1);
      lcd.print("CONT ");

      // display speed on LCD
      lcd.setCursor(6, 1);
      lcd.print(String(OutputSpeedPotMap) + "  ");
      lcd.setCursor(10, 1);
      lcd.print("mm/sec");

      //// TABLE UP CONTINUOUS
      // display status on LCD
      if (ReadPressedUp == LOW) {
        lcd.setCursor(0, 0);
        lcd.print("Up           ");
      }
      // keep looping if up button pressed and upper limit not hit
      // read current time
      ReadOldTime = micros();
      if (ReadPressedUp == LOW && ReadUpperLimit == HIGH) {
        // release stepper brake
        digitalWrite(BrakeEnablePin, LOW);
        // set direction
        digitalWrite(MotorDirectionPin, LOW);
      }
      // reset acceleration counter
      MotorAccelerationCounter = MotorAcceleration;
      // start movement loop
      while (ReadPressedUp == LOW && ReadUpperLimit == HIGH) {
        // wait correct milliseconds before sending pulse (to control speed)
        while (micros() < (ReadOldTime + MotorTimeDelayMicro + MotorAccelerationCounter)) {}
        // send single pulse with enough width to trigger the motor driver
        digitalWrite(MotorStepPin, HIGH);
        delayMicroseconds(PWM_Width);
        digitalWrite(MotorStepPin, LOW);
        // read time of last pulse sent out
        ReadOldTime = micros();
        // check user up button and upper limit
        ReadPressedUp = digitalRead(ButtonUpPin);
        ReadUpperLimit = digitalRead(UpperLimitPin);
        // reduce acceleration counter to ramp up velocity
        if (MotorAccelerationCounter > 1) { MotorAccelerationCounter = MotorAccelerationCounter - 1; }
      }

      //// TABLE DOWN CONTINUOUS
      if (ReadPressedDown == LOW) {
        lcd.setCursor(0, 0);
        lcd.print("Down         ");
      }
      // keep looping if down button pressed and lower limit not hit
      // read current time
      ReadOldTime = micros();
      if (ReadPressedDown == LOW && ReadLowerLimit == HIGH) {
        // release stepper brake
        digitalWrite(BrakeEnablePin, LOW);
        // set direction
        digitalWrite(MotorDirectionPin, HIGH);
      }
      // reset acceleration counter
      MotorAccelerationCounter = MotorAcceleration;
      // start movement loop
      while (ReadPressedDown == LOW && ReadLowerLimit == HIGH) {
        // wait correct milliseconds before sending pulse (to control speed)
        while (micros() < (ReadOldTime + MotorTimeDelayMicro + MotorAccelerationCounter)) {}
        // send single pulse with enough width to trigger the motor driver
        digitalWrite(MotorStepPin, HIGH);
        delayMicroseconds(PWM_Width);
        digitalWrite(MotorStepPin, LOW);
        // read time of last pulse sent out
        ReadOldTime = micros();
        // check user down button and lower limit
        ReadPressedDown = digitalRead(ButtonDownPin);
        ReadLowerLimit = digitalRead(LowerLimitPin);
        // reduce acceleration counter to ramp up velocity
        if (MotorAccelerationCounter > 1) { MotorAccelerationCounter = MotorAccelerationCounter - 1; }
      }
    }

    // ****************************************************
    // if STEP mode 1
    // ****************************************************
    if (DrillMode == 1) {
      // display function
      lcd.setCursor(0, 1);
      lcd.print("STEP ");
      // display parameters for step drilling (step depth, speed %)
      // display step value
      lcd.setCursor(6, 1);
      lcd.print(DrillControl_StepDepth);
      lcd.setCursor(10, 1);
      lcd.print("mm    ");

      // check if up button pressed (and not hit upper limit)
      if (ReadPressedUp == LOW && ReadUpperLimit == HIGH) {
        // release stepper brake
        digitalWrite(BrakeEnablePin, LOW);
        // set direction
        digitalWrite(MotorDirectionPin, LOW);
        // calculate how many pulses needed
        StepPulseCounter = MotorPulsesPerMillimeter * DrillControl_StepDepth;
        // loop through sending exact number of pulses depending on user step value
        while (StepPulseCounter > 0) {
          // send single pulse with enough width to trigger the motor driver
          digitalWrite(MotorStepPin, HIGH);
          delayMicroseconds(PWM_Width);
          digitalWrite(MotorStepPin, LOW);
          // delay to give low feedrate (% of main feedrate)
          delay(MotorTimeDelayMicro * DrillControl_StepSpeed);
          // check if SELECT pressed.  If so, pause steps until released.
          ReadKeyPad = analogRead(KeypadPin);
          while ((ReadKeyPad > 717) & (ReadKeyPad < 757)) {
            ReadKeyPad = analogRead(KeypadPin);
          }
          // decrease pulse counter
          StepPulseCounter = StepPulseCounter - 1;

          // Check if upper limit it, if so, exit loop
          ReadUpperLimit = digitalRead(UpperLimitPin);
          if (ReadUpperLimit == LOW) {
            // exit loop
            StepPulseCounter = 0;
          }
          // check if E-STOP pressed
          ReadEstop = digitalRead(eStopPin);
          if (ReadEstop == HIGH) {
            // exit loop
            StepPulseCounter = 0;
          }
        }
      }

      // check if down button pressed (and not hit lower limit)
      if (ReadPressedDown == LOW && ReadLowerLimit == HIGH) {
        // release stepper brake
        digitalWrite(BrakeEnablePin, LOW);
        // set direction
        digitalWrite(MotorDirectionPin, HIGH);
        // calculate how many pulses needed
        StepPulseCounter = MotorPulsesPerMillimeter * DrillControl_StepDepth;
        // loop through sending exact number of pulses depending on user step value
        while (StepPulseCounter > 0) {
          // send single pulse with enough width to trigger the motor driver
          digitalWrite(MotorStepPin, HIGH);
          delayMicroseconds(PWM_Width);
          digitalWrite(MotorStepPin, LOW);
          // delay to give low feedrate (% of main feedrate)
          delay(MotorTimeDelayMicro * DrillControl_StepSpeed);
          // check if SELECT pressed.  If so, pause steps until released.
          ReadKeyPad = analogRead(KeypadPin);
          while ((ReadKeyPad > 717) & (ReadKeyPad < 757)) {
            ReadKeyPad = analogRead(KeypadPin);
          }
          // decrease pulse counter
          StepPulseCounter = StepPulseCounter - 1;

          // Check if lower limit it, if so, exit loop
          ReadLowerLimit = digitalRead(LowerLimitPin);
          if (ReadLowerLimit == LOW) {
            // exit loop
            StepPulseCounter = 0;
          }
          // check if E-STOP pressed
          ReadEstop = digitalRead(eStopPin);
          if (ReadEstop == HIGH) {
            // exit loop
            StepPulseCounter = 0;
          }
        }
      }
    }
    // ****************************************************
    // if DRILL CANNED CYCLE MODE 2
    // ****************************************************
    if (DrillMode == 2) {
      // display function
      lcd.setCursor(0, 1);
      lcd.print("DRILL");
      // display parameters for canned cycle drilling (total depth and peck depth)
      lcd.setCursor(6, 1);
      lcd.print(DrillControl_Cycledepth);
      lcd.setCursor(10, 1);
      lcd.print("mm    ");
    }

    // ****************************************************
    // check keypad value and adjust parameters accordingly
    // ****************************************************
    // no keys = 1023, select = 737, up = 144, down = 328, left = 0, right = 503
    //
    // button function depends on drilling mode
    // STEP: up = +0.1mm step value, down = -0.1mm step value (min = 0.1, max = 5)
    // CONT: no function
    // DRILL CYCLE: up = +0.25mm total depth, down = -0.25mm total depth (min 0.25, max 20)   [speed = 10% of speed pot, peck depth = 10% total]

    if (ReadKeyPad > 1000) {  // keypad NO KEYS
    }
    if ((ReadKeyPad > 717) & (ReadKeyPad < 757)) {  // keypad SELECT
    }
    if ((ReadKeyPad > 483) & (ReadKeyPad < 523)) {  // keypad RIGHT
    }
    if (ReadKeyPad < 20) {  // keypad LEFT
    }

    //// keypad DOWN
    if ((ReadKeyPad > 308) & (ReadKeyPad < 348)) {
      // check drill mode
      if (DrillMode == 1) {
        // step
        if (DrillControl_StepDepth > 0.1) {  // max step cannot be negative
          DrillControl_StepDepth = DrillControl_StepDepth - 0.1;
        }
      }
      if (DrillMode == 2) {
        // drill canned cycle
        if (DrillControl_Cycledepth > 0.25) {  // max step cannot be negative
          DrillControl_Cycledepth = DrillControl_Cycledepth - 0.25;
        }
      }
    }

    //// keypad UP
    if ((ReadKeyPad > 124) & (ReadKeyPad < 164)) {
      // check drill mode
      if (DrillMode == 1) {
        // step
        if (DrillControl_StepDepth < 20) {  // max step = 20mm
          DrillControl_StepDepth = DrillControl_StepDepth + 0.1;
        }
      }
      if (DrillMode == 2) {
        // drill canned cycle
        if (DrillControl_Cycledepth < 20) {  // max drill cycle step = 20mm
          DrillControl_Cycledepth = DrillControl_Cycledepth + 0.25;
        }
      }
    }

    // check for limits hit and display message
    if (ReadLowerLimit == LOW) {
      lcd.setCursor(0, 0);
      lcd.print("*LOWER LIMIT*");
    }
    if (ReadUpperLimit == LOW) {
      lcd.setCursor(0, 0);
      lcd.print("*UPPER LIMIT*");
    }
    if (ReadLowerLimit == HIGH && ReadUpperLimit == HIGH && ReadPressedUp == HIGH && ReadPressedDown == HIGH && ReadEstop == LOW) {
      // no other user signals are present and all OK to run so display "OK" status
      lcd.setCursor(0, 0);
      lcd.print("OK           ");
    }
    // *************************************************************************

    delay(50);
  }
}
