/// <summary>
/// 读入Rapid中robtarget数据至ListView
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void readPointsBtn_Click(object sender, EventArgs e)
{
if (abbtask == null)
{
Data.LogString = "[error] abbtask is null";
return;
}
//获得RobTarget数据
rd_p1 = (RobTarget)abbtask.GetRapidData("MainModule", "p1").Value;
rd_p2 = (RobTarget)abbtask.GetRapidData("MainModule", "p2").Value;
rd_p3 = (RobTarget)abbtask.GetRapidData("MainModule", "p3").Value;
rd_p4 = (RobTarget)abbtask.GetRapidData("MainModule", "p4").Value;
string[] rd_p_strings = new string[] { rd_p1.ToString(), rd_p2.ToString(), rd_p3.ToString(), rd_p4.ToString() };
string[] points = new string[] { "p1", "p2", "p3", "p4" };
ListViewItem item = null;
this.pointslistView.BeginUpdate();
for (int i = 0; i < points.Length; i++)
{
item = new ListViewItem(points[i]);
item.SubItems.Add(rd_p_strings[i]);
this.pointslistView.Items.Add(item); //添加元素
}
this.pointslistView.EndUpdate(); //更新控件
Data.LogString = "坐标点数据读入完毕`";
}
/// <summary>
/// 获取机器人当前位置。姿态使用XYZ欧拉角表示
/// </summary>
/// <param name="arr"></param>
/// <returns></returns>
public int getCurrentLoc(out double[] arr)
{
int ret = 0;
arr = new double[6];
if (controller == null)
{
ret = (int)ErrorCode.err_no_controller;
return(ret);
}
controller.Logon(UserInfo.DefaultUser);
ABB.Robotics.Controllers.RapidDomain.Task motion_task = controller.Rapid.GetTask("T_ROB1");
if (motion_task == null)
{
ret = (int)ErrorCode.err_no_such_task;
return(ret);
}
RobTarget current_loc = motion_task.GetRobTarget();
arr[0] = current_loc.Trans.X;
arr[1] = current_loc.Trans.Y;
arr[2] = current_loc.Trans.Z;
current_loc.Rot.ToEulerAngles(out arr[3], out arr[4], out arr[5]);
return(ret);
}
public ExternalAxes GetCurrentExternalAxes()
{
if (!_isConnected)
{
logger.Debug("Cannot GetCurrentExternalAxes: not connected to controller");
return(null);
}
RobTarget rt = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(ABB.Robotics.Controllers.MotionDomain.CoordinateSystemType.World);
return(new ExternalAxes(rt.Extax.Eax_a, rt.Extax.Eax_b, rt.Extax.Eax_c, rt.Extax.Eax_d, rt.Extax.Eax_e, rt.Extax.Eax_f));
}
/// <summary>
/// 写入P4坐标点至控制器
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void button_p4_Click(object sender, EventArgs e)
{
using (Mastership mr = Mastership.Request(this.abbcontroller.Rapid))
{
//Reading Robtarget
rd_p4 = (RobTarget)this.abbcontroller.Rapid.GetRapidData("T_ROB1", "MainModule", "P4").Value;
//Writing Robtarget
rd_p4.Trans.FillFromString2(this.textBox_p4.Text);
this.abbcontroller.Rapid.GetRapidData("T_ROB1", "MainModule", "P4").Value = rd_p4;
}
Data.LogString = "[msg] P4写入执行完毕~";
}
///// <summary>
///// Loads a module into de controller from a local file.
///// @TODO: This is an expensive operation, should probably become threaded.
///// </summary>
///// <param name="fullPath"></param>
///// <param name="wipeout"></param>
///// <returns></returns>
//public bool LoadFileToDevice(string fullPath, bool wipeout = true)
//{
// string extension = Path.GetExtension(fullPath), // ".mod"
// filename = Path.GetFileName(fullPath); // "Machina_Server.mod"
// if (!_isConnected)
// {
// throw new Exception($"Could not load module '{fullPath}', not connected to controller");
// }
// // check for correct ABB file extension
// if (!extension.ToLower().Equals(".mod"))
// {
// throw new Exception("Wrong file type, must use .mod files for ABB robots");
// }
// // For the time being, we will always wipe out previous modules on load
// if (ClearAllModules() < 0)
// {
// throw new Exception("Error clearing modules");
// }
// // Load the module
// bool success = false;
// try
// {
// using (Mastership.Request(controller.Rapid))
// {
// // When connecting to a real controller, the reference filesystem
// // for Task.LoadModuleFromFile() becomes the controller's, so it is necessary
// // to copy the file to the system first, and then load it.
// // Create the remoteBufferDirectory if applicable
// FileSystem fs = controller.FileSystem;
// string remotePath = fs.RemoteDirectory + "/" + REMOTE_BUFFER_DIR;
// bool dirExists = fs.DirectoryExists(REMOTE_BUFFER_DIR);
// if (!dirExists)
// {
// logger.Debug($"Creating {remotePath} on remote controller");
// fs.CreateDirectory(REMOTE_BUFFER_DIR);
// }
// //@TODO: Should implement some kind of file cleanup at somepoint...
// // Copy the file to the remote controller
// controller.FileSystem.PutFile(fullPath, $"{REMOTE_BUFFER_DIR}/{filename}", wipeout);
// logger.Debug($"Copied {filename} to {REMOTE_BUFFER_DIR}");
// // Loads a Rapid module to the task in the robot controller
// success = tRob1Task.LoadModuleFromFile($"{remotePath}/{filename}", RapidLoadMode.Replace);
// }
// }
// catch (Exception ex)
// {
// //Console.WriteLine("ERROR: Could not load module");
// logger.Debug(ex);
// throw new Exception("ERROR: Could not load module");
// }
// // True if loading succeeds without any errors, otherwise false.
// if (!success)
// {
// //// Gets the available categories of the EventLog.
// //foreach (EventLogCategory category in controller.EventLog.GetCategories())
// //{
// // if (category.Name == "Common")
// // {
// // if (category.Messages.Count > 0)
// // {
// // foreach (EventLogMessage message in category.Messages)
// // {
// // Console.WriteLine("Program [{1}:{2}({0})] {3} {4}",
// // message.Name, message.SequenceNumber,
// // message.Timestamp, message.Title, message.Body);
// // }
// // }
// // }
// //}
// }
// else
// {
// logger.Debug($"Sucessfully loaded {fullPath}");
// }
// return success;
//}
/// <summary>
/// Returns a Vector object representing the current robot's TCP position.
/// </summary>
/// <returns></returns>
public Vector GetCurrentPosition()
{
if (!_isConnected)
{
logger.Debug("Cannot GetCurrentPosition: not connected to controller");
return(null);
}
RobTarget rt = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(ABB.Robotics.Controllers.MotionDomain.CoordinateSystemType.World);
return(new Vector(rt.Trans.X, rt.Trans.Y, rt.Trans.Z));
}
/// <summary>
/// 定时器更新机器人状态信息
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void timer1_Tick(object sender, EventArgs e)
{
if (Timer_ON)
{
if (this.comboBox2.Text.Contains("工件坐标"))
{
gripper_Loc = abbcontroller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);//读取当前坐标系
}
else if (this.comboBox2.Text.Contains("基坐标"))
{
gripper_Loc = abbcontroller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.Base);//读取当前坐标系
}
else if (this.comboBox2.Text.Contains("世界坐标"))
{
gripper_Loc = abbcontroller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.World);//读取当前坐标系
}
this.x_label.Text = gripper_Loc.Trans.X.ToString();
this.y_label.Text = gripper_Loc.Trans.Y.ToString();
this.z_label.Text = gripper_Loc.Trans.Z.ToString();
this.q1_label.Text = gripper_Loc.Rot.Q1.ToString();
this.q2_label.Text = gripper_Loc.Rot.Q2.ToString();
this.q3_label.Text = gripper_Loc.Rot.Q3.ToString();
this.q4_label.Text = gripper_Loc.Rot.Q4.ToString();
speedData = abbcontroller.Rapid.GetRapidData("T_ROB1", "MainModule", "speeddef");
string[] speedstr = speedData.Value.ToString().Split('[', ',', ']'); //分割字符串
this.speedlabel.Text = speedstr[1] + "mm/s " + this.abbcontroller.MotionSystem.SpeedRatio.ToString() + "%";
this.cyclelabel.Text = this.abbcontroller.Rapid.Cycle.ToString();
this.modelabel.Text = this.abbcontroller.OperatingMode.ToString();
this.motorStatelabel.Text = this.abbcontroller.State.ToString();
if (this.motorStatelabel.Text == "MotorsOn")
{
this.motorStatelabel.ForeColor = Color.Green;
}
else
{
this.motorStatelabel.ForeColor = Color.Red;
}
if (this.modelabel.Text == "Auto")
{
this.modelabel.ForeColor = Color.Green;
}
else
{
this.modelabel.ForeColor = Color.Red;
}
}
else
{
}
}
private void button_Read_Click(object sender, EventArgs e)
{
#if ROBOT_EXIST
ControllerInfo controllerInfo = main_form.controllers[0]; // 我们很自信，就一个控制器，以后有多个的话，这里肯定需要修改
Controller ctrl = ControllerFactory.CreateFrom(controllerInfo);
ctrl.Logon(UserInfo.DefaultUser);
tRob1 = ctrl.Rapid.GetTask("T_ROB1");
#endif
string module_name = comboBox_Module.SelectedItem.ToString();
string var_type = comboBox_Var.SelectedItem.ToString();
int var_type_no = comboBox_Var.SelectedIndex;
string var_name = textBox_varName.Text;
#if ROBOT_EXIST
string str_read_res = "";
switch (var_type)
{
case "Num":
RD_time = tRob1.GetRapidData(module_name, var_name);
ABB_Num_time = (Num)RD_time.Value;
str_read_res = ABB_Num_time.ToString();
break;
case "RobTarget":
RD_rt = tRob1.GetRapidData(module_name, var_name);
ABB_RT_rt = (RobTarget)RD_rt.Value;
str_read_res = ABB_RT_rt.ToString();
break;
default:
break;
}
textBox1_Result.Text = str_read_res;
textBox1_Result.Update();
ctrl.Logoff();
ctrl.Dispose();
#else
switch (var_type)
{
case "Num":
textBox1_Result.Text = "Num";
break;
case "RobTarget":
textBox1_Result.Text = "RobTarget";
break;
default:
break;
}
textBox1_Result.Update();
#endif
}
/// <summary>
/// Returns a Rotation object representing the current robot's TCP orientation.
/// </summary>
/// <returns></returns>
public Rotation GetCurrentOrientation()
{
if (!_isConnected)
{
logger.Debug("Cannot GetCurrentRotation, not connected to controller");
return(null);
}
RobTarget rt = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(ABB.Robotics.Controllers.MotionDomain.CoordinateSystemType.World);
// ABB's convention is Q1..Q4 as W..Z
return(Rotation.FromQuaternion(rt.Rot.Q1, rt.Rot.Q2, rt.Rot.Q3, rt.Rot.Q4));
}
/// <summary>
/// 设置点位
/// </summary>
/// <param name="task_name"></param>
/// <param name="module_name"></param>
/// <param name="var_name"></param>
/// <param name="value"></param>
/// <returns></returns>
public int setValue(string task_name, string module_name, string var_name, double[] value)
{
int ret = 0;
RapidData rapid_data;
if (controller == null)
{
ret = (int)ErrorCode.err_no_controller;
return(ret);
}
if (value.Length != 6)
{
ret = (int)ErrorCode.err_illegal_array;
return(ret);
}
controller.Logon(UserInfo.DefaultUser);
try
{
rapid_data = controller.Rapid.GetRapidData(task_name, module_name, var_name);
}
catch (ABB.Robotics.GenericControllerException)
{
ret = (int)ErrorCode.err_no_such_var;
return(ret);
}
try
{
if (!(rapid_data.Value is RobTarget))
{
ret = (int)ErrorCode.err_incompatible_var;
return(ret);
}
RobTarget target_loc = (RobTarget)rapid_data.Value;
target_loc.Trans.X = (float)value[0];
target_loc.Trans.Y = (float)value[1];
target_loc.Trans.Z = (float)value[2];
target_loc.Rot.FillFromEulerAngles(value[3], value[4], value[5]);
using (Mastership.Request(controller.Rapid))
{
rapid_data.Value = target_loc;
}
}
catch (ABB.Robotics.GenericControllerException)
{
ret = (int)ErrorCode.err_get_mastership_failed;
return(ret);
}
return(ret);
}
private void showRobotPosition()
{
while (true)
{
try
{
JointTarget currentJointPoint = this.controller.MotionSystem.ActiveMechanicalUnit.GetPosition();
tbJ1.Text = Math.Round(currentJointPoint.RobAx.Rax_1, 2).ToString();
tbJ2.Text = Math.Round(currentJointPoint.RobAx.Rax_2, 2).ToString();
tbJ3.Text = Math.Round(currentJointPoint.RobAx.Rax_3, 2).ToString();
tbJ4.Text = Math.Round(currentJointPoint.RobAx.Rax_4, 2).ToString();
tbJ5.Text = Math.Round(currentJointPoint.RobAx.Rax_5, 2).ToString();
tbJ6.Text = Math.Round(currentJointPoint.RobAx.Rax_6, 2).ToString();
RobTarget currentCartesianPoint = this.controller.MotionSystem.ActiveMechanicalUnit.GetPosition(ABB.Robotics.Controllers.MotionDomain.CoordinateSystemType.World);
tbX.Text = Math.Round(currentCartesianPoint.Trans.X, 2).ToString();
tbY.Text = Math.Round(currentCartesianPoint.Trans.Y, 2).ToString();
tbZ.Text = Math.Round(currentCartesianPoint.Trans.Z, 2).ToString();
double q0, q1, q2, q3;
//double Rz,Ry,Rx;
q0 = currentCartesianPoint.Rot.Q1;
q1 = currentCartesianPoint.Rot.Q2;
q2 = currentCartesianPoint.Rot.Q3;
q3 = currentCartesianPoint.Rot.Q4;
//Rz = Math.Atan(2 * (q1 * q2 - q0 * q3) / (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3));
//Ry = Math.Asin(-2*(q0*q2+q1*q3));
//Rx = Math.Atan(2 * (q2 * q3 - q0 * q1) / (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3));
//tbQ1.Text = Rx.ToString();
//tbQ2.Text = Ry.ToString();
//tbQ3.Text = Rz.ToString();
tbQ1.Text = Math.Round(q0, 2).ToString();
tbQ2.Text = Math.Round(q1, 2).ToString();
tbQ3.Text = Math.Round(q2, 2).ToString();
tbQ4.Text = Math.Round(q3, 2).ToString();
}
catch
{ }
}
}
public void TestRobtargetConstructor()
{
RobTarget robTarget = new RobTarget("Target_11", 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0);
var expt_pos_x = 1.0;
var expt_pos_y = 2.0;
var expt_pos_z = 3.0;
var expt_orient_q1 = 4.0;
var expt_orient_q2 = 5.0;
var expt_orient_q3 = 6.0;
var expt_orient_q4 = 7.0;
Assert.AreEqual(expt_pos_x, robTarget.pos.x);
Assert.AreEqual(expt_pos_y, robTarget.pos.y);
Assert.AreEqual(expt_pos_z, robTarget.pos.z);
Assert.AreEqual(expt_orient_q1, robTarget.orient.q1);
Assert.AreEqual(expt_orient_q2, robTarget.orient.q2);
Assert.AreEqual(expt_orient_q3, robTarget.orient.q3);
Assert.AreEqual(expt_orient_q4, robTarget.orient.q4);
Assert.IsInstanceOfType(robTarget, typeof(RobTarget));
}
public MoveJ(bool isConc, RobTarget robTarget, bool hasMultiMoveID, int iD, int speed, bool hasV, int v, bool hasT, int t, int zone, bool hasZ, int z, bool hasInpos, string inpos, string tool, bool hasWobj, string wobj, bool hasTload, string tload)
{
this.isConc = isConc;
ToPoint = robTarget;
this.hasMultiMoveID = hasMultiMoveID;
ID = iD;
Speed = speed;
this.hasV = hasV;
V = v;
this.hasT = hasT;
T = t;
Zone = zone;
this.hasZ = hasZ;
Z = z;
this.hasInpos = hasInpos;
Inpos = inpos;
this.tool = tool;
this.hasWobj = hasWobj;
Wobj = wobj;
this.hasTload = hasTload;
Tload = tload;
}
public MoveJ()
{
this.isConc = false;
ToPoint = new RobTarget();
this.hasMultiMoveID = false;
ID = 0;
Speed = 0;
this.hasV = false;
V = 0;
this.hasT = false;
T = 0;
Zone = 0;
this.hasZ = false;
Z = 0;
this.hasInpos = false;
Inpos = "inpos";
this.tool = "tool0";
this.hasWobj = false;
Wobj = "Wobjcnv1";
this.hasTload = false;
Tload = "tload";
}
/// <summary>
/// 获取指定点位变量
/// </summary>
/// <param name="task_name"></param>
/// <param name="module_name"></param>
/// <param name="var_name"></param>
/// <param name="value"></param>
/// <returns></returns>
public int getValue(string task_name, string module_name, string var_name, out double[] value)
{
int ret = 0;
RapidData rapid_data;
value = new double[6];
if (controller == null)
{
ret = (int)ErrorCode.err_no_controller;
return(ret);
}
controller.Logon(UserInfo.DefaultUser);
try
{
rapid_data = controller.Rapid.GetRapidData(task_name, module_name, var_name);
}
catch (ABB.Robotics.GenericControllerException)
{
ret = (int)ErrorCode.err_no_such_var;
return(ret);
}
if (rapid_data.Value is RobTarget)
{
RobTarget rapid_target = (RobTarget)rapid_data.Value;
value[0] = rapid_target.Trans.X;
value[1] = rapid_target.Trans.Y;
value[2] = rapid_target.Trans.Z;
rapid_target.Rot.ToEulerAngles(out value[3], out value[4], out value[5]);
}
else
{
ret = (int)ErrorCode.err_incompatible_var;
return(ret);
}
return(ret);
}
/// <summary>
/// Create robot target from point and list of quaternion values.
/// </summary>
/// <param name="point">Point</param>
/// <param name="quatList">List of quaternions</param>
/// <returns></returns>
public static RobTarget RobTargetAtPoint1(Point point, List<double> quatList)
{
var target = new RobTarget();
if (point != null)
{
target.FillFromString2(
string.Format(
"[[{0},{1},{2}],[{3},{4},{5},{6}],[0,0,0,0],[9.999999999E09,9.999999999E09,9.999999999E09,9.999999999E09,9.999999999E09,9.999999999E09]];",
point.X, point.Y, point.Z, quatList[0], quatList[1], quatList[2], quatList[3]));
}
return target;
}
/// <summary>
/// Create robot target from point and quaternion values.
/// </summary>
/// <param name="point">Point</param>
/// <param name="q1">Quaternion</param>
/// <param name="q2">Quaternion</param>
/// <param name="q3">Quaternion</param>
/// <param name="q4">Quaternion</param>
/// <returns></returns>
public static RobTarget RobTargetAtPoint0(Point point, double q1, double q2, double q3, double q4)
{
var target = new RobTarget();
if (point != null)
{
target.FillFromString2(
string.Format(
"[[{0},{1},{2}],[{3},{4},{5},{6}],[0,0,0,0],[9.999999999E09,9.999999999E09,9.999999999E09,9.999999999E09,9.999999999E09,9.999999999E09]];",
point.X, point.Y, point.Z, q1, q2, q3, q4));
}
return target;
}
/// <summary>
/// Create robot target from plane.
/// </summary>
/// <param name="plane">Plane</param>
/// <returns></returns>
public static RobTarget RobTargetAtPlane(Plane plane)
{
var target = new RobTarget();
if (plane != null)
{
List<double> quatDoubles = RobotUtils.PlaneToQuaternian(plane);
target.FillFromString2(
string.Format(
"[[{0},{1},{2}],[{3},{4},{5},{6}],[0,0,0,0],[9.999999999E09,9.999999999E09,9.999999999E09,9.999999999E09,9.999999999E09,9.999999999E09]];",
plane.Origin.X, plane.Origin.Y, plane.Origin.Z, quatDoubles[0], quatDoubles[1], quatDoubles[2], quatDoubles[3]));
}
return target;
}
/// <summary>
/// Send robot target to controller.
/// </summary>
/// <param name="moduleName">Name of module</param>
/// <param name="targetName">Name of target</param>
/// <param name="targetValue">New value for target</param>
public static void sendRobTargetToController(string moduleName, string targetName, string targetValue)
{
try
{
var scanner = new NetworkScanner();
scanner.Scan();
ControllerInfoCollection controllers = scanner.Controllers;
using (var controller = ControllerFactory.CreateFrom(controllers[0]))
{
controller.Logon(UserInfo.DefaultUser);
using (Mastership.Request(controller.Rapid))
{
if (controller.OperatingMode == ControllerOperatingMode.Auto)
{
using (var task = controller.Rapid.GetTask("T_ROB1"))
{
var target = new RobTarget();
using (var rapidData = task.GetRapidData(moduleName, targetName))
{
if (rapidData.Value is RobTarget)
{
target.FillFromString2(targetValue);
rapidData.Value = target;
}
}
var result = task.Start(true);
Debug.WriteLine(result.ToString());
task.ResetProgramPointer();
}
}
else
{
Debug.WriteLine("Automatic mode is required to start execution from a remote client.");
}
}
controller.Logoff();
}
}
catch (Exception ex)
{
Debug.WriteLine(ex.Message);
Debug.WriteLine(ex.StackTrace);
}
}
public static Dictionary <string, string> createWeldRoutine0(string filePath, [DefaultArgumentAttribute("{Plane.ByOriginNormal(Point.ByCoordinates(0,0,0),Vector.ByCoordinates(0,0,-1))}")] List <Plane> planes, double waitTime = 2.9, double weldTime = 0.9, double retract = -10)
{
// setup
var targBuilder = new StringBuilder();
int ct = planes.Count;
foreach (Plane plane in planes)
{
RobTarget targ = Dynamo_TORO.DataTypes.RobTargetAtPlane(plane);
string targ2 = "\n\t\t" + "[" + targ.ToString() + "],";
if (plane == planes[planes.Count - 1])
{
targ2 = "\n\t\t" + "[" + targ.ToString() + "]";
}
targBuilder.Append(targ2);
}
// create rapid
string r = "";
using (var tw = new StreamWriter(filePath, false))
{
r = "MODULE MainModule" +
"\n" +
"\n\t" + "! variables" +
"\n\t" + "CONST num wt:=" + waitTime + ";" +
"\n\t" + "CONST num wd:=" + weldTime + ";" +
"\n\t" + "CONST num dz:=" + retract + ";" +
"\n" +
"\n\t" + "! targets" +
"\n\t" + "VAR robtarget targets{" + ct + ",1}:=" +
"\n\t" + "[" + targBuilder.ToString() +
"\n\t" + "];" +
"\n" +
"\n\t" + "! weld routine" +
"\n\t" + "PROC wStart(robtarget target)" +
"\n\t\t" + "MoveL RelTool(target,0,0,dz),v300,z5,t_welder\\WObj:=w_plate;" +
"\n\t\t" + "MoveL target,v300,fine,t_welder\\WObj:=w_plate;" +
"\n\t\t" + "SetDO welder,1;" +
"\n\t\t" + "WaitTime\\InPos,wd;" +
"\n\t\t" + "SetDO welder,0;" +
"\n\t\t" + "MoveL RelTool(target,0,0,dz),v300,z5,t_welder\\WObj:=w_plate;" +
"\n\t\t" + "WaitTime\\InPos,wt;" +
"\n\t" + "ENDPROC\n" +
"\n\t" + "! main routine" +
"\n\t" + "PROC main()" +
"\n\t\t" + "ConfL\\On;" +
"\n\t\t" + "SingArea\\Wrist;" +
"\n\t\t" + "FOR i FROM 1 TO " + ct + " DO" +
"\n\t\t\t" + "TPWrite(valtostr(i))" + " + \" of " + ct + " \" + \"(\" + valtostr(100*i/" + ct + ") + \"%)\";" +
"\n\t\t\t" + "wStart(targets{i,1});" +
"\n\t\t" + "ENDFOR" +
"\n\t\t" + "Stop;" +
"\n\t" + "ENDPROC" +
"\n" +
"\n" + "ENDMODULE"
;
tw.Write(r);
tw.Flush();
}
// end step
return(new Dictionary <string, string>
{
{ "filePath", filePath },
{ "robotCode", r }
});
}
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
/// <summary>
/// Create robot target from coordinate and quaternion values.
/// </summary>
/// <param name="ptX">Coordinate</param>
/// <param name="ptY">Coordinate</param>
/// <param name="ptZ">Coordinate</param>
/// <param name="q1">Quaternion</param>
/// <param name="q2">Quaternion</param>
/// <param name="q3">Quaternion</param>
/// <param name="q4">Quaternion</param>
/// <returns></returns>
public static RobTarget RobTargetAtVals(double ptX, double ptY, double ptZ, double q1, double q2, double q3, double q4)
{
var target = new RobTarget();
{
target.FillFromString2(
string.Format(
"[[{0},{1},{2}],[{3},{4},{5},{6}],[0,0,0,0],[9.999999999E09,9.999999999E09,9.999999999E09,9.999999999E09,9.999999999E09,9.999999999E09]];",
ptX, ptY, ptZ, q1, q2, q3, q4));
}
return target;
}
private void LinearJogging(string Direction, float OffsJointValue)
{
RobTarget CurrentCartesianPoint = this.controller.MotionSystem.ActiveMechanicalUnit.GetPosition(ABB.Robotics.Controllers.MotionDomain.CoordinateSystemType.World);
double q0, q1, q2, q3;
//double Rz, Ry, Rx;
q0 = CurrentCartesianPoint.Rot.Q1;
q1 = CurrentCartesianPoint.Rot.Q2;
q2 = CurrentCartesianPoint.Rot.Q3;
q3 = CurrentCartesianPoint.Rot.Q4;
//Rz = Math.Atan(2 * (q1 * q2 - q0 * q3) / (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3));
//Ry = Math.Asin(-2 * (q0 * q2 + q1 * q3));
//Rx = Math.Atan(2 * (q2 * q3 - q0 * q1) / (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3));
switch (Direction)
{
case "X":
CurrentCartesianPoint.Trans.X += OffsJointValue;
break;
case "Y":
CurrentCartesianPoint.Trans.Y += OffsJointValue;
break;
case "Z":
CurrentCartesianPoint.Trans.Z += OffsJointValue;
break;
case "Q1":
CurrentCartesianPoint.Rot.Q1 += OffsJointValue;
break;
case "Q2":
CurrentCartesianPoint.Rot.Q2 += OffsJointValue;
break;
case "Q3":
CurrentCartesianPoint.Rot.Q3 += OffsJointValue;
break;
case "Q4":
CurrentCartesianPoint.Rot.Q4 += OffsJointValue;
break;
//case "Rx":
// Rx += OffsJointValue;
// break;
//case "Ry":
// Ry += OffsJointValue;
// break;
//case "Rz":
// Rz += OffsJointValue;
// break;
}
//CurrentCartesianPoint.Rot.Q1 = Math.Cos(Rz / 2) * Math.Cos(Ry / 2) * Math.Cos(Rx / 2) - Math.Sin(Rz / 2) * Math.Sin(Ry / 2) * Math.Sin(Rx / 2);
//CurrentCartesianPoint.Rot.Q2 = Math.Cos(Rz / 2) * Math.Cos(Ry / 2) * Math.Sin(Rx / 2) + Math.Sin(Rz / 2) * Math.Sin(Ry / 2) * Math.Cos(Rx / 2);
//CurrentCartesianPoint.Rot.Q3 = Math.Cos(Rz / 2) * Math.Sin(Ry / 2) * Math.Cos(Rx / 2) - Math.Sin(Rz / 2) * Math.Cos(Ry / 2) * Math.Sin(Rx / 2);
//CurrentCartesianPoint.Rot.Q4 = Math.Sin(Rz / 2) * Math.Cos(Ry / 2) * Math.Cos(Rx / 2) + Math.Cos(Rz / 2) * Math.Sin(Ry / 2) * Math.Sin(Rx / 2);
try
{
using (this.master = Mastership.Request(controller.Rapid))
{
RapidData rd = controller.Rapid.GetRapidData("T_ROB1", "Module1", "pTarget");
controller.Rapid.GetTask("T_ROB1").SetProgramPointer("Module1", "MoveLinear");
rd.Value = CurrentCartesianPoint;
controller.Rapid.Start();
}
if (this.controller.Rapid.ExecutionStatus != ExecutionStatus.Running)
{
master.Release();
}
}
catch (System.InvalidOperationException ex)
{
MessageBox.Show("Mastership is held by another client." + ex.Message);
master.Release();
master = Mastership.Request(controller);
}
catch (System.Exception ex)
{
MessageBox.Show("Unexpected error occurred: " + ex.Message);
}
}
/// <summary>
/// This is the method that actually does the work.
/// </summary>
/// <param name="DA">The DA object is used to retrieve from inputs and store in outputs.</param>
protected override void SolveInstance(IGH_DataAccess DA)
{
// Creates the input value list and attachs it to the input parameter
CreateValueList();
// Expire solution of this component (called after expire solution of the created value list)
if (_expire == true)
{
_expire = false;
this.ExpireSolution(true);
}
// Input variables
GH_Controller controllerGoo = null;
int coordinateSystem = 0;
// Catch input data
if (!DA.GetData(0, ref controllerGoo))
{
return;
}
if (!DA.GetData(1, ref coordinateSystem))
{
return;
}
// Output variables
List <Plane> planes = new List <Plane>();
// Get the mechanical units of the controller
MechanicalUnitCollection mechanicalUnits = controllerGoo.Value.MotionSystem.MechanicalUnits;
List <string> mechanicalUnitnames = new List <string>();
// ABB coordinate system type
CoordinateSystemType coord = (CoordinateSystemType)coordinateSystem;
// Get all the planes
for (int i = 0; i < mechanicalUnits.Count; i++)
{
// Try to get the plane
try
{
// Get the ABB joint target of the mechanical unit
RobTarget robTarget = mechanicalUnits[i].GetPosition(coord);
int test = mechanicalUnits[i].DriveModule;
// Update Quaternion
_quat.A = robTarget.Rot.Q1;
_quat.B = robTarget.Rot.Q2;
_quat.C = robTarget.Rot.Q3;
_quat.D = robTarget.Rot.Q4;
// Update point
_point.X = robTarget.Trans.X;
_point.Y = robTarget.Trans.Y;
_point.Z = robTarget.Trans.Z;
// Convert to plane
_quat.GetRotation(out Plane plane);
plane = new Plane(_point, plane.XAxis, plane.YAxis);
// Add to list
planes.Add(plane);
}
// Set null plane if not
catch
{
// Raise a blank message
AddRuntimeMessage(GH_RuntimeMessageLevel.Remark, "Mechanical unit " + mechanicalUnits[i].Name + ": A position plane could not be defined for the selected coordinate system.");
// Add null plane
planes.Add(Plane.Unset);
}
// Add mechanical unit name to the list with names
mechanicalUnitnames.Add(mechanicalUnits[i].Name);
}
// Output
DA.SetDataList(0, planes);
DA.SetDataList(1, mechanicalUnitnames);
}
private void UIshow()
{
ControllerOperatingMode ctrlMode;
ControllerState ctrlState;
ctrlMode = RBTCtroller.OperatingMode;
switch (ctrlMode.ToString())
{
case "0":
btnModel.Text = "Auto"; break;
case "1":
btnModel.Text = "Init"; break;
case "2":
btnModel.Text = "MannualReducedSpeed"; break;
case "3":
btnModel.Text = "ManualFullSpeed"; break;
case "4":
btnModel.Text = "AutoChange"; break;
case "5":
btnModel.Text = "MannualFullSpeedChange"; break;
case "6":
btnModel.Text = "NotApplicable"; break;
default:
btnModel.Text = ctrlMode.ToString();
break;
}
ctrlState = RBTCtroller.State;
switch (ctrlState.ToString())
{
case "0":
lblState.Text = "Init"; break;
case "1":
lblState.Text = "MotorsOff"; break;
case "2":
lblState.Text = "MotorsOn"; break;
case "3":
lblState.Text = "GuardStop"; break;
case "4":
lblState.Text = "EmgStopp"; break;
case "5":
lblState.Text = "EmgStopRest"; break;
case "6":
lblState.Text = "SystemFailure"; break;
case "99":
lblState.Text = "Unknown"; break;
default:
lblState.Text = ctrlState.ToString();
break;
}
//throw new NotImplementedException();
switch (comboBox1.SelectedItem.ToString())
{
case "Base": //注意获取直角坐标的类继承关系，及参数
p = RBTCtroller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.Base);
break;
case "Tool":
p = RBTCtroller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.Tool);
break;
case "WorkObject":
p = RBTCtroller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
break;
case "World":
p = RBTCtroller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.World);
break;
case "Undefined":
J = RBTCtroller.MotionSystem.ActiveMechanicalUnit.GetPosition();
break;
default: break;
}
textBox1Num.Text = p.Trans.X.ToString(); //此次的p要定义到函数外边，否则会提示：有可能未赋值的错误！！！
textBox2Num.Text = p.Trans.Y.ToString();
textBox3Num.Text = p.Trans.Z.ToString();
textBox4Num.Text = p.Rot.Q1.ToString();
textBox5Num.Text = p.Rot.Q2.ToString();
textBox6Num.Text = p.Rot.Q3.ToString(); //此次还有个Q4不知道用来做什么
}
