/// <summary>
/// Set position of the robot by decart coordinates.
/// </summary>
/// <param name="Position"></param>
public void SetDPosition(Decart Position)
{
// Calculate
Joint configuration = KinematicsStatic.IverseKinematicTask(Position);
this.SetJPosition(configuration);
}
/// <summary>
///
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void btnMoveRobotToHome_Click(object sender, EventArgs e)
{
if (!this.enableSending)
{
return;
}
int comIndex = Convert.ToInt32(this.robotSerialPortName.Replace("COM", ""));
try
{
using (DynamixelController myDynamixel = new DynamixelController(comIndex, 1))
{
DynamixelArm myArm = new DynamixelArm(myDynamixel, this.jointIDs);
//System.Threading.Thread.Sleep(2000);
myArm.GoToHome();
Decart pos = myArm.GetDPosition();
Console.WriteLine("XYZPR: {0}", pos.ToString());
}
}
catch (Exception exception)
{
Console.WriteLine("Robot: {0}", exception.Message);
}
}
public void TestDecartToPolar()
{
Decart d = new Decart(10, 6);
Polar p = d;
Decart dd = p;
MessageBox.Show(p.ToString());
MessageBox.Show(dd.ToString());
}
/// <summary>
/// Get position of the robot by decart coordinates
/// </summary>
/// <returns>Decart coordinate.</returns>
public Decart GetDPosition()
{
// Current robot pos.
Joint curJointPos = this.GetJPosition();
// Current robot in XYZPR coordinates.
Decart curDecPos = KinematicsStatic.RightsKinematicTask(curJointPos);
return(curDecPos);
}
/// <summary>
///
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void btnSendCommand_Click(object sender, EventArgs e)
{
if (!this.enableSending)
{
return;
}
int comIndex = Convert.ToInt32(this.robotSerialPortName.Replace("COM", ""));
tsslblSerialStatus.Text = "";
try
{
using (DynamixelController myDynamixel = new DynamixelController(comIndex, 1))
{
DynamixelArm myArm = new DynamixelArm(myDynamixel, this.jointIDs);
myArm.ComState += new EventHandler <StateEvent>(myRobot_RobotComState);
myArm.ServoState += new EventHandler <ErrorEvent>(myRobot_RobotServoState);
/*
* for (int iterator = 0; iterator < 10; iterator++)
* {
* myArm.LJInterpolation(new Joint(20.0d, -20.0d, 20.0d, 20.0d, 0.0d), 20.0d);
* myArm.LJInterpolation(new Joint(-40.0d, 40.0d, -40.0d, -40.0d, 0.0d), 40.0d);
* myArm.LJInterpolation(new Joint(20.0d, -20.0d, 20.0d, 20.0d, 0.0d), 20.0d);
* }
*/
myArm.GoToHome();
myArm.ComState -= new EventHandler <StateEvent>(myRobot_RobotComState);
myArm.ServoState -= new EventHandler <ErrorEvent>(myRobot_RobotServoState);
this.TestProgram(myArm);
}
Joint homeJ = new Joint(0.0d, 0.0d, 0.0d, 0.0d, 0.0d);
Robot.Experimantal.ArmConfiguration myConfig = new Robot.Experimantal.ArmConfiguration(47, 47, 142, 110);
Robot.Experimantal.ArmKinematics myKinematics = new Robot.Experimantal.ArmKinematics(myConfig);
Decart homeD = myKinematics.Forward(homeJ);
string values = homeD.ToString();
Console.WriteLine("Values: {0}", values);
}
catch (Exception exception)
{
Console.WriteLine("Robot: {0}", exception.Message);
}
}
//Angel Dimov
private void btnSetPosition_Click(object sender, EventArgs e)
{
double baseJoint = double.Parse(this.txtBase.Text);
double shoulderJoint = double.Parse(this.txtShoulder.Text);
double elbowJoint = double.Parse(this.txtElbow.Text);
double wristJoint = double.Parse(this.txtWrist.Text);
double gripperJoint = double.Parse(this.txtGripper.Text);
this.trackBar1.Value = (int)baseJoint;
this.trackBar2.Value = (int)shoulderJoint;
this.trackBar3.Value = (int)elbowJoint;
this.trackBar4.Value = (int)wristJoint;
this.trackBar5.Value = (int)gripperJoint;
if (!this.enableSending)
{
return;
}
int comIndex = Convert.ToInt32(this.robotSerialPortName.Replace("COM", ""));
try
{
using (DynamixelController myDynamixel = new DynamixelController(comIndex, 1))
{
DynamixelArm myArm = new DynamixelArm(myDynamixel, this.jointIDs);
//baseJoint -= tempBase;
//shoulderJoint -= tempShoulder;
//elbowJoint -= tempShoulder;
//wristJoint -= tempWrist;
//gripperJoint -= tempGripper;
myArm.LJInterpolation(new Joint(baseJoint, shoulderJoint, elbowJoint, wristJoint, gripperJoint), 20);
Decart pos = myArm.GetDPosition();
Console.WriteLine("XYZPR: {0}", pos.ToString());
}
}
catch (Exception exception)
{
Console.WriteLine("Robot: {0}", exception.Message);
}
}
private void addRandomFlowerXY(Decart dec)
{
try
{
PictureBox rnd = new System.Windows.Forms.PictureBox();
rnd.Image = this.randomImage;
rnd.Name = GetPictureName();
int imY = rnd.Image.Size.Height;
int imX = rnd.Image.Size.Width;
rnd.Location = new System.Drawing.Point(dec.xx, dec.yy);
rnd.Size = new System.Drawing.Size(imX, imY);
this.Controls.Add(rnd);
dec.xx = dec.xx + imX;
}
catch { }
}
/// <summary>
/// Test program fr the robot.
/// </summary>
/// <param name="arm"></param>
private void TestProgram(DynamixelArm arm)
{
//Steve Stavrev
/*
* arm.LJInterpolation(new Joint(15.0d, 0.0d, 0.0d, 0.0d, 0.0d), 20);
* System.Threading.Thread.Sleep(1000);
* arm.LJInterpolation(new Joint(15.0d, 0.0d, 0.0d, 0.0d, 0.0d), 20);
* System.Threading.Thread.Sleep(1000);
*
* arm.LJInterpolation(new Joint(0.0d, 0.0d, 0.0d, 0.0d, 90.0d), 20);
* System.Threading.Thread.Sleep(1000);
*
* arm.LJInterpolation(new Joint(0.0d, 30.0d, 0.0d, 0.0d, 0.0d), 20);
* System.Threading.Thread.Sleep(1000);
* arm.LJInterpolation(new Joint(0.0d, 30.0d, 0.0d, 0.0d, 0.0d), 20);
* System.Threading.Thread.Sleep(1000);
*
* arm.LJInterpolation(new Joint(0.0d, 0.0d, 0.0d, 0.0d, 0.0d), 20);
* System.Threading.Thread.Sleep(1000);
*
*
* arm.LJInterpolation(new Joint(0.0d, 0.0d, 35.0d, 0.0d, 0.0d), 20);
* System.Threading.Thread.Sleep(1000);
* arm.LJInterpolation(new Joint(0.0d, 0.0d, 35.0d, 0.0d, 0.0d), 20);
* System.Threading.Thread.Sleep(1000);
*
* arm.LJInterpolation(new Joint(0.0d, 0.0d, 0.0d, 20.0d, 0.0d), 20);
* System.Threading.Thread.Sleep(1000);
* arm.LJInterpolation(new Joint(0.0d, 0.0d, 0.0d, 10.0d, 0.0d), 20);
* System.Threading.Thread.Sleep(1000);
*/
/*
* //Dzhuvi Juniour
* arm.SetJPosition(new JointConfiguration(-20.0d, 0.0d, 0.0d, 0.0d, 0.0d));
* System.Threading.Thread.Sleep(1000);
* arm.SetJPosition(new JointConfiguration(0.0d, 15.0d, 0.0d, 0.0d, 0.0d));
* System.Threading.Thread.Sleep(1000);
* arm.SetJPosition(new JointConfiguration(0.0d, 15.0d, 0.0d, 0.0d, 0.0d));
* System.Threading.Thread.Sleep(1000);
*
* arm.SetJPosition(new JointConfiguration(0.0d, 0.0d, 0.0d, 0.0d, 90.0d));
* System.Threading.Thread.Sleep(1000);
* arm.SetJPosition(new JointConfiguration(0.0d, 0.0d, 20.0d, 0.0d, 0.0d));
* System.Threading.Thread.Sleep(1000);
*
* arm.SetJPosition(new JointConfiguration(0.0d, 0.0d, 0.0d, 0.0d, 45.0d));
* System.Threading.Thread.Sleep(1000);
*
* arm.SetJPosition(new JointConfiguration(0.0d, 0.0d, -20.0d, 0.0d, 0.0d));
* System.Threading.Thread.Sleep(1000);
* arm.SetJPosition(new JointConfiguration(0.0d, -20.0d, 0.0d, 0.0d, 0.0d));
* System.Threading.Thread.Sleep(1000);
* arm.SetJPosition(new JointConfiguration(0.0d, 0.0d, 0.0d, 0.0d, 0.0d));
* System.Threading.Thread.Sleep(1000);
* //end Dzhuvi Juniour
*/
/*arm.SetJPosition(new JointConfiguration(10.0d, 10.0d, 0.0d, 0.0d, 0.0d));
* System.Threading.Thread.Sleep(1000);
* arm.SetJPosition(new JointConfiguration(-10.0d, 0.0d, 0.0d, 0.0d, 0.0d));
* System.Threading.Thread.Sleep(1000);
* arm.SetJPosition(new JointConfiguration(0.0d, 0.0d, 0.0d, 0.0d, 0.0d));
* System.Threading.Thread.Sleep(1000);*/
// Plamen Velikov
//int i = -1;
//while (true)
//{
// const double maxAngle = 40.0d;
// const double step = 1.0d;
// for (double angle = 0; angle < maxAngle; angle += step)
// {
// arm.SetJPosition(new JointConfiguration(angle * i, angle, angle, angle, angle));
// //System.Threading.Thread.Sleep(1);
// }
// for (double angle = maxAngle; angle > 0; angle -= step)
// {
// arm.SetJPosition(new JointConfiguration(angle * i, angle, angle, angle, angle));
// //System.Threading.Thread.Sleep(1);
// }
// i = i > 0 ? -1 : 1;
//}
//for (int i = 0; i < length; i++)
//{
// arm.SetJPosition(new JointConfiguration(0.0d, 0.0d, 0.0d, 0.0d, 40.0d));
// System.Threading.Thread.Sleep(1000);
// arm.SetJPosition(new JointConfiguration(0.0d, 0.0d, 0.0d, 0.0d, -40.0d));
// System.Threading.Thread.Sleep(1000);
//}
// Krasimir Yosifov: 29.11.2014
/*
* for (int i = 0; i < 360; i+=5)
* {
* arm.SetJPosition(new JointConfiguration(Math.Sin(i * 3.14 / 180) * 30, 0, (double)Math.Cos(i * 3.14 / 180) * 30, 0.0d, 0.0d));
* System.Threading.Thread.Sleep(1);
* }
*/
/*
* //Krasimir ...
* arm.SetJPosition(new Joint(0, 0, 0, 0, 120));
* for (int i = 0; i < 36; i++)
* {
* arm.SetJPosition(new Joint(0, i, 0, i, 120));
* System.Threading.Thread.Sleep(1);
* }
* System.Threading.Thread.Sleep(500);
* arm.SetJPosition(new Joint(0, 36, 0, 36, 52));
* System.Threading.Thread.Sleep(500);
* for (int i = 36; i > 20; i--)
* {
* arm.SetJPosition(new Joint(0, i, 0, 36, 52));
* System.Threading.Thread.Sleep(1);
* }
*
* for (int i = 0; i < 12; i++)
* {
* arm.SetJPosition(new Joint(i*5, 20, 0, 36-i, 52));
* System.Threading.Thread.Sleep(10);
* }
* arm.SetJPosition(new Joint(55, 20, 0, 27, 120));
* System.Threading.Thread.Sleep(1000);
*/
// Linda Massarwe ...
/*
* //arm.LJInterpolation(new Joint(0, 0, 0, 5, 0), 20.0d);
* arm.LJInterpolation(new Joint(62.0d, -27.0d, 5, 15, 0.0d), 20.0d);
* System.Threading.Thread.Sleep(2000);
*
* arm.LJInterpolation(new Joint(50, 23, 0, 0, 0), 20.0d);
* System.Threading.Thread.Sleep(3000);
*
* arm.LJInterpolation(new Joint(0.0d, 0.0d, 0.0d, 0.0d, 0.0d), 20);
*/
// Angel Dimov
arm.LJInterpolation(new Joint(35.0d, 0.0d, 0.0d, 0.0d, 75.0d), 20);
System.Threading.Thread.Sleep(3000);
//arm.GoToHome();
Decart pos = arm.GetDPosition();
Console.WriteLine("XYZPR: {0}", pos.ToString());
}
/// <summary>
/// Invers kinematics task.
/// </summary>
/// <param name="Configuration">The input position is in [mm].</param>
/// <returns>The output configuration is in [deg].</returns>
public Joint IverseKinematicTask(Decart position)
{
Joint configuration = new Joint(0.0d, 0.0d, 0.0d, 0.0d, 0.0d);
// Support variables
double RR, LF, RM, GA, AL;
if (position.Z < 0)
{
position.Z = 0;
}
if (position.Z < 300 && position.X < 0)
{
position.X = 100;
}
RR = Math.Sqrt(position.X * position.X + position.Y * position.Y);
LF = 2 * this.configuration.L1 + this.configuration.LG;
if (position.Z == this.configuration.H)
{
RM = LF;
}
else if (position.Z == 0)
{
RM = Math.Sqrt(LF * LF - this.configuration.H * this.configuration.H);
}
else
{
RM = Math.Sqrt(LF * LF - (this.configuration.H - position.Z) * (this.configuration.H - position.Z));
}
if (RR > RM)
{
RR = RM;
}
position.P = position.P / this.configuration.C;
position.R = position.R / this.configuration.C;
position.R = RR - this.configuration.LG * Math.Cos(position.P);
position.Z = this.configuration.H - position.Z - this.configuration.LG * Math.Sin(position.P);
if (position.R == 0)
{
GA = Math.Sign(position.Z) * Math.PI / 2;
}
else
{
GA = Math.Atan(position.Z / position.R);
}
AL = Math.Sqrt(position.R * position.R + position.Z * position.Z) / 2;
AL = Math.Atan(Math.Sqrt(this.configuration.L1 * this.configuration.L1 - AL * AL) / AL);
if (position.X == 0)
{
configuration.Base = Math.Sign(position.Y) * Math.PI / 2;
}
else
{
configuration.Base = Math.Atan(position.Y / position.X);
}
configuration.Shoulder = GA - AL;
configuration.Elbow = GA + AL;
configuration.Wrist = position.P + position.R + this.configuration.R1 * configuration.Base;
configuration.Gripper = position.P - position.R - this.configuration.R1 * configuration.Base;
configuration.Base = Rad2Deg(configuration.Base);
configuration.Shoulder = Rad2Deg(configuration.Shoulder);
configuration.Elbow = Rad2Deg(configuration.Elbow);
configuration.Wrist = Rad2Deg(configuration.Wrist);
configuration.Gripper = Rad2Deg(configuration.Gripper);
return(configuration);
}
private void ThreadEntry()
{
this.scrW = Screen.PrimaryScreen.WorkingArea.Width;
this.scrH = Screen.PrimaryScreen.WorkingArea.Height;
Invoke(new AddFlowerDelegate(() => Cursor.Hide()));
Decart dec = new Decart();
Object[] obj = new Object[] { dec };
for (dec.yy = 0; dec.yy < scrH; dec.yy += 100)
{
for (dec.xx = 0; dec.xx < scrW;)
{
Invoke(this.addFlowerXY, obj);
System.Threading.Thread.Sleep(10);
}
}
for (int fl = 0; fl < 250; fl++)
{
Invoke(this.addFlower);
System.Threading.Thread.Sleep(10);
}
Invoke(new AddFlowerDelegate(() =>
{
int lh = TextRenderer.MeasureText(this.label1.Text, label1.Font).Height * 2;
int sh = Screen.PrimaryScreen.WorkingArea.Height / 4;
this.label1.Location = new Point(label1.Location.X, sh);
this.label2.Location = new Point(label1.Location.X, sh + lh);
this.label3.Location = new Point(label1.Location.X, sh + lh * 2);
this.label4.Location = new Point(label1.Location.X, sh + lh * 4);
}
));
AddTextDelegate showLabel = new AddTextDelegate((lb, x) =>
{
lb.Visible = true;
}
);
foreach (KeyValuePair <Label, string> pair in this.dict)
{
Invoke(showLabel, new Object[] { pair.Key, ' ' });
for (int i = 0; i < pair.Value.Length; i++)
{
Invoke(this.addTextDelegate, new Object[] { pair.Key, pair.Value[i] });
System.Threading.Thread.Sleep(50);
}
}
Invoke(new AddFlowerDelegate(() =>
{
Cursor.Show();
this.myTimer.Tick += this.timerEventShow;
this.myTimer.Interval = 100;
this.myTimer.Start();
}
));
}
/// <summary>
/// Invers kinematics task.
/// </summary>
/// <param name="Configuration">The input position is in [mm].</param>
/// <returns>The output configuration is in [deg].</returns>
public static Joint IverseKinematicTask(Decart Position)
{
// Output variables
double T1, T2, T3, T4, T5;
// Support variables
double RR, LF, RM, GA, AL;
// Decart configuration
double X0 = Position.X;
double Y0 = Position.Y;
double Z0 = Position.Z;
double P0 = Position.P;
double R0 = Position.R;
if (Z0 < 0)
{
Z0 = 0;
}
if (Z0 < 300 && X0 < 0)
{
X0 = 100;
}
RR = Math.Sqrt(X0 * X0 + Y0 * Y0);
LF = 2 * L1 + LG;
if (Z0 == H)
{
RM = LF;
}
else if (Z0 == 0)
{
RM = Math.Sqrt(LF * LF - H * H);
}
else
{
RM = Math.Sqrt(LF * LF - (H - Z0) * (H - Z0));
}
if (RR > RM)
{
RR = RM;
}
P0 = P0 / C;
R0 = R0 / C;
R0 = RR - LG * Math.Cos(P0);
Z0 = H - Z0 - LG * Math.Sin(P0);
if (R0 == 0)
{
GA = Math.Sign(Z0) * Math.PI / 2;
}
else
{
GA = Math.Atan(Z0 / R0);
}
AL = Math.Sqrt(R0 * R0 + Z0 * Z0) / 2;
AL = Math.Atan(Math.Sqrt(L1 * L1 - AL * AL) / AL);
if (X0 == 0)
{
T1 = Math.Sign(Y0) * Math.PI / 2;
}
else
{
T1 = Math.Atan(Y0 / X0);
}
T2 = GA - AL;
T3 = GA + AL;
T4 = P0 + R0 + R1 * T1;
T5 = P0 - R0 - R1 * T1;
//return new double[5] { T1, T2, T3, T4, T5 };
T1 = Rad2Deg(T1);
T2 = Rad2Deg(T2);
T3 = Rad2Deg(T3);
T4 = Rad2Deg(T4);
T5 = Rad2Deg(T5);
return(new Joint(T1, T2, T3, T4, 0.0d));
}
