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