/*-----------------------------------------------------------------------------------------------------------------------------------------------
Assumption to take into account for the running of this code:
When the setup loop runs, the steppers do a 180 degree rotation. The point they move to is the point X10 Y0 and this is used as the start point for any move made.
Because of this, the code runs best if after each path is finished, it is reset. This will reset the stepper motors to their starting position
It is als set that there are 36000 steps per revolution for each stepper. This was to aid the resolution of the stepper motors when they are rotating.
Each degree is 100 steps of the stepper motor
To input a point for the end effector to move to the format P# X# Y# P followed by 1 or 2 ( distinguishing which path to take)
X followed by a positive number for the X position and Y followed by a positive number for the Y position
An example is
P1 X15 Y5
*Reset Button
P2 X15 Y5
-------------------------------------------------------------------------------------------------------------------------------------------------*/
#include<Stepper.h> // include the stepper library
#include<math.h> // include to the math library
#define path0 00
#define path1 01 // definition of path 1 movement call
#define path2 02 // definition of path 2 movement call
#define home 03
int L1 = 10; // L1 defines the length of the Link Arm joined to Joint 1. This length can be adjusted.
int L2 = 10; // L2 defines the length of the Link arm connected to Joint 2, at hte end of Link Arm 1. The ned effector point (destination point) is the end of this link arm.
int J1_step, J2_step; // J1 & J2_step are calculated number of steps for stepper 1 & 2 to make on each iteration of the for loops in each path respectively.
int path; // path defines the path inputted into the serial monitor by the user in which the linkages move.
float xa, ya; // xa & ya are the positions that the end effector must move to on each iteration of the loop on the x nd y axes respectively to move in the direction of the destination point
float xb, yb; // xa & ya are the positions that the end effector must moved from on each previous iteration of the loop on the x nd y axes respectively
float x0 = 10; // x0 defines the start point of the linkage on the x axis
float y0 = 0; // y0 defines the start point of the linkage on the y axis
float xdes, ydes; // xdes & ydes are the inputted points by the user for the x & y positions respectively that they want the arm to move to
float alpha1, alpha2; // alpha1 is the angle created between the x axis and link arm 1, alpha2 is the angle made between link arm 1 & 2
float thetha, thetha_1; // thetha is the angle created between a line from the centre of Joint 1 to the destination point and link 1
float beta; // beta is the angle created between the x axis and a line from the centre of Joint 1 to the destination point. Thetha and beta are equal to alpha 1
float beta_1; // beta 1 the beta angle of the previous iteration of hte counter loop. Used to find the change in angle between iterations
float alpha1_deg, alpha2_deg; // alpha1&2_deg define the function to convert the alpha angles from radians to degrees
float diff_alp1, diff_alp2; // diff_alp is the different or change in alpha 1 & 2 between each iteration of the loop when it is run. this is converted to steps to move the joint the correct no. of steps each iteration
float alpha1_1, alpha2_1; // alpha1&2_1 is the alpha1 of the iteraton of the loop previous to the current pos
float alpha1_1_deg, alpha2_1_deg; // alpha1&2_1_deg is the alpha1_deg of the iteraton of the loop previous to the current pos
const int stepsPerRevolution = 36000; // definion of 36000 steps in one stepper revolution Stepper gear ratio set to 180:1 giving 100 steps per degree
Stepper J1(stepsPerRevolution, 2, 3, 4, 5); // Joint 1 Stepper hardware connection to Digital Pins
Stepper J2(stepsPerRevolution, 6, 7, 8, 9); // Joint 2 Stepper hardware connection to Digital Pins
/*----------------------------------------------- SETUP FUNCTION -------------------------------------------------
The set up function runs for a single iteration as soon as the program runs. In it the serial monitor is turned on and set
to 9600 baud. The start command is written in the serial monitor and the Stepper motor for joint 1 is rotated 180 degrees to
its start point
------------------------------------------------------------------------------------------------------------------*/
void setup()
{
Serial.begin(9600); // Sets the data rate in bits per second (baud) for serial data transmission to 9600
Serial.println("MOVING TO START POSITION (X10 Y0)"); // Prints the initial message for the user to the serial monitor
J1.step(18000); // moves the stepper motor for joint 1 to its start position
J2.step(18000); // moves the stepper motor for joint 1 to its start position
Serial.println("INPUT A PATH TYPE AND DESTINATION POINT"); // Prints the message to ask the user to type a path type and the desired destination point of the link arms
}
/*----------------------------------------------- LOOP FUNCTION --------------------------------------------------
The loop function runs continuously once the setup loop is completed. In this, the operator can type in the destination position
of the the end effector in the format of P# X# Y#. P is the designated path (1 or 2) and X and Y are the co-ordinates of the end point.
Path 1 runs in a single straight line from the start point to the destination point.
Path 2 runs the end effector from the start point parallel to the X Axis and then Parallel to the Y axis to the destination point
------------------------------------------------------------------------------------------------------------------*/
void loop()
{
if (Serial.available()) // something has been typed
{
char ch = Serial.read(); // read the input character
switch (ch) // Switch Case to define inputted character
{
case 'P': path = Serial.parseInt(); // P inputted means look for the following number to define the path of the plotter
Serial.println(" ");
Serial.print("MOVING ON PATH: "); // Serial Monitor Output of path followed by path number
Serial.println(path); // Inputted Movemode Number
Serial.println(" ");
break; // End of Case P
case 'X':
case 'x': xdes = Serial.parseFloat(); // if it is X then expect a float to follow. The following float will be the x axis destination point
break; // End of Case X Values are stored.. Do nothing more for now.
case 'Y':
case 'y': ydes = Serial.parseFloat(); // if it is Y then expect a float to follow. The following float will be the y axis destination point
break; // End of Case Y Values are stored.. Do nothing more for now.
case '#':
case '\n': // return key has been hit. Output a move if the X OR Y value has changed since the last CR.
//if (path == path0) path_0(xdes, ydes);
if (path == path1) path_1(xdes, ydes); // call path 1 movement if the inputted path is path_1 (1)
if (path == path2) path_2(xdes, ydes); // call path 2 movement if the inputted path is path_2 (2)
//if (path == home) home_0(xdes, ydes);
break; // Values are stored.. Do nothing more for now.
default:
Serial.print(ch);
if (ch != ' ')Serial.println("Not recognised"); // A non-valid input so ignore.
break;
}
}
}
/*----------------------------------------------- FUNCTION FOR MOVEMENT ON PATH 1 -----------------------------------
Path 1 movement is called by inputting 'P1' before the X and Y values of the destination point. Path 1 moves from the start
point (x0, y0) to the destination point (xdes, ydes) with the end effector moving in a straight line between the two points.
---------------------------------------------------------------------------------------------------------------------*/
void path_1 (float xdes, float ydes)
{
int i = 0; // i is an integar used in counting for the iterations of the for loops on path 1 and 2
int res = 20; // the resolution for the path 1 movemnt is 20
int inc = 20; // the increment for the path 1 movemnt is 20
Serial.print("DESTINATION POINT IS (X, Y): "); // Print the phrase to highlight the inputted values by the user
Serial.print(xdes); // Print the inputted value for X
Serial.write(9); // Indent the gap between the two values
Serial.println(ydes); // Print the inputted value for Y and finish the line here
float xrange = (xdes - x0); //definition of the x range as the absolute value of the difference between the destination of x and the start point
float yrange = ydes - y0; //definition of the y range as the absolute value of the difference between the destination of y and the start point
for(i = 0; i <= inc; i ++) // for loop (i) starting at 0 and running for 20 iterations
{
xa = x0 + ((xrange/res) * i); // xa is the x position that the end effector must move to on each iteration of the loop
xb = x0 + ((xrange/res) * (i - 1)); // xb is the x position that the end effector has come from on that iteration on the loop
ya = y0 + ((yrange/res) * i); // ya is the y position that the end effector must move to on each iteration of the loop
yb = y0 + ((yrange/res) * (i - 1)); // yb is the y position that the end effector has come from on that iteration on the loop
alpha2 = -acos(((sq(xa) + sq(ya)) - (sq(L1) + sq(L2))) / (2*L1*L2)); // caluclation of alpha2 (angle between origin of J2 and end point)
alpha2_1 = -acos(((sq(xb) + sq(yb)) - (sq(L1) + sq(L2))) / (2*L1*L2)); // caluclation of alpha2_1 (angle between origin of J2 and end point on the previous iteration of the loop)
thetha = atan(((L2)*(sin(-alpha2)))/(L1 + (L2*cos(-alpha2)))); // calculation of thetha (angle between origin and end point)
thetha_1 = atan(((L2)*(sin(-alpha2_1)))/(L1 + (L2*cos(-alpha2_1)))); // calculation of thetha_1 (angle between origin and end point on the previous iteration of the loop)
beta = atan(ya/xa); // calculation of beta (the angle of the line from the origin to the end point with the x axis)
beta_1 = atan(yb/xb); // calculation of beta (the angle of the line from the origin to the end point with the x axis on the previous iteration of the loop)
alpha1 = thetha + beta; // caluculation of alpha1 ( the addition of thetha and beta - the angle between the x axis and Link 1)
alpha1_1 = thetha_1 + beta_1; // caluculation of alpha1 ( the addition of thetha and beta - the angle between the x axis and Link 1 on the previous iteration of the loop)
alpha1_deg = alpha1 * (180/PI); // conversion of alpha1 from radians to degrees
alpha1_1_deg = alpha1_1 * (180/PI); // conversion of alpha1 from the previous loo iteration from radians to degrees
alpha2_deg = alpha2 * (180/PI); // conversion of alpha2 from radians to degrees
alpha2_1_deg = alpha2_1 * (180/PI); // conversion of alpha2 from the previous loop iteration from radians to degrees
diff_alp1 = (alpha1_deg - alpha1_1_deg); // calculation of the difference between alpha1 across two iterations of the loop
diff_alp2 = (alpha2_deg - alpha2_1_deg); // calculation of the difference between alpha2 across two iterations of the loop
J1_step = (diff_alp1) * 100; // conversion of degrees to steps on the stepper motor for Joint 1
J2_step = (diff_alp2) * 100; // conversion of degrees to steps on the stepper motor for Joint 2
Serial.print(xa); // print the current position of the end effector on the x axis
Serial.write(9); // Indent the gap between the two values
//Serial.println(xb);
Serial.println(ya); // print the current position of the end effector on the y axis
//Serial.write(9);
//Serial.println(yb);
//Serial.print("A2: ");
//Serial.println(alpha2);
//Serial.print("T: ");
//Serial.println(thetha);
//Serial.print("B: ");
//Serial.println(beta);
//Serial.print("J1 Step: ");
//Serial.println(J1_step);
//Serial.print("J2 Steps: ");
//Serial.println(J2_step);
delay(50); // include a 50ms delay after each iteration of hte loop
J1.step(J1_step); // command to tell the J1 stepper motor to step the designated noumber of steps for that iteration
J2.step(J2_step); // command to tell the J2 stepper motor to step the designated noumber of steps for that iteration
}
}
/*----------------------------------------------- FUNCTION FOR MOVEMENT ON PATH 2 -----------------------------------
Path 2 movement is called by inputting 'P2' before the X and Y values of the destination point. Path 2 moves from the start
point (x0, y0) to the destination point (xdes, ydes) with the end effector moving firstly parallel to the X axis until it is
vertically below the destination point and then parallel to the Y Axis as far as the destination point
---------------------------------------------------------------------------------------------------------------------*/
void path_2 (float xdes, float ydes)
{
int i = 0; // i is an integar used in counting for the iterations of the for loops on path 1 and 2
int o = 0; // the integar that hte second path has to take as i is already counting
int res = 10; // the resolution for the path 1 movemnt is 10
int inc = 21; // the increment for the path 1 movemnt is 20
Serial.print("DESTINATION POINT IS (X, Y): "); // Print the phrase to highlight the inputted values by the user
Serial.print(xdes); // Print the inputted value for X
Serial.write(9); // Indent the gap between the two values
Serial.println(ydes); // Print the inputted value for Y and finish the line here
float xrange = (xdes - x0); //definition of the x range as the absolute value of the difference between the destination of x and the start point
float yrange = ydes - y0; //definition of the y range as the absolute value of the difference between the destination of y and the start point
for(i = 0; i <= inc; i ++) // for loop (i) starting at 0 and running for 20 iterations
{
if (i <= inc/2) // if statement highlighting hte actions if the counter in the 1st half of increment
{
xa = x0 + ((xrange/res) * i); // xa is the x position that the end effector must move to on each iteration of the loop
xb = x0 + ((xrange/res) * (i - 1)); // xb is the x position that the end effector has come from on that iteration on the loop
ya = y0; //for the 1st half of the path, ya remains at the origin y position
yb = y0; //for the 1st half of the path, yb also remains at the origin y position
}
else // else refers to any time the counter is outside the 1st half of the count (2nd half)
{
//for the 2nd half of the path, xa remains at the last x position
xb = xa; //for the 2nd half of the path, xb also remains at the last xa position
ya = y0 + ((yrange/res) * o); // ya is the y position that the end effector must move to on each iteration of the loop
yb = y0 + ((yrange/res) * (o - 1)); // yb is the y position that the end effector has come from on that iteration on the loop
o++; // add 1 to o on each iteratio of the loop
}
alpha2 = -acos(((sq(xa) + sq(ya)) - (sq(L1) + sq(L2))) / (2*L1*L2)); // caluclation of alpha2 (angle between origin of J2 and end point)
alpha2_1 = -acos(((sq(xb) + sq(yb)) - (sq(L1) + sq(L2))) / (2*L1*L2)); // caluclation of alpha2_1 (angle between origin of J2 and end point on the previous iteration of the loop)
thetha = atan(((L2)*(sin(-alpha2)))/(L1 + (L2*cos(-alpha2)))); // calculation of thetha (angle between origin and end point)
thetha_1 = atan(((L2)*(sin(-alpha2_1)))/(L1 + (L2*cos(-alpha2_1)))); // calculation of thetha_1 (angle between origin and end point on the previous iteration of the loop)
beta = atan(ya/xa); // calculation of beta (the angle of the line from the origin to the end point with the x axis)
beta_1 = atan(yb/xb); // calculation of beta (the angle of the line from the origin to the end point with the x axis on the previous iteration of the loop)
alpha1 = thetha + beta; // caluculation of alpha1 ( the addition of thetha and beta - the angle between the x axis and Link 1)
alpha1_1 = thetha_1 + beta_1; // caluculation of alpha1 ( the addition of thetha and beta - the angle between the x axis and Link 1 on the previous iteration of the loop)
alpha1_deg = alpha1 * (180/3.14159265358979); // conversion of alpha1 from radians to degrees
alpha1_1_deg = alpha1_1 * (180/3.14159265358979); // conversion of alpha1 from the previous loop iteration from radians to degrees
alpha2_deg = alpha2 * (180/3.14159265358979); // conversion of alpha2 from radians to degrees
alpha2_1_deg = alpha2_1 * (180/3.14159265358979); // conversion of alpha2 from the previous loop iteration from radians to degrees
diff_alp1 = (alpha1_deg - alpha1_1_deg); // calculation of the difference between alpha1 across two iterations of the loop
diff_alp2 = (alpha2_deg - alpha2_1_deg); // calculation of the difference between alpha2 across two iterations of the loop
J1_step = (diff_alp1) * 100; // conversion of degrees to steps on the stepper motor for Joint 1
J2_step = (diff_alp2) * 100; // conversion of degrees to steps on the stepper motor for Joint 2
//Serial.println(i);
Serial.print(xa); // print the current position of the end effector on the x axis
Serial.write(9); // Indent the gap between the two values
//Serial.println(xb);
Serial.println(ya); // print the current position of the end effector on the y axis
/*Serial.write(9);
Serial.println(yb);
Serial.print("A2: ");
Serial.println(alpha2);
Serial.print("T: ");
Serial.println(thetha);
Serial.print("B: ");
Serial.println(beta);
Serial.print("J1 Step: ");
Serial.println(J1_step);
Serial.print("J2 Steps: ");
Serial.println(J2_step);*/
delay(50); // include a 50ms delay after each iteration of hte loop
J1.step(J1_step); // command to tell the J1 stepper motor to step the designated noumber of steps for that iteration
J2.step(J2_step); // command to tell the J2 stepper motor to step the designated noumber of steps for that iteration
}
}
/*void home_0 (float xdes, float ydes)
{
xdes = 10;
ydes = 0;
J1.step(J1_pos);
J2.step(J2_pos);
}
void path_0 (float xdes, float ydes)
{
xdes = x0;
ydes = y0;
}*/