private void button11_Click(object sender, EventArgs e)
{
if (arm == null)
{
return;
}
int angdeg = (int)Link5Numeric.Value;
double angle = angdeg * Math.PI / 180;
arm.links[5] = ArmLink.rotateX(arm.links[5], angle);
g.Clear(Color.White);
arm.drawArmcyl(xdim, ydim, g);
double[][] effector = arm.linkPosition(6);
Link5AngleText.Text = Convert.ToString(arm.links[5].anglex * 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 (connected)
{
if (alligned)
{
String cmd;
if (Servo0BaseIndexRadio.Checked)
{
cmd = "#4 P";
}
else
{
cmd = " #5 P";
}
int link5pos = RobotArm.link5Servo(arm.links[5].anglex);
cmd += Convert.ToString(link5pos);
cmd += " T1000\r";
serialPort1.Write(cmd);
}
}
}
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)
}
