private void CalibrateCameraWithRobot()
{
bool angleFound = false;
for (int i = 0; i < 10; i++)
{
if (App.Current.PolarisController.GetRigidBody(Controls.PolarisCameraController.RigidBodyIndex.Camera).isInRange)
{
angleFound = true;
PolarisCameraController.PolarisRigidBody rBody = App.Current.PolarisController.GetRigidBody(Controls.PolarisCameraController.RigidBodyIndex.Camera);
Vector3D rpyFromQ = App.Current.CoordinateTranslator.QuaternionToRPY(rBody.qx, rBody.qy, rBody.qz, rBody.qo);
radianAngleFound = -rpyFromQ.Y;
radianZAngleFound = -(rpyFromQ.X + 1.571);
}
else
{
Thread.Sleep(1000);
}
}
if (angleFound)
{
double degreeFound = RadianToAngle(radianAngleFound);
double zDegreeFound = RadianToAngle(radianZAngleFound);
App.Current.MachineHomed = true;
App.Current.CoordinateTranslator.SetBaseRotation(radianAngleFound, radianZAngleFound);// zWithBias);
}
else
{
MessageBox.Show("The angle could not be found please check the calibration area and camera setup.",
"Angle not found",
MessageBoxButton.OK,
MessageBoxImage.Error);
}
this.DialogResult = false;
this.Close();
}
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();
}));
}
