private void checkBox1_Click(object sender, EventArgs e) { alligned = AlignWithRobotCheckBox.Checked && connected; AlignWithRobotCheckBox.Checked = alligned; if (alligned) { int basepos = RobotArm.baseServo(arm.links[1].angley); int link2pos = RobotArm.link2Servo(arm.links[2].anglez); int link3pos = RobotArm.link3Servo(arm.links[3].anglez); int link4pos = RobotArm.link4Servo(arm.links[4].anglez); int grippos = RobotArm.link5Servo(arm.links[5].anglex); int effwidth = RobotArm.gripServo(arm.effectorwidth); String cmd = ((Servo0BaseIndexRadio.Checked) ? " #0 P" : "#1 P") + Convert.ToString(basepos); cmd += ((Servo0BaseIndexRadio.Checked) ? " #1 P" : " #2 P") + Convert.ToString(link2pos); cmd += ((Servo0BaseIndexRadio.Checked) ? " #2 P" : " #3 P") + Convert.ToString(link3pos); cmd += ((Servo0BaseIndexRadio.Checked) ? " #3 P" :" #4 P") + Convert.ToString(link4pos); cmd += ((Servo0BaseIndexRadio.Checked) ? " #1 P" : " #5 P") + Convert.ToString(grippos); cmd += ((Servo0BaseIndexRadio.Checked) ? " #1 P" : " #6 P") + Convert.ToString(effwidth) + " T100\r"; serialPort1.Write(cmd); } }
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); } } }