protected override void OnDoWork(DoWorkEventArgs e)
{
DateTime start = new DateTime();
DateTime stop = new DateTime();
while (dotracking)
{
start = DateTime.Now;
if (trackForceSensor)
{
if (!errorStateActive && UniversalRobotHelperFunctions.IsForceOverLimit())
{
errorStateActive = true;
Vector3D forceDirection = UniversalRobotHelperFunctions.GetForceDirection();
UniversalRobotController.URRobotCoOrdinate currentRobotCoord = App.Current.URController.GetCurrentLocation();
Point3D currentCamSetPoint = new Point3D(
currentRobotCoord.x,
currentRobotCoord.y,
currentRobotCoord.z);
Vector3D lookDirection = new Vector3D(
currentRobotCoord.qx,
currentRobotCoord.qy,
currentRobotCoord.qz
);
Point3D safePoint = UniversalRobotHelperFunctions.AdjustLocationToForceSensor(currentCamSetPoint, lookDirection, forceDirection);
App.Current.URController.SetVirtualEStopOverride(true, safePoint.X, safePoint.Y, safePoint.Z);
}
}
stop = DateTime.Now;
Thread.Sleep(20);
}
}
public static void TareForceSensor()
{
TareForceReading = UniversalRobotHelperFunctions.GetForceReadout();
}