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 button9_Click(object sender, EventArgs e)
{
double x = (double)DestinationXText.Value;
double y = (double)DestinationYText.Value;
double z = (double)DestinationZText.Value;
double curbaseangle = arm.links[1].angley;
double curth1 = arm.links[2].anglez;
double curth2 = arm.links[3].anglez;
double curth3 = arm.links[4].anglez;
double dbaseangle = 0, dth1 = 0, dth2 = 0, dth3 = 0;
Boolean reachable = RobotArm.solveIK(ref dbaseangle, ref dth1, ref dth2, ref dth3, x, y, z, curth1, curth2, curth3);
DestinationText.Text = (reachable) ? "Reachable" : "Unreachable";
DestBaseAngleText.Text = (reachable) ? Convert.ToString(dbaseangle * 180 / Math.PI) : "N/A";
DestLink2Text.Text = (reachable) ? Convert.ToString(dth1 * 180 / Math.PI) : "N/A";
DestLink3Text.Text = (reachable) ? Convert.ToString(dth2 * 180 / Math.PI) : "N/A";
DestLink4Text.Text = (reachable) ? Convert.ToString(dth3 * 180 / Math.PI) : "N/A";
if (reachable)
{
destsolved = true;
destbase = dbaseangle;
destth1 = dth1;
destth2 = dth2;
destth3 = dth3;
}
else
{
destsolved = false;
}
}
public Trajectory demoSinwave()
{
double baseangle = 0, th1 = 0, th2 = 0, th3 = 0;
double z, Dz, x, y;
RobotArm.solveIK(ref baseangle, ref th1, ref th2, ref th3, 25, 20, 10, links[2].anglez, links[3].anglez, links[4].anglez);
Trajectory t = RobotArm.planTrajectory(this, baseangle, th1, th2, th3, 10);
Dz = 13 * 0.025;
x = 25;
for (z = 12; z > -1; z -= Dz)
{
y = 20 + 3 * Math.Sin(z);
double newbaseangle = 0, newth1 = 0, newth2 = 0, newth3 = 0;
Boolean solved = RobotArm.solveIK(ref newbaseangle, ref newth1, ref newth2, ref newth3, x, y, z, th1, th2, th3);
// appending trajectory
t.len++;
t.moves[t.len - 1].Dbase = newbaseangle - baseangle;
t.moves[t.len - 1].Dth1 = newth1 - th1;
t.moves[t.len - 1].Dth2 = newth2 - th2;
t.moves[t.len - 1].Dth3 = newth3 - th3;
baseangle = newbaseangle;
th1 = newth1;
th2 = newth2;
th3 = newth3;
}
return(t);
}
public static Trajectory planTrajectory(RobotArm arm, double Ebase, double Eth1,
double Eth2, double Eth3, int time)
{
double Sbase = arm.links[1].angley;
double Sth1 = arm.links[2].anglez;
double Sth2 = arm.links[3].anglez;
double Sth3 = arm.links[4].anglez;
Trajectory t = new Trajectory(Sbase, Sth1, Sth2, Sth3, Ebase, Eth1, Eth2, Eth3, time);
double baseangle = Sbase;
double th1 = Sth1;
double th2 = Sth2;
double th3 = Sth3;
while ((Math.Abs(baseangle - Ebase) > 0.01) || (Math.Abs(th1 - Eth1) > 0.01) || (Math.Abs(th2 - Eth2) > 0.01) || (Math.Abs(th3 - Eth3) > 0.01))
{
t.len++;
if (Math.Abs(baseangle - Ebase) > 0.01)
{
t.moves[t.len - 1].Dbase = t.stepbase;
baseangle += t.stepbase;
}
else
{
t.moves[t.len - 1].Dbase = 0;
}
if (Math.Abs(th1 - Eth1) > 0.01)
{
t.moves[t.len - 1].Dth1 = t.stepth1;
th1 += t.stepth1;
}
else
{
t.moves[t.len - 1].Dth1 = 0;
}
if (Math.Abs(th2 - Eth2) > 0.01)
{
t.moves[t.len - 1].Dth2 = t.stepth2;
th2 += t.stepth2;
}
else
{
t.moves[t.len - 1].Dth2 = 0;
}
if (Math.Abs(th3 - Eth3) > 0.01)
{
t.moves[t.len - 1].Dth3 = t.stepth3;
th3 += t.stepth3;
}
else
{
t.moves[t.len - 1].Dth3 = 0;
}
}
return(t);
}
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);
}
}
}
public static RobotArm lynxmotionL6()
{
RobotArm arm = new RobotArm(7);
arm.links[1] = ArmLink.translateXYZ(arm.links[0], 0, 6.7, 0);
arm.links[2] = ArmLink.translateXYZ(arm.links[1], 0, 0, 0);
arm.links[3] = ArmLink.translateXYZ(arm.links[2], 15, 0, 0);
arm.links[4] = ArmLink.translateXYZ(arm.links[3], 17.5, 0, 0);
arm.links[5] = ArmLink.translateXYZ(arm.links[4], 3, 0, 0);
arm.links[6] = ArmLink.translateXYZ(arm.links[5], 4, 0, 0);
return(arm);
}
private void button1_Click_2(object sender, EventArgs e)
{
Trajectory t = RobotArm.planTrajectory(arm, destbase, destth1,
destth2, destth3, 10);
}