/*
 Arduino code for a CNC X-Y table actuated by 2 steppers.
 Author: Augustin Merle
 Version 1.0 - 12/11/2022
 WOKWI link: https://wokwi.com/projects/347681237875294803
*/

// Libraries
#include<Stepper.h> // Add the library to control steppers
#include<math.h>    // Add the library to use math functions

// Parameters
#define RESOLUTION 0.001  // Resolution of stepper motors
#define LINE 1            // Associate the number 1 with linear movement
#define ARC_CLW 2         // Associate the number 2 with clockwise circular movement
#define ARC_CCLW 3        // Associate the number 3 with counter-clockwise circular movement
#define RAPID 0           // Associate the number 0 with rapid movement
int movemode;             // To register the type of movement

const int stepsPerRevolution = 200;               // Number of encoder step per revolution
Stepper X_Axis(stepsPerRevolution, 2, 3, 4, 5);   // Create an instance for the X Stepper
Stepper Y_Axis(stepsPerRevolution, 6, 7, 8, 9);  // Create an instance for the Y Stepper

float posX;       // Absolute position of the system on the X axis
float posY;       // Absolute position of the system on the Y axis
float gCodeX;     // G-code's input of relative distance to move in the X direction
float gCodeY;     // G-code's relative distance to move in the Y direction
int xsteps;       // Number of steps to be taken by the X-axis stepper
int ysteps;       // Number of steps to be taken by the Y-axis stepper
int speed;        // Stepper's speed value
float ival;       // X coordinate value of arc centres
float jval;       // Y coordinate value of arc centres


// Setup programm
void setup()
{
  Serial.begin(9600);                   // Set the baud rate
  X_Axis.setSpeed(30);                  // Set speed for the X-axis stepper to 30%
  Y_Axis.setSpeed(30);                  // Set speed for the X-axis stepper to 30%
  posX = 0;                             // Reset the actual position of the system on the X axis
  posY = 0;                             // Reset the actual position of the system on the Y axis
  Serial.println("Enter G Code line");  // Ask for keyboard input from User
}

// Main program
void loop()
{
  if (Serial.available()) {   // If something has been entered
    char ch = Serial.read();  // Read the input character

    switch (ch) {   // Analyse the input character
      case'G':                        // If a "G" is read
      case'g':                        // If a "g" is read
        movemode = Serial.parseInt(); // Convert the numbers following in an integer and store them as a movement type input
        break;                        // Exit the switch case

      case 'X':                       // If a "X" is read
      case 'x':                       // If a "x" is read
        gCodeX = Serial.parseFloat(); // Convert the numbers following in a float and store them as an X input value
        break;                        // Exit the switch case

      case 'Y':                       // If a "Y" is read
      case 'y':                       // If a "y" is read
        gCodeY = Serial.parseFloat(); // Convert the numbers following in a float and store them as an Y input value
        break;                        // Exit the switch case

      case 'F':                       // If a "F" is read
      case 'f':                       // If a "f" is read
        speed = Serial.parseInt();    // Convert the numbers following in an integer and store them as a speed value
        break;                        // Exit the switch case

      case 'I':                       // If a "I" is read
      case 'i':                       // If a "i" is read
        ival = Serial.parseFloat();   // Convert the numbers following in a float and store them as an X circle center input value
        break;                        // Exit the switch case

      case 'J':                       // If a "J" is read
      case 'j':                       // If a "j" is read
        jval = Serial.parseFloat();   // Convert the numbers following in a float and store them as an Y circle center input value
        break;                        // Exit the switch case

      case  '#':                  // If a hash sign is read
        // Print the line read
        Serial.print("G");
        Serial.print(movemode);   // Print the move ID read
        Serial.print("\t");       // Print a tabulation
        Serial.print("X");
        Serial.print(gCodeX);     // Print the X movement value read
        Serial.print("\t");       // Print a tabulation
        Serial.print("Y");
        Serial.print(gCodeY);     // Print the Y movement value read
        Serial.print("\t");       // Print a tabulation
        if(movemode == ARC_CCLW || movemode == ARC_CLW) {  // Print the GCode related to curves only if the movement is a curve
          Serial.print("I");
          Serial.print(ival);     // Print the I position value read
          Serial.print("\t");     // Print a tabulation
          Serial.print("J");
          Serial.print(jval);     // Print the J position value read
          Serial.print("\t");     // Print a tabulation
        }
        Serial.print("F");
        Serial.println(speed);    // Print the speed value read
        
        // Start the program corresponding to the G value
        if(movemode == LINE) {                              // If the mode value equal the LINE value
          linemove(gCodeX, gCodeY, speed);                  // Run the linemove function with the specified parameters
        }
        if(movemode == ARC_CCLW) {                          // If the mode value equal the ARC_CCLW value
          arcmove_CCLW(gCodeX, gCodeY, ival, jval, speed);  // Run the arcmove_CCLW function with the specified parameters
        }
        if(movemode == RAPID) {                             // If the mode value equal the RAPID value
          rapidmove(gCodeX, gCodeY);                        // Run the rapidmove function with the specified parameters
        }
        break;                                              // Exit the switch case

      default:                              // If the character is not searched by the previous use cases
        if (ch != ' ' && ch != '\n') {      // If the character is other than a space or the return key
          Serial.print(ch);                 // Print the non-valid character
          Serial.print("\t");               // Print a tabulation
          Serial.println("Not recognised"); // Print "Not recognised"
        }
        break;    // Exit the switch case
    }
  }
}

// Linear movement program
void linemove(float x, float y, int f) 
{
  xsteps = int(x*100);    // Convert the distance needing to be covered into a number of steps
  ysteps = int(y*100);    // Convert the distance needing to be covered into a number of steps
  int xcount = 0;         // Initialize the step counter
  int ycount = 0;         // Initialize the step counter

  X_Axis.setSpeed(f);     // Set the speed for the X axis stepper
  Y_Axis.setSpeed(f);     // Set the speed for the Y axis stepper
  
  // Print the action
  Serial.print("Line to X:");
  Serial.print(x);            // Print the distance needing to be covered in the X axis
  Serial.print(",");          // Add a comma to separate values
  Serial.print("\t");         // Add a tabulation to visualy separate values
  Serial.print("Y:");
  Serial.print(y);            // Print the distance needing to be covered in the Y axis
  Serial.print(",");          // Add a comma to separate values
  Serial.print("\t");         // Add a tabulation to visualy separate values
  Serial.print("Speed: ");
  Serial.println(f);          // Print the speed requested

  // Loop to create line at multiples of 0° or 45° angle
  while (xcount != xsteps || ycount != ysteps) // Keep the loop running as long as any step number is not reached
  {
    if (xcount < xsteps) {  // If the stepper need to advance in the X positive direction
      X_Axis.step(1);       // Go one step in the X positive direction
      xcount++;             // increment the step counter in the X axis
      posX += 0.01;         // Increment the absolute X position
    }
    
    if (xcount > xsteps) {  // If the stepper need to advance in the X negative direction
      X_Axis.step(-1);      // Go one step in the X negative direction
      xcount--;             // Decrement the step counter in the X axis
      posX -= 0.01;         // Decrement the absolute X position
    }

    if (ycount < ysteps) {  // If the stepper need to advance in the Y positive direction
      Y_Axis.step(1);       // Go one step in the Y positive direction
      ycount++;             // increment the step counter in the Y axis
      posY += 0.01;         // Increment the absolute Y position
    }

    if (ycount > ysteps) {  // If the stepper need to advance in the Y negative direction
      Y_Axis.step(-1);      // Go one step in the Y negative direction
      ycount--;             // Decrement the step counter in the Y axis
      posY -= 0.01;         // Decrement the absolute Y position
    }

    // Display the absolute position
    Serial.print("X:");
    Serial.print(posX);     // Display the absolute X position
    Serial.print(",");      // Add a comma to separate values
    Serial.print("\t");     // Add a tabulation to visualy separate values
    Serial.print("Y:");
    Serial.println(posY);   // Display the absolute Y position
  }

  // Reset all G-Code dimension inputs after each call of the program
  gCodeX = 0; // Reset the X input
  gCodeY = 0; // Reset the Y input
}


// The circular clockwise movement program doesn't exist as
// you told us in the "Skeleton Code for Assignment 2" video
// that we didn't had to create it.


// Circular counter-clockwise movement program
void arcmove_CCLW(float x, float y, float i, float j, int f)
{
  float deltax;         // Create the variable holding the value of the horizontal gap between the center and starting point
  float deltay;         // Create the variable holding the value of the vertical gap between the center and starting point
  float arcPosX = 0;    // Create and initialize the variable holding the value of the X position in the arc
  float arcPosY = 0;    // Create and initialize the variable holding the value of the Y position in the arc
  float IncrX = 0;      // Create and initialize the variable holding the value of the increment in the X axis
  float IncrY = 0;      // Create and initialize the variable holding the value of the increment in the X axis
  float radius;         // Create the variable holding the value of the radius of the arc

  X_Axis.setSpeed(f);   // Set the speed for the X axis stepper
  Y_Axis.setSpeed(f);   // Set the speed for the Y axis stepper

  deltax = abs(x-i);    // Calculate the value of the horizontal distance to the center
  deltay = abs(y-j);    // Calculate the value of the vertical distance to the center
  radius = sqrt((deltax*deltax)+(deltay*deltay)); // Apply pythagoras theorem to calculate the arc radius
  float tangent_slope = atan(y/x);                // Calculate the tangent slope
  float included_angle = tangent_slope*2;         // Calculate the arc angle needed

  // Loop to create an arc in the 4th quadrant
  for (float inc = 0; inc < included_angle; inc += RESOLUTION) {  // As long as the condition is true, run the loop and increase 
    IncrX = radius*sin(inc)-arcPosX;          // Increments needed for the X stepped
    IncrY = (j-(radius*cos(inc)))-arcPosY;    // Increments needed for the Y stepped
    arcPosX = radius*sin(inc);                // Register the current X position
    arcPosY = j-(radius*cos(inc));            // Register the current Y position
    X_Axis.step(round(IncrX*100));            // Order the X-axis stepper to move a number of steps
    Y_Axis.step(round(IncrY*100));            // Order the Y-axis stepper to move a number of steps
    posX += IncrX;                            // Add the increments on the X axis to the absolute X position
    posY += IncrY;                            // Add the increments on the Y axis to the absolute Y position
    
    // Display the absolute position
    Serial.print("X:");
    Serial.print(posX);     // Display the absolute X position
    Serial.print(",");      // Add a comma to separate values
    Serial.print("\t");     // Add a tabulation to visualy separate values
    Serial.print("Y:");
    Serial.println(posY);   // Display the absolute Y position
  }

  // Loop to create an arc in the 1st quadrant
  for (float inc = 0; inc > included_angle; inc -= RESOLUTION) {  // For each 0.001° do
    IncrX = ((radius*cos(-inc))+i)-arcPosX;   // Increments needed for the X stepped
    IncrY = radius*sin(-inc)-arcPosY;         // Increments needed for the Y stepped
    arcPosX = (radius*cos(-inc)+i);           // Register the current X position
    arcPosY = radius*sin(-inc);               // Register the current Y position
    X_Axis.step(round(IncrX*100));            // Order the X-axis stepper to move a number of steps
    Y_Axis.step(round(IncrY*100));            // Order the Y-axis stepper to move a number of steps
    posX += IncrX;                            // Add the increments on the X axis to the absolute X position
    posY += IncrY;                            // Add the increments on the Y axis to the absolute Y position

    // Display the absolute position
    Serial.print("X:");
    Serial.print(posX);     // Display the absolute X position
    Serial.print(",");      // Add a comma to separate values
    Serial.print("\t");     // Add a tabulation to visualy separate values
    Serial.print("Y:");
    Serial.println(posY);   // Display the absolute Y position
  }

  // Reset all dimension inputs after each call of the program
  gCodeX = 0; // Reset the X input
  gCodeY = 0; // Reset the Y input
  ival = 0;   // Reset the I input
  jval = 0;   // Reset the J input
}


// Rapid movement program
void rapidmove(float x, float y) 
{
  xsteps = int(x*100);    // Convert the distance into a number of steps
  ysteps = int(y*100);    // Convert the distance into a number of steps
  int xcount = 0;         // reset the step counter
  int ycount = 0;         // reset the step counter

  X_Axis.setSpeed(100);   // Set the speed for the X axis stepper
  Y_Axis.setSpeed(100);   // Set the speed for the Y axis stepper

  // Print the action
  Serial.print("Rapid mvt to X:");
  Serial.print(x);            // Print the distance needing to be covered in the X axis
  Serial.print(",");          // Add a comma to separate values
  Serial.print("\t");         // Add a tabulation to visualy separate values
  Serial.print("Y:");
  Serial.print(y);            // Print the distance needing to be covered in the Y axis

  // Loop to create rapid movement
  while (xcount != xsteps || ycount != ysteps) // Keep the loop running as long as the step number is not reached
  {
    if (xcount < xsteps) {  // If the stepper need to advance in the X positive direction
      X_Axis.step(1);       // Go one step in the X positive direction
      xcount++;             // Increment the step counter in the X axis
      posX += 0.01;         // Increment the absolute position
    }
    
    if (xcount > xsteps) {  // If the stepper need to advance in the X negative direction
      X_Axis.step(-1);      // Go one step in the X negative direction
      xcount--;             // Decrement the step counter in the X axis
      posX -= 0.01;         // Decrement the absolute position
    }

    if (ycount < ysteps) {  // If the stepper need to advance in the Y positive direction
      Y_Axis.step(1);       // Go one step in the Y positive direction
      ycount++;             // increment the step counter in the Y axis
      posY += 0.01;         // Increment the absolute position
    }

    if (ycount > ysteps) {  // If the stepper need to advance in the Y negative direction
      Y_Axis.step(-1);      // Go one step in the Y negative direction
      ycount--;             // Decrement the step counter in the Y axis
      posY -= 0.01;         // Decrement the absolute position
    }

    // Display the absolute position
    Serial.print("X:");
    Serial.print(posX);     // Display the absolute X position
    Serial.print(",");      // Add a comma to separate values
    Serial.print("\t");     // Add a tabulation to visualy separate values
    Serial.print("Y:");
    Serial.println(posY);   // Display the absolute Y position
  }

  // Reset all dimension inputs after each call of the program
  gCodeX = 0; // Reset the X input
  gCodeY = 0; // Reset the Y input
}