void UpdateStatusLeds()
{
//UR
URConnected.Fill = App.Current.URController.IsUniversalRobotConnected ? App.Current.ThemeColor : Brushes.Gray;
URReady.Fill = App.Current.URController.URRobotStatus.RobotMode == 7 ? App.Current.ThemeColor : Brushes.Gray;
URDistanceInRange.Fill = !double.IsNaN(UniversalRobotHelperFunctions.GetCurrentZDistance()) ? App.Current.ThemeColor : Brushes.Gray;
URHomed.Fill = App.Current.MachineHomed ? App.Current.ThemeColor : Brushes.Gray;
URFault.Fill = App.Current.URSecondController.GetCurrentSafetyStatus() != URSecondaryController.SafetyType.Normal ? Brushes.Tomato : Brushes.Gray;
URFreedrive.Fill = App.Current.URController.GetFreeDriveStatus() ? App.Current.ThemeColor : Brushes.Gray;
URErrorMessageBox.Text = App.Current.URSecondController.GetCurrentSafetyStatus().ToString();
SettingsOn.Content = App.Current.URController.URRobotStatus.RobotMode == 7 ? "Off" : "On";
double currentDistanceMM = UniversalRobotHelperFunctions.GetCurrentZDistance();
if (double.IsNaN(currentDistanceMM))
{
URDistanceValue.Text = "Out of range.";
}
else
{
double roundedDistance = Math.Round(currentDistanceMM);
URDistanceValue.Text = roundedDistance.ToString() + " mm";
}
//polaris
PolarisConnected.Fill = App.Current.PolarisController.IsPolarisConnected ? App.Current.ThemeColor : Brushes.Gray;
RobotMarkerVisible.Fill = App.Current.PolarisController.GetURRobotRigidBody().isInRange ? App.Current.ThemeColor : Brushes.Gray;
Patientent1MarkerVisible.Fill = App.Current.PolarisController.GetUserRigidBodyOne().isInRange ? App.Current.ThemeColor : Brushes.Gray;
PatientMarker2Visible.Fill = App.Current.PolarisController.GetUserRigidBodyTwo().isInRange ? App.Current.ThemeColor : Brushes.Gray;
//force sensor
ForceOverLimit.Fill = UniversalRobotHelperFunctions.IsForceOverLimit() ? Brushes.Tomato : Brushes.Gray;
TrackButton.IsEnabled = App.Current.MachineHomed;
}
public CalibrationManualWindow()
{
InitializeComponent();
UniversalRobotHelperFunctions.TareForceSensor();
CreateUIUpdateThread();
LoadPositions();
}
private void UiTimer_Tick(object sender, EventArgs e)
{
bool isForceOverLimit = UniversalRobotHelperFunctions.IsForceOverLimit();
if (isForceOverLimit)
{
bool closeWindow = false;
App.Current.URController.SetVirtualEStopOverride(true);
if (MessageBox.Show("The force sensor is over the defined limit! Freedrive has been activated. Freedrive mode will be disable when this dialog closes. Would you like to continue with calibration?",
"Perform Calibration?",
MessageBoxButton.YesNo,
MessageBoxImage.Information) == MessageBoxResult.Yes)
{
UniversalRobotHelperFunctions.TareForceSensor();
}
else
{
closeWindow = true;
}
App.Current.URController.SetVirtualEStopOverride(false);
if (closeWindow)
{
this.Close();
}
}
}
Point3D TrackZAndUpdateStartPoint(Point3D inputPoint, double mmToAddToDistance)
{
double analogConverted = UniversalRobotHelperFunctions.GetCurrentZDistance();
double currentZDistance = coilToTargetDistance + mmToAddToDistance;
if (double.IsNaN(analogConverted) ||
double.IsNaN(currentZDistance))
{
string errorMessage = "Cannot read a valid distance from the laser. Unable to perform move!";
if (CheckRunThroughAll())
{
errorMessage += " The Run Through All moves will now be cancelled.";
}
MessageBox.Show(errorMessage, "Unable to perform move!", MessageBoxButton.OK, MessageBoxImage.Error);
return(new Point3D(
App.Current.URController.URRobotStatus.ToolVectorActual_1,
App.Current.URController.URRobotStatus.ToolVectorActual_2,
App.Current.URController.URRobotStatus.ToolVectorActual_3));
}
if (analogConverted < currentZDistance)
{
startPoint = UniversalRobotHelperFunctions.AdjustLocationToLaser(startPoint, lookDirection, analogConverted, currentZDistance);
}
return(UniversalRobotHelperFunctions.AdjustLocationToLaser(inputPoint, lookDirection, analogConverted, currentZDistance));
}
private void MoveToZ_Click(object sender, RoutedEventArgs e)
{
double analogConverted = UniversalRobotHelperFunctions.GetCurrentZDistance();
double currentZDistance = App.Current.ApplicationSettings.CoilToTargetDistanceMM;
if (double.IsNaN(analogConverted) ||
double.IsNaN(currentZDistance))
{
MessageBox.Show("Cannot read a valid distance from the laser. Unable to perform move!", "Unable to perform move!", MessageBoxButton.OK, MessageBoxImage.Error);
}
else
{
Point3D move = UniversalRobotHelperFunctions.AdjustLocationToLaser(GetCurrentPoint(), GetCurrentLookDirection(), analogConverted, currentZDistance);
MoveToPoint(move, GetCurrentLookDirectionAsPoint(), false);
}
}
Point3D TrackZ(Point3D inputPoint, double mmToAddToDistance)
{
double analogConverted = UniversalRobotHelperFunctions.GetCurrentZDistance();
double currentZDistance = coilToTargetDistance + mmToAddToDistance;
if (double.IsNaN(analogConverted) ||
double.IsNaN(currentZDistance))
{
MessageBox.Show("Cannot read a valid distance from the laser. Unable to perform move!", "Unable to perform move!", MessageBoxButton.OK, MessageBoxImage.Error);
return(new Point3D(
App.Current.URController.URRobotStatus.ToolVectorActual_1,
App.Current.URController.URRobotStatus.ToolVectorActual_2,
App.Current.URController.URRobotStatus.ToolVectorActual_3));
}
return(UniversalRobotHelperFunctions.AdjustLocationToLaser(inputPoint, lookDirection, analogConverted, currentZDistance));
}
public CalibrateWindow()
{
InitializeComponent();
coilToTargetDistance = App.Current.ApplicationSettings.CoilToTargetDistanceMM;
ZeroOutReponses();
UniversalRobotHelperFunctions.TareForceSensor();
CreateUIUpdateThread();
UpdateLabels(1, App.Current.ApplicationSettings.TherapyPassOneJumpMM);
SetStartingPoint();
if (App.Current.ApplicationSettings.TrackTOFSensor)
{
string returnValue = Microsoft.VisualBasic.Interaction.InputBox("Enter the Coil to Target distance", "Coil to Target distance entry", App.Current.ApplicationSettings.CoilToTargetDistanceMM.ToString());
try
{
coilToTargetDistance = Convert.ToDouble(returnValue);
if (coilToTargetDistance <= 0)
{
coilToTargetDistance = App.Current.ApplicationSettings.CoilToTargetDistanceMM;
}
}
catch { }
if (MessageBox.Show("The calibration will now begin and the tool will move to a Coil to Target distance of " + coilToTargetDistance + "mm."
+ "\nWould you like to continue with the calibration?",
"Perform Calibration?",
MessageBoxButton.YesNo,
MessageBoxImage.Question)
!= MessageBoxResult.Yes)
{
doCalibration = false;
}
else
{
MoveDownToZAndSetPoint(startPoint);
}
}
else
{
checkBoxTrackZ.IsEnabled = false;
initialPoint = startPoint;
}
}
private void UiTimer_Tick(object sender, EventArgs e)
{
this.Dispatcher.BeginInvoke(DispatcherPriority.Normal, (Action)(() =>
{
UpdateStatusLeds();
TextBoxX.Text = App.Current.URController.URRobotStatus.ToolVectorActual_1.ToString();
TextBoxY.Text = App.Current.URController.URRobotStatus.ToolVectorActual_2.ToString();
TextBoxZ.Text = App.Current.URController.URRobotStatus.ToolVectorActual_3.ToString();
//TextBoxQX.Text = App.Current.URController.URRobotStatus.ToolVectorActual_4.ToString();
//TextBoxQY.Text = App.Current.URController.URRobotStatus.ToolVectorActual_5.ToString();
//TextBoxQZ.Text = App.Current.URController.URRobotStatus.ToolVectorActual_6.ToString();
Vector3D angle = new Vector3D(App.Current.URController.URRobotStatus.ToolVectorActual_4,
App.Current.URController.URRobotStatus.ToolVectorActual_5,
App.Current.URController.URRobotStatus.ToolVectorActual_6);
Vector3D rpy = App.Current.CoordinateTranslator.ToRollPitchYawV2(angle);
TextBoxQX.Text = radtoang(rpy.X).ToString();
TextBoxQY.Text = radtoang(rpy.Y).ToString();
TextBoxQZ.Text = radtoang(rpy.Z).ToString();
LabelStatus.Content = App.Current.URController.IsUniversalRobotConnected ? "Connected" : "Connecting...";
LabelStatus.Foreground = App.Current.URController.IsUniversalRobotConnected ? App.Current.ThemeColor : Brushes.Tomato;
RobotStatusLabel.Content = App.Current.URController.IsUniversalRobotConnected ?
UniversalRobotHelperFunctions.RobotModeToString(App.Current.URController.URRobotStatus.RobotMode)
:
"...";
//Polaris information
PolarisLabelStatus.Content = App.Current.PolarisController.IsPolarisConnected ? "Connected" : "Connecting...";
PolarisLabelStatus.Foreground = App.Current.PolarisController.IsPolarisConnected ? App.Current.ThemeColor : Brushes.Tomato;
PolarisCameraController.PolarisRigidBody rBody = new PolarisCameraController.PolarisRigidBody();
if (ComboBoxPolarisSelection.SelectedIndex == 0)
{
rBody = App.Current.PolarisController.GetURRobotRigidBody();
}
else if (ComboBoxPolarisSelection.SelectedIndex == 1)
{
rBody = App.Current.PolarisController.GetUserRigidBodyOne();
}
else if (ComboBoxPolarisSelection.SelectedIndex == 2)
{
rBody = App.Current.PolarisController.GetUserRigidBodyTwo();
}
PolarisX.Text = rBody.x.ToString();
PolarisY.Text = rBody.y.ToString();
PolarisZ.Text = rBody.z.ToString();
//PolarisQX.Text = rBody.qx.ToString();
//PolarisQY.Text = rBody.qy.ToString();
//PolarisQZ.Text = rBody.qz.ToString();
//PolarisQO.Text = rBody.qo.ToString();
Vector3D rpyFromQ = App.Current.CoordinateTranslator.QuaternionToRPY(rBody.qx,
rBody.qy, rBody.qz, rBody.qo);
// PolarisQX.Text = radtoang(rpyFromQ.X).ToString();
//PolarisQY.Text = radtoang(rpyFromQ.Y).ToString();
//PolarisQZ.Text = radtoang(rpyFromQ.Z).ToString();
PolarisQX.Text = (rpyFromQ.X).ToString();
PolarisQY.Text = (rpyFromQ.Y).ToString();
PolarisQZ.Text = (rpyFromQ.Z).ToString();
AnalogRead.Text = UniversalRobotHelperFunctions.ConvertAnalogReaderToMM(App.Current.URSecondController.GetAnalogValue()).ToString();
safetyStatus.Text = App.Current.URSecondController.GetCurrentSafetyStatus().ToString();
}));
}
private void TareForce_Click(object sender, RoutedEventArgs e)
{
UniversalRobotHelperFunctions.TareForceSensor();
}
