Hello everyone. I am new to Arduino as well as this forum. I will go straight to the point with what I am attempting to do as a project.
What I am trying to create is a machine that can draw a figure. No need for any type of figure, because all I’m trying to do is to do it built in. I am attempting to write a code using two servo motors (TowerPro SG-5010 Double Ball Bearing to be exact) and I want to to draw a square. I went ahead and wrote the code, and after a few troubleshooting moments, the code was successful and the command was being sent successfully to the servos. However, there is a huge problem with accuracy. It’s like the servos are trying to draw the square, but it’s all going wrong. I mean, it is barely drawing a straight line. At first, I tried to give it input signals from 5 KOhm pot resistors. That part was successful. The problem began when I removed them, and I manually put values in the “map” function after doing some calculations on how to draw the square. I put a 1000 uF capacitor to keep the servos as stable as possible (they were going berserk when I tried before this step). It is just not drawing what it should. The output is a highly inaccurate drawing :(. I have written the code, and here it is.
#include <Servo.h>
Servo mys1;
Servo mys2;
int poten1 = 0;
int poten2 = 1;
int v;
void setup()
{
mys1.attach(5);//Servo motor 2
mys2.attach(6);//Servo motor 1
}
void loop()
{
square();
}
void square()
{
//Step 1
double ma = 180.0;
int i = 21600;
int j = 11700;
v = map(i, 0.0, 32400.0, 0.0, ma);
mys2.write(v);
v = map(j, 0.0 ,32400.0, 0.0, ma);
mys1.write(v);
delay(2000);
//step1
while(i >= 16200)
{
i = i - 180;
v = map(i, 0.0, 32400.0, 0.0, ma);
mys2.write(v);
j = j + 270;
v = map(j, 0.0 ,32400.0, 0.0, ma);
mys1.write(v);
delay(100);
}
delay(1000);
//step2
while(j <= 28080)
{
i = i - 80;
v = map(i, 0.0, 32400.0, 0.0, ma);
mys2.write(v);
j = j + 216;
v = map(j, 0.0 ,32400.0, 0.0, ma);
mys1.write(v);
delay(100);
}
delay(1000);
//step3
while(i <= 16200)
{
i = i + 180;
v = map(i, 0.0, 32400.0, 0.0, ma);
mys2.write(v);
j = j - 150;
v = map(j, 0.0 ,32400.0, 0.0, ma);
mys1.write(v);
delay(100);
}
delay(1000);
//step4
while(i <= 21600)
{
i = i + 90;
v = map(i, 0.0, 32400.0, 0.0, ma);
mys2.write(v);
j = j - 216;
v = map(j, 0.0 ,32400.0, 0.0, ma);
mys1.write(v);
delay(100);
}
delay(1000);
}
Please note that the calculations are not exact due to what I believe are from the servos. It’s like they are rotating a bit more than just 180 degrees, but the Arduino still considers them as servos with only 180 degree rotation. I tried changing the max value (I named that variable “ma” in the code) and it actually made things worse. Is there any code that would allow me to draw a perfect straight line horizontally and vertically? Or perhaps there is a problem with my reasoning with the mathematical computations?
To make things clearer, here is how I thought of drawing the square (assuming both servos were 100% accurate and would turn to a maximum of 180 degrees):
But first, here is how I have the servos connected to each other:
SERVO1--------------->SERVO2------------------>End-effector(pen)
Distances between servos are even. Consider them L1 and L2.
I drew the figure on a paper, and took the lengths of each joint I plugged on the servos. Then I thought of creating a translation by manipulating two rotations (which are the servos). So, I took an initial angle. Servo 1 = 120 degrees (counter clockwise rotation) and servo 2 = 90 degrees.
The way I thought of drawing the line is that for each degree servo 1 goes clockwise or counterclockwise, provided it is greater than 90 degrees, servo 2 should rotate in the opposite direction. Which is why in my code you’ll see the numbers from 0 to 32400. 32400 is actually 180 multiplied by 180. This way, I can have accuracy on every single angle, instead of possible imbalances I get from other examples I saw (v, 0, 1023, …). And for every angle made, there is a short pause of 100 milliseconds so that the Arduino catches up with the values, and the servos make it in time to their destination before acquiring new data from the while loop I put them in. (It was for loop before, but that just gave me trouble for some reason). But, unfortunately, it was not a straight line. It was more of a small curve… Like it’s trying to fix it, but something wrong is happening and it’s just continuing the trajectory its getting. So, if there is a code, or at least any explanation on how to fix this problem with my code, I’d really appreciate it. Sorry for the long read, and thank you all in advance 
