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