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 button13_Click(object sender, EventArgs e)
{
if (arm == null)
{
return;
}
int closing = (int)GripCloseNumeric.Value;
arm.effectorwidth -= (arm.effectorwidth > 0) ? closing : 0;
if (arm.effectorwidth < 0)
{
arm.effectorwidth = 0;
}
g.Clear(Color.White);
arm.drawArmcyl(xdim, ydim, g);
double[][] effector = arm.linkPosition(6);
GripWidthText.Text = Convert.ToString(arm.effectorwidth);
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;
if (connected)
{
if (alligned)
{
String cmd;
if (Servo0BaseIndexRadio.Checked)
{
cmd = " #5 P";
}
else
{
cmd = " #6 P";
}
int grippos = RobotArm.gripServo(arm.effectorwidth);
cmd += Convert.ToString(grippos);
cmd += " T1000\r";
serialPort1.Write(cmd);
}
}
}
