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);
}
private void button3_Click(object sender, EventArgs e)
{
arm = RobotArm.lynxmotionL6();
g = panel1.CreateGraphics();
xdim = panel1.Width;
ydim = panel1.Height;
g.Clear(Color.White);
this.arm.drawArmcyl(xdim, ydim, g);
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;
Link1AngleText.Text = Link2AngleText.Text = Link3AngleText.Text = Link4AngleText.Text = Link5AngleText.Text = "0";
GripWidthText.Text = "100";
Link1AngleText.Text = "0";
Link2AngleText.Text = "0";
Link3AngleText.Text = "0";
Link4AngleText.Text = "0";
Link5AngleText.Text = "0";
yawText.Text = "0";
pitchText.Text = "0";
rollText.Text = "0";
}
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);
}
}
}