//private int isSucces;
public void GeoListener()
{
GeoMagicPose gPose;
//isSucces = startScheduler();
if (startScheduler() == 1)
{
isConnected = true;
shouldStop = false;
while (!shouldStop)
{
if (isAlive() == 1)
{
double[] geoArray = GetCoordinates();
gPose = new GeoMagicPose();
gPose.Xpose = geoArray[3];
gPose.Ypose = geoArray[4];
gPose.Zpose = geoArray[5];
gPose.RXpose = geoArray[6];
gPose.RYpose = geoArray[7];
gPose.RZpose = geoArray[8];
gPose.BtnState = geoArray[12];
if (OnGeoPose != null)
{
OnGeoPose(gPose);
}
//setForces(force.Xforce, force.Zforce, force.Yforce);
//setForces(0, 0, 1);
Thread.Sleep(10);
}
else
{
if (OnException != null)
{
OnException("Der er mistet forbindelse til Geomagic Touch.", "GMT");
}
isConnected = false;
break;
}
}
//if (stopScheduler() == 1)
//{
// isConnected = false;
//}
}
else
{
if (OnException != null)
{
OnException("Der kunne ikke oprettes forbindelse til GeoMagic Touch", "GMT");
}
isConnected = false;
}
}
///// <summary>Calculates the new position to be sent to the robot.</summary>
//private URPose CalculatePose()
//{
// double x = newCenterURPose.Xpose + ((actualGeoPose.Xpose + newCenterGeoPose.Xpose * -1) * -1 / 1000); // Calculates each direction of the position based on the registered center position of the robot and GeoMagicTouch when movement is initiated.
// double y = newCenterURPose.Ypose + ((actualGeoPose.Zpose + newCenterGeoPose.Zpose * -1) / 1000); // Changes in direction of the GeoMagicTouch is calculated by taking the actual position and adding the invers of the center position.
// double z = newCenterURPose.Zpose + ((actualGeoPose.Ypose + newCenterGeoPose.Ypose * -1) / 1000); // The x-direction is inversed to inverse the movement from the GeoMagicTouch to the robot based on observation angle of the robot.
// //double Rx = actualGeoPose.RXpose; // Direct transfor of the rotation angles from the GeoMagicTouch
// //double Ry = actualGeoPose.RYpose; // These values are not used in this version
// //double Rz = actualGeoPose.RZpose;
// double a11 = Math.Cos(actualGeoPose.RXpose * -1) * Math.Cos(actualGeoPose.RYpose);
// double a22 = Math.Cos(actualGeoPose.RZpose) * Math.Cos(actualGeoPose.RYpose) - Math.Sin(actualGeoPose.RZpose) * Math.Sin(actualGeoPose.RXpose * -1) * Math.Sin(actualGeoPose.RYpose);
// double a33 = Math.Cos(actualGeoPose.RZpose) * Math.Cos(actualGeoPose.RXpose * -1);
// double angle = Math.Acos(0.5 * (a11 + a22 + a33 - 1));
// double a32 = Math.Sin(actualGeoPose.RZpose) * Math.Cos(actualGeoPose.RYpose) + Math.Cos(actualGeoPose.RZpose) * Math.Sin(actualGeoPose.RXpose * -1) * Math.Sin(actualGeoPose.RYpose);
// double a23 = -Math.Sin(actualGeoPose.RZpose) * Math.Cos(actualGeoPose.RXpose * -1);
// double e1 = ((a32 - a23) / (2 * Math.Sin(angle))) * angle;
// double a13 = Math.Sin(actualGeoPose.RXpose * -1);
// double a31 = Math.Sin(actualGeoPose.RZpose) * Math.Sin(actualGeoPose.RYpose) - Math.Cos(actualGeoPose.RZpose) * Math.Sin(actualGeoPose.RXpose * -1) * Math.Cos(actualGeoPose.RYpose);
// double e2 = ((a13 - a31) / (2 * Math.Sin(angle))) * angle;
// double a21 = Math.Cos(actualGeoPose.RZpose) * Math.Sin(actualGeoPose.RYpose) + Math.Sin(actualGeoPose.RZpose) * Math.Sin(actualGeoPose.RXpose * -1) * Math.Cos(actualGeoPose.RYpose);
// double a12 = -(Math.Cos(actualGeoPose.RXpose * -1) * Math.Sin(actualGeoPose.RYpose));
// double e3 = ((a21 - a12) / (2 * Math.Sin(angle))) * angle;
// URPose p = new URPose(x, y, z, e1, e2 + 3.14, e3); // Creates a position object based on the different calculated directions.
// return p; // Last three values are fixed to maintain the same position of the robots head.
//}
//private URPose CalculateJoints()
//{
// URPose joints = new URPose();
// joints.RXpose = actualGeoPose.RYpose - 2.3; // Direct transfor of the rotation angles from the GeoMagicTouch
// joints.RYpose = actualGeoPose.RXpose - 1.6; // These values are not used in this version
// joints.RZpose = actualGeoPose.RZpose;
// return joints;
//}
/// <summary>Indicating the movement process can begin.</summary>
private void BtnPressed()
{
if (!isMoving)
{
newCenterURPose = actualURPose; // When the buttons on the GeoMagicTouch is pressed, the actual positions of the
newCenterGeoPose = actualGeoPose; // robot and GeoMagicTouch is registered as new center positions
isMoving = true;
}
}
private void setGeoPose(GeoMagicPose geoPose)
{
actualGeoPose = geoPose;
//updateGeoPoseLabelsEvent(geoPose);
if (geoPose.BtnState == 3)
{
btnPressed();
}
else
{
btnUnPressed();
}
if (isMoving)
{
URPose pose = calculatePose();
m.SendPose(pose);
}
}
/// <summary>Sets the instance of the actual position of the GeoMagicTouch and initializes the movement of the robot.</summary>
/// <param name="geoPose">New position from the GeoMagicTouch.</param>
private void SetGeoPose(GeoMagicPose geoPose)
{
actualGeoPose = geoPose;
MoveRobot();
}
/// <summary>Listener to establish connection to the GeoMagicTouch.
/// If successful it gets current device state and sets force on the GeoMagicTouch.</summary>
/// <param name="are">AutoResetEvent that will be set when the connection is successful.</param>
public void GeoListener(AutoResetEvent are)
{
GeoMagicPose gPose;
if (startScheduler() == 1) // startScheduler() called to connect to GeoMagicTouch.
{
isConnected = true;
are.Set(); // Throws event when connection is successful
shouldStop = false;
while (!shouldStop)
{
if (isAlive() == 1) // Every iteration it checks whether the connection is still alive
{
double[] geoArray = GetCoordinates();
gPose = new GeoMagicPose();
gPose.Xpose = geoArray[0];
gPose.Ypose = geoArray[1];
gPose.Zpose = geoArray[2];
gPose.Joint1pose = geoArray[3];
gPose.Joint2pose = geoArray[4];
gPose.Joint3pose = geoArray[5];
gPose.RXpose = geoArray[6];
gPose.RYpose = geoArray[7];
gPose.RZpose = geoArray[8];
gPose.BtnState = geoArray[12];
if (OnGeoPose != null)
{
OnGeoPose(gPose); // Event thrown to signal the current position of the GeoMagicTouch
}
setForces(force.Xforce, force.Zforce, force.Yforce); // Force set on the GeoMagicTouch based on the values of the local variable
Thread.Sleep(10); // Sleep thread to optain a frequency of 100Hz
}
else
{
if (OnException != null)
{
OnException("Der er mistet forbindelse til Geomagic Touch.", "GMT");
}
isConnected = false;
break;
}
}
}
else
{
if (OnException != null)
{
OnException("Der kunne ikke oprettes forbindelse til GeoMagic Touch", "GMT");
}
isConnected = false;
}
if (shouldStop)
{
if (stopScheduler() == 1)
{
} // stopScheduler called to close connection to the GeoMagicTouch
else
{
if (OnException != null)
{
OnException("GeoMagic Touch kunne ikke stoppes.", "GMT");
}
isConnected = false;
}
}
}