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(); })); }