/// <summary>
/// Get the Euler angles of a link
/// </summary>
/// <param name="linkindex"></param>
/// <returns>Returns Euler angles as: Pitch, Yaw, Roll</returns>
public EulerParams linkAngles(int linkindex)
{
int i, j, rowsc = 0, colsc = 0;
double[] angles = new double[3];
double[][] R = new double[4][];
for (i = 0; i < 4; i++)
{
R[i] = new double[4];
for (j = 0; j < 4; j++)
{
R[i][j] = links[linkindex].T[i][j];
}
}
for (i = linkindex - 1; i > 0; i--)
{
R = ArmLink.mulMatrices(links[i].T, R, 4, 4, 4, ref rowsc, ref colsc);
}
EulerParams p = new EulerParams();
p.Yaw = Math.Atan2(-R[2][0], Math.Sqrt(Math.Pow(R[0][0], 2) + Math.Pow(R[1][0], 2))); // yaw (about y)
p.Pitch = Math.Atan2(R[1][0] / Math.Cos(angles[1]), R[0][0] / Math.Cos(angles[1])); // pitch (about x)
p.Roll = Math.Atan2(R[2][1] / Math.Cos(angles[1]), R[2][2] / Math.Cos(angles[1])); // roll (about z)
return(p);
}
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);
}
}
}
private void timer1_Tick(object sender, EventArgs e)
{
clockticks++;
if (clockticks <= trajectory.len)
{
if (trajectory.moves[clockticks - 1].Dbase != 0)
{
arm.links[1] = ArmLink.rotateY(arm.links[1], trajectory.moves[clockticks - 1].Dbase);
}
if (trajectory.moves[clockticks - 1].Dth1 != 0)
{
arm.links[2] = ArmLink.rotateZ(arm.links[2], trajectory.moves[clockticks - 1].Dth1);
}
if (trajectory.moves[clockticks - 1].Dth2 != 0)
{
arm.links[3] = ArmLink.rotateZ(arm.links[3], trajectory.moves[clockticks - 1].Dth2);
}
if (trajectory.moves[clockticks - 1].Dth3 != 0)
{
arm.links[4] = ArmLink.rotateZ(arm.links[4], trajectory.moves[clockticks - 1].Dth3);
}
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);
Link2AngleText.Text = Convert.ToString(arm.links[2].anglez * 180 / Math.PI);
Link3AngleText.Text = Convert.ToString(arm.links[3].anglez * 180 / Math.PI);
Link4AngleText.Text = Convert.ToString(arm.links[4].anglez * 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 (alligned)
{
String cmd;
// link#1 motion command
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);
// link #2 motion command
if (Servo0BaseIndexRadio.Checked)
{
cmd = " #1 P";
}
else
{
cmd = " #2 P";
}
int link2pos = RobotArm.link2Servo(arm.links[2].anglez);
cmd += Convert.ToString(link2pos);
cmd += " T1000\r";
serialPort1.Write(cmd);
// link #3 motion command
if (Servo0BaseIndexRadio.Checked)
{
cmd = "#2 P";
}
else
{
cmd = " #3 P";
}
int link3pos = RobotArm.link3Servo(arm.links[3].anglez);
cmd += Convert.ToString(link3pos);
cmd += " T1000\r";
serialPort1.Write(cmd);
// link #4 motion command
if (Servo0BaseIndexRadio.Checked)
{
cmd = " #3 P";
}
else
{
cmd = " #4 P";
}
int link4pos = RobotArm.link4Servo(arm.links[4].anglez);
cmd += Convert.ToString(link4pos);
cmd += " T1000\r";
serialPort1.Write(cmd);
}
}
else
{
moving = false;
timer1.Enabled = false;
}
}
