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