/*
Name: Finn Hourigan
Student ID: 17228522
Date: 11th Nov 2022
Module: DM6001 - Low Cost Automated Systems
Title: Programming Task 2 - CNC machine caple of linear and circular interpolation
Description:
Capable of transforming G-code inputs into co-ordinate outputs in the XY
plane for the following motions:
Linear (all directions, 360 degrees)
Clockwise curve (up to 90 deg)
Counterclockwise curve (up to 90 deg)
G-code guide:
G00 - Rapid move to a given point (X,Y) relative to initial position, no co-ordinates generated
G01 - Line move to a given point (X,Y) relative to initial position, co-ordinates generated
G02 - Clockwise arc to given point (X,Y) relative to initial positon, with arc centre at
point I,J. Co-ordinates of path generated
G03 - Anticlockwise arc to given point (X,Y) relative to initial positon, with arc centre at
point I,J. Co-ordinates of path generated
F is used to define the feed rate i.e speed of drawing tool
# is used to denote the end of a code block
If entering a series of codes at once, do not include # on end of last code
Example code:
G03 X5 Y5 I0 J5 F100 #
*/
#include<Stepper.h> //Stepper library required
#include<math.h> //math library required
#define RESOLUTION 0.01 //resolution constant required for arcs
#define LINE 1 //LINE defined with value of 1
#define ARC_CW 2 //ARC_CW defined with value of 2
#define ARC_CCLW 3 //ARC_CCLW defined with value of 3
#define RAPID 0 //RAPID defined with value of 0
int movemode; //variable movemode is to hold an integer value [0,1,2,3] which indicates required action
int previousmovemode = RAPID; //used to check if previous movemode was RAPID, in which case
// the start co-ordinate in a linemove must also be printed.
// initialised to RAPID
const int stepsPerRevolution = 200; //200 steps in one revolution of motor
Stepper X_Axis(stepsPerRevolution, 8,9,10,11); //Create an instance of Stepper, for X-axis motion
Stepper Y_Axis(stepsPerRevolution, 4,5,6,7); //Create an instance of Stepper, for Y-axis motion
float xpos, ypos; //Variables for storing x and y values from G-code inputs
int speed; // variable for storing speed of drawing tool (in units/sec)
const int MAX = 1000; //global constant, used for defining speed of rapid movement
float ival, jval; // arc centres
float xabs = 0; //absolute x co-ordinate, begins at origin x=0
float yabs = 0; //absolute y co-ordinate, begins at origin y=0
void setup() {
Serial.begin(9600); //Set serial port baud rate
Serial.println("Enter CNC instructions in G-code format: "); // Keyboard input from User required
}
//******Main loop START*****
void loop() {
if (Serial.available()) { // something has been typed
char ch = Serial.read(); // read the input character
switch (ch) {
case'G': previousmovemode = movemode; //Stores movemode from last block in variable 'previousmovemode'
movemode = Serial.parseInt(); //Assigns new movemode from G-code input
break;
case 'X':
case 'x': xpos = Serial.parseFloat(); // if it is X then expect a float to follow
break; // Values are stored.. Do nothing more for now.
case 'Y':
case 'y': ypos = Serial.parseFloat(); // if it is X then expect a float to follow
break; // Values are stored.. Do nothing more for now.
case 'F':
case 'f': speed = Serial.parseInt(); // get value for speed and store. Value is for the next move so do nothing else right now.
break;
case 'I':
case 'i': ival = Serial.parseFloat(); // if it is X then expect a float to follow
break; // Values are stored.. Do nothing more for now. case 'X':
case 'J': jval = Serial.parseFloat(); // if it is X then expect a float to follow
break; // Values are stored.. Do nothing more for now.
case '#': // # symbol indicates end of code block
case '\n': // return key has been hit. Output a move
if (movemode == LINE) linemove(xpos, ypos, speed); //G01 initiates linemove function
if (movemode == ARC_CW)arcmove_CW(xpos, ypos, ival, jval); //G02 initiates arcmove_CW function
if (movemode == ARC_CCLW)arcmove_CCLW(xpos, ypos, ival, jval); //G03 initiates arcmove_CCLW function
if (movemode == RAPID) rapid(xpos, ypos); //G0 initiates rapid function
xpos=0; //reset values for next code block
ypos=0;
ival=0;
jval=0;
break;
default:
Serial.print(ch);
if (ch != ' ')Serial.println("Not recognised"); // A non-valid input so ignore.
break;
}
}
}// ***** Main loop END ***********
//Movemode specified by G01, moves drawing implement in straight line to point (x,y)
//relative to initial position at the given speed
void linemove(float x, float y,int speed) /// ***START OF LINEMOVE*****
{
/*
Only prints start co-ordinates of a line if the previous movement was RAPID,
in which case it is needed as no co-ordinates are generated by RAPID
*/
if (previousmovemode == RAPID){
PrintXY(xabs,yabs); //PRINT start co-ordinates if last movemode was RAPID
previousmovemode = LINE; //set to LINE as only needs to run on first iteration (need in arcmove_CW & arcmove_CCLW)
}
xabs = xabs + x; //Updating absolute co-ordinates
yabs = yabs + y;
PrintXY(xabs,yabs); ///PRINT end co-ordinates of line
steppermove(x,y,speed); //Moves steppers to reflect linear move from (0,0) to (x,y), at the given speed
}///***** END OF LINEMOVE***************
/*
Movemode specified by G03: arcmove_CCLW
Function ARCMOVE_CCLW generates co-ordinate pairs forming a counter-clockwise curve
starting at the current position and finishing at point (x,y) relative to the current positon.
The point (i,j) relative to current position represents the curve's centrepoint
NB: arc is approximated by series of vertical and horizontal lines
*/
/////**** START OF ARCMOVE_CCLW*******
void arcmove_CCLW(float x, float y, float i, float j)
{
float deltax, deltay; //initialsing variables required for radius calculation
float radius; // radius variable initialised
deltax = abs(x - i); //absolute value of distance between x and i
deltay = abs(y - j); //absolute value of distance between y and j
radius = sqrt((deltax * deltax) + (deltay * deltay)); //pythagoras to find the radius
float tangent_slope = abs(atan(y / x)); //determining tangent slope
float included_angle = tangent_slope * 2; //determining included angle of arc
float arcdeltax; ///incremental change in X
float arcdeltay; ///incremental change in Y
if(x>0 && y>0){ ////START **** CCLW ARC in First quadrant ****
for (float inc = 0; inc < (included_angle); inc += RESOLUTION) {
arcdeltax = radius * (sin(inc+RESOLUTION)-sin(inc)) ;
arcdeltay = radius * (cos(inc)-cos(inc+RESOLUTION));
linemove(arcdeltax,0,speed); //X_axis stepper moved by increment arcdeltax at given speed
linemove(0,arcdeltay,speed); //Y_axis stepper moved by increment arcdeltay at given speed
}
} ////END **** CCLW ARC in First quadrant ****
if(x<0 && y>0){ ////START **** CCLW ARC IN 2nd quadrant ****
for (float inc = 0; inc < (included_angle); inc += RESOLUTION) {
arcdeltax = radius * (cos(inc+RESOLUTION)-cos(inc)) ;
arcdeltay = radius * (sin(inc+RESOLUTION)-sin(inc));
linemove(arcdeltax,0,speed); //X_axis stepper moved by increment arcdeltax at given speed
linemove(0,arcdeltay,speed); //Y_axis stepper moved by increment arcdeltay at given speed
}
} ////END **** CCLW ARC IN 2nd quadrant ****
if(x<0 && y<0){ ////START **** CCLW ARC in 3rd quadrant ****
for (float inc = 0; inc < (included_angle); inc += RESOLUTION) {
arcdeltax = radius * (sin(inc) - sin(inc+RESOLUTION)) ;
arcdeltay = radius * (cos(inc+RESOLUTION) - cos(inc));
linemove(arcdeltax,0,speed); //X_axis stepper moved by increment arcdeltax at given speed
linemove(0,arcdeltay,speed); //Y_axis stepper moved by increment arcdeltay at given speed
}
} ////END **** CCLW ARC in 3rd quadrant ****
if(x>0 && y<0){ ////START **** CCLW ARC in 4th quadrant ****
for (float inc = 0; inc < (included_angle); inc += RESOLUTION) {
arcdeltax = radius * (cos(inc) - cos(inc+RESOLUTION)) ;
arcdeltay = radius * (sin(inc) - sin(inc+RESOLUTION));
linemove(arcdeltax,0,speed); //X_axis stepper moved by increment arcdeltax at given speed
linemove(0,arcdeltay,speed); //Y_axis stepper moved by increment arcdeltay at given speed
}
} ////END **** CCLW ARC in 4th quadrant ****
}/// END OF arcmove_CCLW function ****************
/*
Movemode specified by G02: arcmove_CW
Function arcmove_CW generates co-ordinate pairs forming a clockwise curve
starting at the current position and finishing at point (x,y) relative to the current positon.
The point (i,j) relative to current position represents the curve's centrepoint
NB: arc is approximated by series of vertical and horizontal lines
*/
/////**** START OF arcmove_CW*********************
void arcmove_CW(float x, float y, float i, float j)
{
float deltax, deltay; //initialsing variables required for radius calculation
float radius;
deltax = abs(x - i); //absolute value of distance between x and i
deltay = abs(y - j); //absolute value of distance between y and j
radius = sqrt((deltax * deltax) + (deltay * deltay)); //pythagoras to find the radius
float tangent_slope = abs(atan(y / x));
float included_angle = tangent_slope * 2;
float arcdeltax; ///incremental change in X
float arcdeltay; ///incremental change in Y
if(x>0 && y>0){ ////START **** CW ARC in First quadrant ****
for (float inc = 0; inc < (included_angle); inc += RESOLUTION) {
arcdeltax = radius * (cos(inc)-cos(inc+RESOLUTION)); //trig identities used to determined arcdeltax
arcdeltay = radius * (sin(inc+RESOLUTION)-sin(inc)) ; //trig identities used to determined arcdeltay
linemove(arcdeltax,0,speed); //X_axis stepper moved by increment arcdeltax at given speed
linemove(0,arcdeltay,speed); //Y_axis stepper moved by increment arcdeltay at given speed
}
} ////END **** CW ARC in First quadrant ****
if(x<0 && y>0){ ////START **** CW ARC IN 2nd quadrant ****
for (float inc = 0; inc < (included_angle); inc += RESOLUTION) {
arcdeltax = radius * (sin(inc) - sin(inc+RESOLUTION)); //trig identities used to determined arcdeltax
arcdeltay = radius * (cos(inc) - cos(inc+RESOLUTION)) ; //trig identities used to determined arcdeltay
linemove(arcdeltax,0,speed); //X_axis stepper moved by increment arcdeltax at given speed
linemove(0,arcdeltay,speed); //Y_axis stepper moved by increment arcdeltay at given speed
}
} ////END **** CW ARC IN 2nd quadrant ****
if(x<0 && y<0){ ////START **** CW ARC in 3rd quadrant ****
for (float inc = 0; inc < (included_angle); inc += RESOLUTION) {
arcdeltax = radius * (cos(inc+RESOLUTION) - cos(inc)) ; //trig identities used to determined arcdeltax
arcdeltay = radius * (sin(inc) - sin(inc+RESOLUTION)); //trig identities used to determined arcdeltay
linemove(arcdeltax,0,speed); //X_axis stepper moved by increment arcdeltax at given speed
linemove(0,arcdeltay,speed); //Y_axis stepper moved by increment arcdeltay at given speed
}
} ////END **** CW ARC in 3rd quadrant ****
if(x>0 && y<0){ ////START **** CW ARC in 4th quadrant ****
for (float inc = 0; inc < (included_angle); inc += RESOLUTION) {
arcdeltax = radius * (sin(inc+RESOLUTION)-sin(inc)) ; //trig identities used to determined arcdeltax
arcdeltay = radius * (cos(inc+RESOLUTION) - cos(inc)); //trig identities used to determined arcdeltay
linemove(arcdeltax,0,speed); //X_axis stepper moved by increment arcdeltax at given speed
linemove(0,arcdeltay,speed); //Y_axis stepper moved by increment arcdeltay at given speed
}
} ////END **** CW ARC in 4th quadrant ****
}/// END OF CCLW function ****************
/*
Movemode specified by G00: rapid
rapid function is for changing the absolute co-ordinates of the CNC machine's drawing tool
to point (x,y) relative to current positon, but without ouputting any co-ordinates
*/
void rapid (float x, float y) /// *****rapid function start ********
{
xabs = xabs + x; //Updating absolute co-ordinates
yabs = yabs + y;
// No print required for rapid move
int storespeed = speed; //value of speed before rapid move is stored
int speed = MAX; ///Speed of rapid movement = MAX
steppermove(x,y,speed); //Steppers move tool to required co-ordinates
speed = storespeed; ///speed is reset to value before rapid move
} /// ******rapid code end ********
///PrintXY function used to print a pair of co-ordinates
void PrintXY (float x,float y)
{
Serial.write(9); // tab
Serial.print(x); // print x value
Serial.print(','); // print comma
Serial.write(9); // tab
Serial.println(y); // print y value
} //// PrintXY function end **********
/*
Steppermove moves X and Y stepper motors to move drawing tool to point (x,y)
relative to current position, used within linemove and rapid functions
*/
void steppermove (float x,float y, int speed)
{
int xsteps = x*100; ///Conversion of 1 unit (mm) = 100 steps
int ysteps = y*100;
int xcount=0, ycount=0; //xcount and ycount initialised = 0
int xint = x; ///Conversion to int, required for modulus operator ()%()
if (x==0) { /// Vertical lines
if(y>0){ /////Vertical line upwards
while (ycount < ysteps) { //Y_axis stepper rotates correct amount of steps
Y_Axis.setSpeed(speed); //to draw vertical line length y upwards
Y_Axis.step(1);
ycount++;
}
}
if(y<0){ /////Vertical line downwards
while (ycount > ysteps) { //Y_axis stepper rotates correct amount of steps
Y_Axis.setSpeed(speed); //to draw vertical line length y downwards
Y_Axis.step(-1);
ycount--;
}
}
}
if (y==0) { /// Horizontal lines
if(x>0){ /////Horizontal line right
while (xcount < xsteps) { //X_axis stepper rotates correct amount of steps
X_Axis.setSpeed(speed); //to draw horizontal line length x to the right
X_Axis.step(1);
xcount++;
}
}
if(x<0){ /////Horizontal line left
while (xcount > xsteps) { //X_axis stepper rotates correct amount of steps
X_Axis.setSpeed(speed); //to draw horizontal line length x to the left
X_Axis.step(-1);
xcount--;
}
}
}
///Determination of ratios of speeds for diagonal steppermove
float xspeed = (speed * abs(x)) / (sqrt(x*x + y*y)); //Pythagoras used to find x-component of speed vector
float yspeed = (speed * abs(y))/ (sqrt(x*x + y*y)); //Pythagoras used to find y-component of speed vector
X_Axis.setSpeed(xspeed); //Speed of X_axis stepper motor is set
Y_Axis.setSpeed(yspeed); //Speed of Y_axis stepper motor is set
////Diagonal lines
/////First quadrant (0 < angle < pi/2) diagonal steppermove
if (x > 0 && y > 0) {
while (xcount < xsteps && ycount < ysteps)
{ //While-loop alternatingly steps X and Y motors as required,
//giving the illusion of smooth simultaneous movement
X_Axis.step(1);
xcount=xcount+1;
if (xcount%xint == 0 && xcount!=0){ // Fulfils correct amount of steps but
Y_Axis.step(y); // prevents possibility of non-integer
ycount = ycount +y; // step input (not accepted by stepper)
}
}
}///END OF 1st quadrant loop
/////2nd Quadrant (pi/2 < angle < pi) diagonal steppermove
if (x < 0 && y > 0) {
while (xcount < abs(xsteps) && ycount < ysteps)
{ //While-loop alternatingly steps X and Y motors as required,
//giving the illusion of smooth simultaneous movement
X_Axis.step(-1);
xcount=xcount+1;
if (xcount%xint == 0 && xcount!=0){
Y_Axis.step(y);
ycount = ycount +y;
}
}
}///END OF 2nd quadrant diagonal steppermove loop
///// 3rd Quadrant diagonal steppermove
if (x < 0 && y < 0) {
while (xcount < abs(xsteps) && ycount < abs(ysteps))
{ //While-loop alternatingly steps X and Y motors as required,
//giving the illusion of smooth simultaneous movement
X_Axis.step(-1);
xcount=xcount+1;
if (xcount%xint == 0 && xcount!=0){
Y_Axis.step(y); /////NB y is already negative
ycount = ycount -y;
}
}
}///END OF 3rd quadrant diagonal steppermove loop
////4th quadrant diagonal steppermove
if (x > 0 && y < 0) {
while (xcount < abs(xsteps) && ycount < abs(ysteps))
{//While-loop alternatingly steps X and Y motors as required,
//giving the illusion of smooth simultaneous movement
X_Axis.step(1);
xcount=xcount+1;
if (xcount%xint == 0 && xcount!=0){
Y_Axis.step(y); /////NB y is already negative
ycount = ycount -y;
}
}
} ///END OF 4th quadrant diagonal steppermove loop
} ///***** END OF steppermove ******
//**************** END OF CODE ******************