// For testing CNC milling spline //
// For LCD I2C //
#include <LiquidCrystal_I2C.h>   
#include  <Wire.h>
LiquidCrystal_I2C lcd(0x27, 16, 4);
// For Stepper Controller //
#include <Stepper.h>
#include <AccelStepper.h>
/* Stepper 1/X 
steps per rev 400 (full step setting), 1:195 Cycloid,
50 teeth drive gear, tooth pitch 10 mm,
1 rotation of stepper @400 steps, and 1/195*50*10 mm
mmstepsX= 400*195/50/10 = 156 */
const float mmstepsX = 156;
/* Stepper 2/Z 
steps per rev 400 (full step setting), 5 mm per rev ball screw
mmstepsZ=400/5 = 80*/
const float mmstepsZ = 80;
/* Stepper 3/A 
steps per rev 400 (full step setting), gear ratio 60
DegstepsA=400*60/360 = 66.66667*/
const float degstepsA = 400*60/360;
  // For stepper // not in use //
  // Stepper myStepperX(mmstepsX, 22, 23);
  // Stepper myStepperZ(mmstepsZ, 24, 25);
  // Stepper myStepperA(degstepsA, 26, 27);
// Define stepper driver pins//
AccelStepper mystepperX(AccelStepper::DRIVER, 22, 23);
AccelStepper mystepperZ(AccelStepper::DRIVER, 24, 25);
AccelStepper mystepperA(AccelStepper::DRIVER, 26, 27);
float mmX; // Actual position of X //
float mmZ; // Actual position of Z //
float mmXmv; // Max move mm X //
float mmZmv; // Max move mm Z //
float degA ; // Actual position of A //
int degAmv; // Max move deg A //
long stX; // Steps Number X //
long stZ; // Steps Number Z //
long stA; // Steps Number A //
float sts=100; // Step to move in each loop //


void setup() 
{
  //initialize lcd screen //
  lcd.begin(16,4);
  // turn on the backlight //
  lcd.backlight();
    // Stepper library // Not in use
    // Stepper speed in mm/Degree per min//
      //myStepperX.setSpeed(120);
      //myStepperZ.setSpeed(120);
      //myStepperA.setSpeed(720);
  mystepperX.setMaxSpeed(1000);
  mystepperZ.setMaxSpeed(1000);
  mystepperA.setMaxSpeed(1000);
  mystepperX.setAcceleration(500);
  mystepperZ.setAcceleration(500);
  mystepperA.setAcceleration(500);

// For Max movement //
  mmXmv = 6.2;
  mmZmv= 300.4;
  degAmv = 36;
}
void loop() {
  // Not in use //
  /* myStepperX.step(20);
  myStepperZ.step(10);
  myStepperA.step(5); */
    if (mmXmv > mmX){
    stX=stX+sts;
    mmX = mmX + sts/mmstepsX;
    }
  if (mmZ < mmZmv){
    stZ=stZ+sts;
    mmZ = mmZ + sts/mmstepsZ;
    }
  if (degA < degAmv){
    stA=stA+sts;
    degA=degA+sts/degstepsA;
    } 
  mystepperX.moveTo(stX);
  mystepperX.runToPosition();
  mystepperZ.moveTo(stZ);
  mystepperZ.runToPosition();
  mystepperA.moveTo(stA);
  mystepperA.runToPosition();
  // For X axis distance //
  lcd.setCursor(0, 0);
  lcd.print("X: ");
  lcd.setCursor(3, 0);
  lcd.print(mmX);
  // For Z axis distance //
  lcd.setCursor(0, 1);
  lcd.print("Z: ");
  lcd.setCursor(3, 1);
  lcd.print(mmZ);
    // For Z axis distance //
  lcd.setCursor(0, 2);
  lcd.print("A: ");
  lcd.setCursor(7, 2);
  lcd.print("Deg");
  lcd.setCursor(3, 2);
  lcd.print(degA,0);
}