static void Main(string[] args)
{
//机械臂控制上下文句柄
UInt16 rshd = 0xffff;
//创建机械臂控制对象
RobotAdepter robot = new RobotAdepter();
MetaData.wayPoint_S waypoint = new MetaData.wayPoint_S();
double[] target = { 0, 0, 0, 0, 0, 0 }; //注意这个里面的值是弧度！
//获取所有工具名称列表
List <string> toolNamelist;
MetaData.ToolInEndDesc robotTool;
robot.GetToolNameList(ServerInfo.SERVER_URL, out toolNamelist);
foreach (string name in toolNamelist)
{
robot.GetTool(ServerInfo.SERVER_URL, name, out robotTool);
}
//获取所有坐标系名称列表
List <string> coordNameList;
robot.GetCoordNameList(ServerInfo.SERVER_URL, out coordNameList);
foreach (string name in coordNameList)
{
MetaData.CoordCalibrate coord = new MetaData.CoordCalibrate();
//分配内存
IntPtr pt_coord = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(MetaData.CoordCalibrate)));
coord = (MetaData.CoordCalibrate)Marshal.PtrToStructure(pt_coord, typeof(MetaData.CoordCalibrate));
//获取坐标系参数
robot.GetCoord(ServerInfo.SERVER_URL, name, ref coord);
//必须释放内存否则会造成内存泄露!!!
Marshal.FreeHGlobal(pt_coord);
}
//机械臂运动控制示例
if (RobotAdepter.rs_create_context(ref rshd) == Util.RSERR_SUCC)
{
Console.Out.WriteLine("rshd={0}", rshd);
//链接机械臂服务器
if (RobotAdepter.rs_login(rshd, ServerInfo.robotIP, ServerInfo.serverPort) == Util.RSERR_SUCC)
{
Console.Out.WriteLine("login succ.");
//轴动到目标位置
RobotAdepter.rs_move_joint(rshd, target, true);
//直接获取机械臂当前位置信息
RobotAdepter.rs_get_current_waypoint(rshd, ref waypoint);
//打印路点信息
RobotAdepter.PrintWaypoint(waypoint);
RobotAdepter.rs_logout(rshd);
}
}
}
//打印路点信息
public static void PrintWaypoint(MetaData.wayPoint_S point)
{
Console.Out.WriteLine("---------------------------------------------------------------------------------------");
Console.Out.WriteLine("pos.x={0} y={1} z={2}", point.cartPos.x, point.cartPos.y, point.cartPos.z);
Console.Out.WriteLine("ori.w={0} x={1} y={2} z={3}", point.orientation.w, point.orientation.x, point.orientation.y, point.orientation.z);
Console.Out.WriteLine("joint1={0} joint2={1} joint3={2}", point.jointpos[0] * 180 / Util.M_PI, point.jointpos[1] * 180 / Util.M_PI, point.jointpos[2] * 180 / Util.M_PI);
Console.Out.WriteLine("joint4={0} joint5={1} joint6={2}", point.jointpos[3] * 180 / Util.M_PI, point.jointpos[4] * 180 / Util.M_PI, point.jointpos[5] * 180 / Util.M_PI);
Console.Out.WriteLine("---------------------------------------------------------------------------------------");
}
//回调函数
static void CurrentPositionCallback(ref MetaData.wayPoint_S waypoint, IntPtr arg)
{
PrintWaypoint(waypoint);
}
public static extern int rs_inverse_kin(UInt16 rshd, double[] joint_radia, ref MetaData.Pos pos, ref MetaData.Ori ori, ref MetaData.wayPoint_S waypoint);
public static extern int rs_forward_kin(UInt16 rshd, double[] joint_radia, ref MetaData.wayPoint_S waypoint);
public static extern int rs_get_current_waypoint(UInt16 rshd, ref MetaData.wayPoint_S waypoint);
