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); } } }