// 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);
}
mega:SCL
mega:SDA
mega:AREF
mega:GND.1
mega:13
mega:12
mega:11
mega:10
mega:9
mega:8
mega:7
mega:6
mega:5
mega:4
mega:3
mega:2
mega:1
mega:0
mega:14
mega:15
mega:16
mega:17
mega:18
mega:19
mega:20
mega:21
mega:5V.1
mega:5V.2
mega:22
mega:23
mega:24
mega:25
mega:26
mega:27
mega:28
mega:29
mega:30
mega:31
mega:32
mega:33
mega:34
mega:35
mega:36
mega:37
mega:38
mega:39
mega:40
mega:41
mega:42
mega:43
mega:44
mega:45
mega:46
mega:47
mega:48
mega:49
mega:50
mega:51
mega:52
mega:53
mega:GND.4
mega:GND.5
mega:IOREF
mega:RESET
mega:3.3V
mega:5V
mega:GND.2
mega:GND.3
mega:VIN
mega:A0
mega:A1
mega:A2
mega:A3
mega:A4
mega:A5
mega:A6
mega:A7
mega:A8
mega:A9
mega:A10
mega:A11
mega:A12
mega:A13
mega:A14
mega:A15
lcd1:GND
lcd1:VCC
lcd1:SDA
lcd1:SCL
A4988
drv1:ENABLE
drv1:MS1
drv1:MS2
drv1:MS3
drv1:RESET
drv1:SLEEP
drv1:STEP
drv1:DIR
drv1:GND.1
drv1:VDD
drv1:1B
drv1:1A
drv1:2A
drv1:2B
drv1:GND.2
drv1:VMOT
A4988
drv2:ENABLE
drv2:MS1
drv2:MS2
drv2:MS3
drv2:RESET
drv2:SLEEP
drv2:STEP
drv2:DIR
drv2:GND.1
drv2:VDD
drv2:1B
drv2:1A
drv2:2A
drv2:2B
drv2:GND.2
drv2:VMOT
A4988
drv3:ENABLE
drv3:MS1
drv3:MS2
drv3:MS3
drv3:RESET
drv3:SLEEP
drv3:STEP
drv3:DIR
drv3:GND.1
drv3:VDD
drv3:1B
drv3:1A
drv3:2A
drv3:2B
drv3:GND.2
drv3:VMOT
stepper1:A-
stepper1:A+
stepper1:B+
stepper1:B-
stepper2:A-
stepper2:A+
stepper2:B+
stepper2:B-
stepper3:A-
stepper3:A+
stepper3:B+
stepper3:B-
keypad1:R1
keypad1:R2
keypad1:R3
keypad1:R4
keypad1:C1
keypad1:C2
keypad1:C3
keypad1:C4
led1:A
led1:C
led2:A
led2:C
led3:A
led3:C
led4:A
led4:C
r1:1
r1:2
r2:1
r2:2
r3:1
r3:2
r4:1
r4:2