private void button1_Click(object sender, EventArgs e)
{
RobotArm arm = RobotArm.lynxmotionL6();
arm.links[1] = ArmLink.rotateY(arm.links[1], Math.PI / 4);
arm.links[1] = ArmLink.rotateZ(arm.links[1], -Math.PI / 4);
arm.links[3] = ArmLink.rotateY(arm.links[3], -Math.PI / 4);
arm.links[2] = ArmLink.rotateY(arm.links[2], Math.PI / 4);
double[][] effector = arm.linkPosition(4);
double x = effector[0][0];
double y = effector[1][0];
double z = effector[2][0];
String str = Convert.ToString(x) + " , " + Convert.ToString(y) + " , " + Convert.ToString(z);
effectorPositionText.Text = str;
Graphics g = panel1.CreateGraphics();
int panelx = panel1.Width;
int panely = panel1.Height;
arm.drawArm(panelx, panely, g);
}
public static ArmLink rotateZ(ArmLink link, double angle)
{
int i, rows = 0, cols = 0;
double[][] Rz = new double[4][];
for (i = 0; i < 4; i++)
{
Rz[i] = new double[4];
}
// assigning values to the rotation matrix
Rz[0][0] = Math.Cos(angle);
Rz[0][1] = -Math.Sin(angle);
Rz[0][2] = Rz[0][3] = 0;
Rz[1][0] = Math.Sin(angle);
Rz[1][1] = Math.Cos(angle);
Rz[1][2] = Rz[1][3] = 0;
Rz[2][0] = Rz[2][1] = 0;
Rz[2][2] = 1;
Rz[2][3] = 0;
Rz[3][0] = Rz[3][1] = Rz[3][2] = 0;
Rz[3][3] = 1;
// Rotation matrix about Z filled
double totalangle = (link.anglez + angle >= 2 * Math.PI) ? link.anglez + angle - 2 * Math.PI : link.anglez + angle;
ArmLink newlink = new ArmLink(link.anglex, link.angley, totalangle);
newlink.T = mulMatrices(link.T, Rz, 4, 4, 4, ref rows, ref cols);
return(newlink);
}
public static ArmLink translateXYZ(ArmLink link, double Dx, double Dy, double Dz)
{
int i, rows = 0, cols = 0;
double[][] Tmatrix;
Tmatrix = new double[4][];
for (i = 0; i < 4; i++)
{
Tmatrix[i] = new double[4];
}
// assigning values to the translation matrix
Tmatrix[0][0] = 1;
Tmatrix[0][1] = Tmatrix[0][2] = 0;
Tmatrix[0][2] = 0;
Tmatrix[0][3] = Dx;
Tmatrix[1][0] = 0;
Tmatrix[1][1] = 1;
Tmatrix[1][2] = 0;
Tmatrix[1][3] = Dy;
Tmatrix[2][0] = 0;
Tmatrix[2][1] = 0;
Tmatrix[2][2] = 1;
Tmatrix[2][3] = Dz;
Tmatrix[3][0] = Tmatrix[3][1] = Tmatrix[3][2] = 0;
Tmatrix[3][3] = 1;
// Translationb matrix filled
ArmLink newlink = new ArmLink();
newlink.T = Tmatrix;
return(newlink);
}
/// <summary>
/// Get the Euler angles of a link
/// </summary>
/// <param name="linkindex"></param>
/// <returns>Returns Euler angles as: Pitch, Yaw, Roll</returns>
public EulerParams linkAngles(int linkindex)
{
int i, j, rowsc = 0, colsc = 0;
double[] angles = new double[3];
double[][] R = new double[4][];
for (i = 0; i < 4; i++)
{
R[i] = new double[4];
for (j = 0; j < 4; j++)
{
R[i][j] = links[linkindex].T[i][j];
}
}
for (i = linkindex - 1; i > 0; i--)
{
R = ArmLink.mulMatrices(links[i].T, R, 4, 4, 4, ref rowsc, ref colsc);
}
EulerParams p = new EulerParams();
p.Yaw = Math.Atan2(-R[2][0], Math.Sqrt(Math.Pow(R[0][0], 2) + Math.Pow(R[1][0], 2))); // yaw (about y)
p.Pitch = Math.Atan2(R[1][0] / Math.Cos(angles[1]), R[0][0] / Math.Cos(angles[1])); // pitch (about x)
p.Roll = Math.Atan2(R[2][1] / Math.Cos(angles[1]), R[2][2] / Math.Cos(angles[1])); // roll (about z)
return(p);
}
public RobotArm(int linknum)
{
this.linknum = linknum;
this.effectorwidth = 100; // 100% open
links = new ArmLink[this.linknum];
links[0] = new ArmLink(); // universal link
}
private void button4_Click(object sender, EventArgs e)
{
if (arm == null)
{
return;
}
int angdeg = (int)Link1RateNumeric.Value;
double angle = angdeg * Math.PI / 180;
arm.links[1] = ArmLink.rotateY(arm.links[1], angle);
g.Clear(Color.White);
arm.drawArmcyl(xdim, ydim, g);
double[][] effector = arm.linkPosition(6);
Link1AngleText.Text = Convert.ToString(arm.links[1].angley * 180 / Math.PI);
double x = effector[0][0];
double y = effector[1][0];
double z = effector[2][0];
String str = Convert.ToString(x) + " , " + Convert.ToString(y) + " , " + Convert.ToString(z);
effectorPositionText.Text = str;
//double[] orientation = arm.linkAngles(6);
//yawText.Text = Convert.ToString(orientation[0]*180/Math.PI);
//pitchText.Text = Convert.ToString(orientation[1]*180/Math.PI);
//rollText.Text = Convert.ToString(orientation[2]*180/Math.PI);
EulerParams effectorOrientation = arm.linkAngles(6);
yawText.Text = Convert.ToString(effectorOrientation.Yaw * 180 / Math.PI);
pitchText.Text = Convert.ToString(effectorOrientation.Pitch * 180 / Math.PI);
rollText.Text = Convert.ToString(effectorOrientation.Roll * 180 / Math.PI);
if (connected)
{
if (alligned)
{
String cmd;
if (Servo0BaseIndexRadio.Checked)
{
cmd = " #0 P";
}
else
{
cmd = " #1 P";
}
int basepos = RobotArm.baseServo(arm.links[1].angley);
cmd += Convert.ToString(basepos);
cmd += " T1000\r";
serialPort1.Write(cmd);
}
}
}
public static RobotArm lynxmotionL6()
{
RobotArm arm = new RobotArm(7);
arm.links[1] = ArmLink.translateXYZ(arm.links[0], 0, 6.7, 0);
arm.links[2] = ArmLink.translateXYZ(arm.links[1], 0, 0, 0);
arm.links[3] = ArmLink.translateXYZ(arm.links[2], 15, 0, 0);
arm.links[4] = ArmLink.translateXYZ(arm.links[3], 17.5, 0, 0);
arm.links[5] = ArmLink.translateXYZ(arm.links[4], 3, 0, 0);
arm.links[6] = ArmLink.translateXYZ(arm.links[5], 4, 0, 0);
return(arm);
}
public void drawArmcyl(int xdim, int ydim, Graphics gr)
{
int rowsc = 0, colsc = 0;
double H = 0;
double R = 1;
// drawing the effector now
Effector effector = new Effector(3, 1.5, 2, 10, -3, 0);
effector.opening = effectorwidth;
effector.T = ArmLink.mulMatrices(links[5].T, links[6].T, 4, 4, 4, ref rowsc, ref colsc);
effector.T = ArmLink.mulMatrices(links[4].T, effector.T, 4, 4, 4, ref rowsc, ref colsc);
effector.T = ArmLink.mulMatrices(links[3].T, effector.T, 4, 4, 4, ref rowsc, ref colsc);
effector.T = ArmLink.mulMatrices(links[2].T, effector.T, 4, 4, 4, ref rowsc, ref colsc);
effector.T = ArmLink.mulMatrices(links[1].T, effector.T, 4, 4, 4, ref rowsc, ref colsc);
effector.drawEffector(gr, xdim, ydim);
// link #1 cylinder
Cylinder cylinder1 = new Cylinder(4.5, 4.5, 10, -3, 0, Color.Blue, Cylinder.ALONGY);
cylinder1.T = links[0].T;
cylinder1.drawCylinder(gr, xdim, ydim);
// link #4 cylinder
Cylinder cylinder4 = new Cylinder(R, 4.5, 10, -3, 0, Color.Peru, Cylinder.ALONGX);
cylinder4.T = ArmLink.mulMatrices(links[3].T, links[4].T, 4, 4, 4, ref rowsc, ref colsc);
cylinder4.T = ArmLink.mulMatrices(links[2].T, cylinder4.T, 4, 4, 4, ref rowsc, ref colsc);
cylinder4.T = ArmLink.mulMatrices(links[1].T, cylinder4.T, 4, 4, 4, ref rowsc, ref colsc);
cylinder4.drawCylinder(gr, xdim, ydim);
// link #3 cylinder
Cylinder cylinder3 = new Cylinder(R, 16.5, 10, -3, 0, Color.Green, Cylinder.ALONGX);
cylinder3.T = ArmLink.mulMatrices(links[2].T, links[3].T, 4, 4, 4, ref rowsc, ref colsc);
cylinder3.T = ArmLink.mulMatrices(links[1].T, cylinder3.T, 4, 4, 4, ref rowsc, ref colsc);
cylinder3.drawCylinder(gr, xdim, ydim);
// link#2 cylinder
Cylinder cylinder2 = new Cylinder(R, 13.5, 10, -3, 0, Color.Red, Cylinder.ALONGX);
cylinder2.T = ArmLink.mulMatrices(links[1].T, links[2].T, 4, 4, 4, ref rowsc, ref colsc);
cylinder2.drawCylinder(gr, xdim, ydim);
}
public ArmLink(ArmLink prevlink, double dispX, double dispY, double dispZ,
double rotX, double rotY, double rotZ)
{
int i, j;
ArmLink newlink = ArmLink.translateXYZ(prevlink, dispX, dispY, dispZ);
newlink = ArmLink.rotateX(newlink, rotX);
newlink = ArmLink.rotateY(newlink, rotY);
newlink = ArmLink.rotateZ(newlink, rotY);
// copying transfomation matrix from newlink to the object
for (i = 0; i < 4; i++)
{
for (j = 0; j < 4; j++)
{
this.T[i][j] = newlink.T[i][j];
}
}
// transformation matrix copied
}
public void addDHline(double anglei_1, double ai_1, double di, double homeangle)
{
linknum++;
// creating a new line in the Denavit - Hartenberg matrix
DHmatrix[linknum - 1] = new double[4];
// filling the line now
DHmatrix[linknum - 1][0] = anglei_1; // angle about the x-axis
// bewteen i-1 and i link
DHmatrix[linknum - 1][1] = ai_1; // distance between i-1 and i link
// on the x-axis
DHmatrix[linknum - 1][2] = di; // displace of link i on the z-axis
DHmatrix[linknum - 1][3] = anglei_1; // home angle of link-i
// Denavit - Hartenberg line added
// Now assigning a frame to the link
links[linknum - 1] = ArmLink.rotateX(links[linknum - 2], anglei_1); // rotating about x by angle i-1
// to align with link i
links[linknum - 1] = ArmLink.translateXYZ(links[linknum - 1], ai_1, 0, di); // translating on x by the distance bewteen
// i and i-1 and translating on z by displacement di
links[linknum - 1] = ArmLink.rotateZ(links[linknum - 1], homeangle); // rotating about z to home position (angle theta)
}
public double[][] linkPosition(int linkindex)
{
int i, rows = 0, cols = 0;
double[][] zeropoint;
zeropoint = new double[4][];
for (i = 0; i < 4; i++)
{
zeropoint[i] = new double[1];
}
zeropoint[0][0] = 0;
zeropoint[1][0] = 0;
zeropoint[2][0] = 0;
zeropoint[3][0] = 1;
zeropoint = ArmLink.mulMatrices(links[linkindex].T, zeropoint, 4, 4, 1, ref rows, ref cols);
for (i = linkindex - 1; i > 0; i--)
{
zeropoint = ArmLink.mulMatrices(links[i].T, zeropoint, 4, 4, 1, ref rows, ref cols);
}
return(zeropoint);
}
public void drawEffector(Graphics gr, int panelxdim, int panelydim)
{
double x, y, z;
System.Drawing.Pen pen1 = new Pen(Color.Aqua);
System.Drawing.Pen pen2 = new Pen(Color.Brown);
double[][] point1 = new double[4][];
double[][] point2 = new double[4][];
double[][] point3 = new double[4][];
double[][] point4 = new double[4][];
int i;
for (i = 0; i < 4; i++)
{
point1[i] = new double[1];
point2[i] = new double[1];
point3[i] = new double[1];
point4[i] = new double[1];
}
for (y = -height / 2; y <= height / 2; y += 0.05)
{
point1[0][0] = 0;
point1[1][0] = y;
point1[2][0] = opening * width / 200;
point1[3][0] = 1;
point2[0][0] = fingerlen;
point2[1][0] = y;
point2[2][0] = opening * width / 200;
point2[3][0] = 1;
double x1, y1, z1;
double x2, y2, z2;
point3[0][0] = 0;
point3[1][0] = y;
point3[2][0] = -opening * width / 200;
point3[3][0] = 1;
point4[0][0] = fingerlen;
point4[1][0] = y;
point4[2][0] = -opening * width / 200;
point4[3][0] = 1;
double x3 = 0, y3, z3;
double x4, y4, z4;
// transforming points using transformation matrix T
int rows = 0, cols = 0;
point1 = ArmLink.mulMatrices(T, point1, 4, 4, 1, ref rows, ref cols);
point2 = ArmLink.mulMatrices(T, point2, 4, 4, 1, ref rows, ref cols);
point3 = ArmLink.mulMatrices(T, point3, 4, 4, 1, ref rows, ref cols);
point4 = ArmLink.mulMatrices(T, point4, 4, 4, 1, ref rows, ref cols);
x1 = point1[0][0];
y1 = point1[1][0];
z1 = point1[2][0];
x2 = point2[0][0];
y2 = point2[1][0];
z2 = point2[2][0];
x3 = point3[0][0];
y3 = point3[1][0];
z3 = point3[2][0];
x4 = point4[0][0];
y4 = point4[1][0];
z4 = point4[2][0];
// converting coordinates to 2D
double Sx1 = ((x1 - eyeofsX) * eyedistance / (z1 + eyedistance));
double Sy1 = ((y1 - eyeofsY) * eyedistance / (z1 + eyedistance));
double Sx2 = ((x2 - eyeofsX) * eyedistance / (z2 + eyedistance));
double Sy2 = ((y2 - eyeofsY) * eyedistance / (z2 + eyedistance));
double Sx3 = ((x3 - eyeofsX) * eyedistance / (z3 + eyedistance));
double Sy3 = ((y3 - eyeofsY) * eyedistance / (z3 + eyedistance));
double Sx4 = ((x4 - eyeofsX) * eyedistance / (z4 + eyedistance));
double Sy4 = ((y4 - eyeofsY) * eyedistance / (z4 + eyedistance));
double pixpercmX = panelxdim / 35;
double pixpercmY = panelydim / 35;
// drawing lines
gr.DrawLine(pen2, (float)(Sx1 * pixpercmX), (float)(panelydim - Sy1 * pixpercmY), (float)(Sx2 * pixpercmX), (float)(panelydim - Sy2 * pixpercmY));
gr.DrawLine(pen2, (float)(Sx3 * pixpercmX), (float)(panelydim - Sy3 * pixpercmY), (float)(Sx4 * pixpercmX), (float)(panelydim - Sy4 * pixpercmY));
}
for (y = -height / 2; y <= height / 2; y += 0.05)
{
point1[0][0] = 0;
point1[1][0] = y;
point1[2][0] = -width / 2;
point1[3][0] = 1;
point2[0][0] = 0;
point2[1][0] = y;
point2[2][0] = width / 2;
point2[3][0] = 1;
double x1, y1, z1;
double x2, y2, z2;
// transforming points using transformation matrix T
int rows = 0, cols = 0;
point1 = ArmLink.mulMatrices(T, point1, 4, 4, 1, ref rows, ref cols);
point2 = ArmLink.mulMatrices(T, point2, 4, 4, 1, ref rows, ref cols);
x1 = point1[0][0];
y1 = point1[1][0];
z1 = point1[2][0];
x2 = point2[0][0];
y2 = point2[1][0];
z2 = point2[2][0];
// converting coordinates to 2D
double Sx1 = ((x1 - eyeofsX) * eyedistance / (z1 + eyedistance));
double Sy1 = ((y1 - eyeofsY) * eyedistance / (z1 + eyedistance));
double Sx2 = ((x2 - eyeofsX) * eyedistance / (z2 + eyedistance));
double Sy2 = ((y2 - eyeofsY) * eyedistance / (z2 + eyedistance));
double pixpercmX = panelxdim / 35;
double pixpercmY = panelydim / 35;
// drawing lines
gr.DrawLine(pen1, (float)(Sx1 * pixpercmX), (float)(panelydim - Sy1 * pixpercmY), (float)(Sx2 * pixpercmX), (float)(panelydim - Sy2 * pixpercmY));
}
pen1.Dispose();
pen2.Dispose();
}
public void drawCylinder(Graphics gr, int panelxdim, int panelydim)
{
int i;
double[][] basecenter = new double[4][];
for (i = 0; i < 4; i++)
{
basecenter[i] = new double[1];
}
double[][] peekcenter = new double[4][];
for (i = 0; i < 4; i++)
{
peekcenter[i] = new double[1];
}
double[][] baseperim = new double[4][];
for (i = 0; i < 4; i++)
{
baseperim[i] = new double[1];
}
double[][] peekperim = new double[4][];
for (i = 0; i < 4; i++)
{
peekperim[i] = new double[1];
}
// constructing base center point
basecenter[0][0] = 0;
basecenter[1][0] = 0;
basecenter[2][0] = 0;
basecenter[3][0] = 1;
switch (direction)
{
case ALONGX:
// constructing peek center point
peekcenter[0][0] = H;
peekcenter[1][0] = 0;
peekcenter[2][0] = 0;
peekcenter[3][0] = 1;
break;
case ALONGY:
// constructing peek center point
peekcenter[0][0] = 0;
peekcenter[1][0] = H;
peekcenter[2][0] = 0;
peekcenter[3][0] = 1;
break;
case ALONGZ:
// constructing peek center point
peekcenter[0][0] = 0;
peekcenter[1][0] = H;
peekcenter[2][0] = 0;
peekcenter[3][0] = 1;
break;
}
for (i = 0; i < 360; i++)
{
switch (direction)
{
case ALONGX:
// base circumference point
baseperim[0][0] = 0;
baseperim[1][0] = R * Math.Cos(Math.PI * i / 180);
baseperim[2][0] = R * Math.Sin(Math.PI * i / 180);
baseperim[3][0] = 1;
// peek base circumference point
peekperim[0][0] = H;
peekperim[1][0] = R * Math.Cos(Math.PI * i / 180);
peekperim[2][0] = R * Math.Sin(Math.PI * i / 180);
peekperim[3][0] = 1;
break;
case ALONGY:
// base circumference point
baseperim[0][0] = R * Math.Cos(Math.PI * i / 180);
baseperim[1][0] = 0;
baseperim[2][0] = R * Math.Sin(Math.PI * i / 180);
baseperim[3][0] = 1;
// peek base circumference point
peekperim[0][0] = R * Math.Cos(Math.PI * i / 180);
peekperim[1][0] = H;
peekperim[2][0] = R * Math.Sin(Math.PI * i / 180);
peekperim[3][0] = 1;
break;
case ALONGZ:
// base circumference point
baseperim[0][0] = R * Math.Cos(Math.PI * i / 180);
baseperim[1][0] = R * Math.Sin(Math.PI * i / 180);
baseperim[2][0] = 0;
baseperim[3][0] = 1;
// peek base circumference point
peekperim[0][0] = R * Math.Cos(Math.PI * i / 180);
peekperim[1][0] = R * Math.Sin(Math.PI * i / 180);
peekperim[2][0] = H;
peekperim[3][0] = 1;
break;
}
// transforming coordinates with translation matrix
int rows = 0, cols = 0;
baseperim = ArmLink.mulMatrices(T, baseperim, 4, 4, 1, ref rows, ref cols);
peekperim = ArmLink.mulMatrices(T, peekperim, 4, 4, 1, ref rows, ref cols);
// Computing screen coordinates
double Xs1, Ys1, Xs2, Ys2, x1, y1, z1, x2, y2, z2;
x1 = baseperim[0][0];
y1 = baseperim[1][0];
z1 = baseperim[2][0];
x2 = peekperim[0][0];
y2 = peekperim[1][0];
z2 = peekperim[2][0];
Ys1 = ((y1 - eyeofsY) * eyedistance / (z1 + eyedistance));
Xs1 = ((x1 - eyeofsX) * eyedistance / (z1 + eyedistance));
Ys2 = (y2 - eyeofsY) * eyedistance / (z2 + eyedistance);
Xs2 = (x2 - eyeofsX) * eyedistance / (z2 + eyedistance);
double pixpercmX = panelxdim / 35;
double pixpercmY = panelydim / 35;
System.Drawing.Pen pen = new Pen(color);
// drawing circles
gr.DrawEllipse(pen, (float)(Xs1 * pixpercmX), (float)(panelydim - Ys1 * pixpercmY), 1, 1);
gr.DrawEllipse(pen, (float)(Xs2 * pixpercmX), (float)(panelydim - Ys2 * pixpercmY), 1, 1);
if (i % 15 == 0)
{
gr.DrawLine(pen, (float)(Xs1 * pixpercmX), (float)(panelydim - Ys1 * pixpercmY), (float)(Xs2 * pixpercmX), (float)(panelydim - Ys2 * pixpercmY));
}
}
}
private void timer1_Tick(object sender, EventArgs e)
{
clockticks++;
if (clockticks <= trajectory.len)
{
if (trajectory.moves[clockticks - 1].Dbase != 0)
{
arm.links[1] = ArmLink.rotateY(arm.links[1], trajectory.moves[clockticks - 1].Dbase);
}
if (trajectory.moves[clockticks - 1].Dth1 != 0)
{
arm.links[2] = ArmLink.rotateZ(arm.links[2], trajectory.moves[clockticks - 1].Dth1);
}
if (trajectory.moves[clockticks - 1].Dth2 != 0)
{
arm.links[3] = ArmLink.rotateZ(arm.links[3], trajectory.moves[clockticks - 1].Dth2);
}
if (trajectory.moves[clockticks - 1].Dth3 != 0)
{
arm.links[4] = ArmLink.rotateZ(arm.links[4], trajectory.moves[clockticks - 1].Dth3);
}
g.Clear(Color.White);
arm.drawArmcyl(xdim, ydim, g);
double[][] effector = arm.linkPosition(6);
Link1AngleText.Text = Convert.ToString(arm.links[1].angley * 180 / Math.PI);
Link2AngleText.Text = Convert.ToString(arm.links[2].anglez * 180 / Math.PI);
Link3AngleText.Text = Convert.ToString(arm.links[3].anglez * 180 / Math.PI);
Link4AngleText.Text = Convert.ToString(arm.links[4].anglez * 180 / Math.PI);
double x = effector[0][0];
double y = effector[1][0];
double z = effector[2][0];
String str = Convert.ToString(x) + " , " + Convert.ToString(y) + " , " + Convert.ToString(z);
effectorPositionText.Text = str;
EulerParams orientation = arm.linkAngles(6);
yawText.Text = Convert.ToString(orientation.Yaw * 180 / Math.PI);
pitchText.Text = Convert.ToString(orientation.Pitch * 180 / Math.PI);
rollText.Text = Convert.ToString(orientation.Roll * 180 / Math.PI);
if (alligned)
{
String cmd;
// link#1 motion command
if (Servo0BaseIndexRadio.Checked)
{
cmd = " #0 P";
}
else
{
cmd = " #1 P";
}
int basepos = RobotArm.baseServo(arm.links[1].angley);
cmd += Convert.ToString(basepos);
cmd += " T1000\r";
serialPort1.Write(cmd);
// link #2 motion command
if (Servo0BaseIndexRadio.Checked)
{
cmd = " #1 P";
}
else
{
cmd = " #2 P";
}
int link2pos = RobotArm.link2Servo(arm.links[2].anglez);
cmd += Convert.ToString(link2pos);
cmd += " T1000\r";
serialPort1.Write(cmd);
// link #3 motion command
if (Servo0BaseIndexRadio.Checked)
{
cmd = "#2 P";
}
else
{
cmd = " #3 P";
}
int link3pos = RobotArm.link3Servo(arm.links[3].anglez);
cmd += Convert.ToString(link3pos);
cmd += " T1000\r";
serialPort1.Write(cmd);
// link #4 motion command
if (Servo0BaseIndexRadio.Checked)
{
cmd = " #3 P";
}
else
{
cmd = " #4 P";
}
int link4pos = RobotArm.link4Servo(arm.links[4].anglez);
cmd += Convert.ToString(link4pos);
cmd += " T1000\r";
serialPort1.Write(cmd);
}
}
else
{
moving = false;
timer1.Enabled = false;
}
}
