emcsh.cc

来自「CNC 的开放码,EMC2 V2.2.8版」· CC 代码 · 共 2,603 行 · 第 1/5 页

    if (objc != 1) {
	Tcl_SetResult(interp, "emc_tool_offset: need no args",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    if (emcUpdateType == EMC_UPDATE_AUTO) {
	updateStatus();
    }

    tlobj =
	Tcl_NewDoubleObj(convertLinearUnits
			 (emcStatus->task.toolOffset.tran.z));

    Tcl_SetObjResult(interp, tlobj);
    return TCL_OK;
}

static int emc_load_tool_table(ClientData clientdata,
			       Tcl_Interp * interp, int objc,
			       Tcl_Obj * CONST objv[])
{
    if (objc != 2) {
	Tcl_SetResult(interp, "emc_load_tool_table: need file",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    if (0 != sendLoadToolTable(Tcl_GetStringFromObj(objv[1], 0))) {
	Tcl_SetResult(interp, "emc_load_tool_table: can't open file",
		      TCL_VOLATILE);
	return TCL_OK;
    }

    return TCL_OK;
}

static int emc_set_tool_offset(ClientData clientdata,
			       Tcl_Interp * interp, int objc,
			       Tcl_Obj * CONST objv[])
{
    int tool;
    double length;
    double diameter;

    if (objc != 4) {
	Tcl_SetResult(interp,
		      "emc_set_tool_offset: need <tool> <length> <diameter>",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    if (0 != Tcl_GetIntFromObj(0, objv[1], &tool)) {
	Tcl_SetResult(interp,
		      "emc_set_tool_offset: need tool as integer, 0..",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }
    if (0 != Tcl_GetDoubleFromObj(0, objv[2], &length)) {
	Tcl_SetResult(interp,
		      "emc_set_tool_offset: need length as real number",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }
    if (0 != Tcl_GetDoubleFromObj(0, objv[3], &diameter)) {
	Tcl_SetResult(interp,
		      "emc_set_tool_offset: need diameter as real number",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    if (0 != sendToolSetOffset(tool, length, diameter)) {
	Tcl_SetResult(interp, "emc_set_tool_offset: can't set it",
		      TCL_VOLATILE);
	return TCL_OK;
    }

    return TCL_OK;
}

static int emc_abs_cmd_pos(ClientData clientdata,
			   Tcl_Interp * interp, int objc,
			   Tcl_Obj * CONST objv[])
{
    int axis;
    Tcl_Obj *posobj;

    if (objc != 2) {
	Tcl_SetResult(interp,
		      "emc_abs_cmd_pos: need exactly 1 non-negative integer",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    if (emcUpdateType == EMC_UPDATE_AUTO) {
	updateStatus();
    }

    if (TCL_OK == Tcl_GetIntFromObj(0, objv[1], &axis)) {
	if (axis == 0) {
	    posobj =
		Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.traj.
						    position.tran.x));
	} else if (axis == 1) {
	    posobj =
		Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.traj.
						    position.tran.y));
	} else if (axis == 2) {
	    posobj =
		Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.traj.
						    position.tran.z));
	} else {
	    if (axis == 3) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.position.a));
	    } else if (axis == 4) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.position.b));
	    } else if (axis == 5) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.position.c));
	    } else if (axis == 6) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.position.u));
	    } else if (axis == 7) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.position.v));
	    } else if (axis == 8) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.position.w));
	    } else {
		posobj = Tcl_NewDoubleObj(0.0);
	    }
	}
    } else {
	Tcl_SetResult(interp, "emc_abs_cmd_pos: bad integer argument",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    Tcl_SetObjResult(interp, posobj);
    return TCL_OK;
}

static int emc_abs_act_pos(ClientData clientdata,
			   Tcl_Interp * interp, int objc,
			   Tcl_Obj * CONST objv[])
{
    int axis;
    Tcl_Obj *posobj;

    if (objc != 2) {
	Tcl_SetResult(interp,
		      "emc_abs_act_pos: need exactly 1 non-negative integer",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    if (emcUpdateType == EMC_UPDATE_AUTO) {
	updateStatus();
    }

    if (TCL_OK == Tcl_GetIntFromObj(0, objv[1], &axis)) {
	if (axis == 0) {
	    posobj =
		Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.traj.
						    actualPosition.tran.
						    x));
	} else if (axis == 1) {
	    posobj =
		Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.traj.
						    actualPosition.tran.
						    y));
	} else if (axis == 2) {
	    posobj =
		Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.traj.
						    actualPosition.tran.
						    z));
	} else {
	    if (axis == 3) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.
							actualPosition.a));
	    } else if (axis == 4) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.
							actualPosition.b));
	    } else if (axis == 5) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.
							actualPosition.c));
	    } else if (axis == 6) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.
							actualPosition.u));
	    } else if (axis == 7) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.
							actualPosition.v));
	    } else if (axis == 8) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.
							actualPosition.w));
	    } else {
		posobj = Tcl_NewDoubleObj(0.0);
	    }
	}
    } else {
	Tcl_SetResult(interp, "emc_abs_act_pos: bad integer argument",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    Tcl_SetObjResult(interp, posobj);
    return TCL_OK;
}

static int emc_rel_cmd_pos(ClientData clientdata,
			   Tcl_Interp * interp, int objc,
			   Tcl_Obj * CONST objv[])
{
    int axis;
    Tcl_Obj *posobj;

    if (objc != 2) {
	Tcl_SetResult(interp,
		      "emc_rel_cmd_pos: need exactly 1 non-negative integer",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    if (emcUpdateType == EMC_UPDATE_AUTO) {
	updateStatus();
    }

    if (TCL_OK == Tcl_GetIntFromObj(0, objv[1], &axis)) {
	if (axis == 0) {
	    posobj =
		Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.traj.
						    position.tran.x -
						    emcStatus->task.origin.
						    tran.x));
	} else if (axis == 1) {
	    posobj =
		Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.traj.
						    position.tran.y -
						    emcStatus->task.origin.
						    tran.y));
	} else if (axis == 2) {
	    posobj =
		Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.traj.
						    position.tran.z -
						    emcStatus->task.origin.
						    tran.z -
						    emcStatus->task.
						    toolOffset.tran.z));
	} else {
	    /*! \todo FIXME-- no rotational offsets yet */
	    if (axis == 3) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.position.a -
							emcStatus->task.
							origin.a));
	    } else if (axis == 4) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.position.b -
							emcStatus->task.
							origin.b));
	    } else if (axis == 5) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.position.c -
							emcStatus->task.
							origin.c));
	    } else if (axis == 6) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.position.u - 
							emcStatus->task.
							origin.u));
	    } else if (axis == 7) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.position.v - 
							emcStatus->task.
							origin.v));
	    } else if (axis == 8) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.position.w - 
							emcStatus->task.
							origin.w));
	    } else {
		posobj = Tcl_NewDoubleObj(0.0);
	    }
	}
    } else {
	Tcl_SetResult(interp, "emc_rel_cmd_pos: bad integer argument",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    Tcl_SetObjResult(interp, posobj);
    return TCL_OK;
}

static int emc_rel_act_pos(ClientData clientdata,
			   Tcl_Interp * interp, int objc,
			   Tcl_Obj * CONST objv[])
{
    int axis;
    Tcl_Obj *posobj;

    if (objc != 2) {
	Tcl_SetResult(interp,
		      "emc_rel_act_pos: need exactly 1 non-negative integer",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    if (emcUpdateType == EMC_UPDATE_AUTO) {
	updateStatus();
    }

    if (TCL_OK == Tcl_GetIntFromObj(0, objv[1], &axis)) {
	if (axis == 0) {
	    posobj =
		Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.traj.
						    actualPosition.tran.x -
						    emcStatus->task.origin.
						    tran.x));
	} else if (axis == 1) {
	    posobj =
		Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.traj.
						    actualPosition.tran.y -
						    emcStatus->task.origin.
						    tran.y));
	} else if (axis == 2) {
	    posobj =
		Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.traj.
						    actualPosition.tran.z -
						    emcStatus->task.origin.
						    tran.z -
						    emcStatus->task.
						    toolOffset.tran.z));
	} else {
	    if (axis == 3) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.
							actualPosition.a -
							emcStatus->task.
							origin.a));
	    } else if (axis == 4) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.
							actualPosition.b -
							emcStatus->task.
							origin.b));
	    } else if (axis == 5) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.
							actualPosition.c -
							emcStatus->task.
							origin.c));
	    } else if (axis == 6) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.
							actualPosition.u -
							emcStatus->task.
							origin.u));
	    } else if (axis == 7) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.
							actualPosition.v -
							emcStatus->task.
							origin.v));
	    } else if (axis == 8) {
		posobj =
		    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->motion.
							traj.
							actualPosition.w -
							emcStatus->task.
							origin.w));
	    } else {
		posobj = Tcl_NewDoubleObj(0.0);
	    }
	}
    } else {
	Tcl_SetResult(interp, "emc_rel_act_pos: bad integer argument",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    Tcl_SetObjResult(interp, posobj);
    return TCL_OK;
}

static int emc_joint_pos(ClientData clientdata,
			 Tcl_Interp * interp, int objc,
			 Tcl_Obj * CONST objv[])
{
    int axis;
    Tcl_Obj *posobj;

    if (objc != 2) {
	Tcl_SetResult(interp,
		      "emc_rel_act_pos: need exactly 1 non-negative integer",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    if (emcUpdateType == EMC_UPDATE_AUTO) {
	updateStatus();
    }

    if (TCL_OK == Tcl_GetIntFromObj(0, objv[1], &axis)) {
	posobj = Tcl_NewDoubleObj(emcStatus->motion.axis[axis].input);
    } else {
	Tcl_SetResult(interp, "emc_joint_pos: bad integer argument",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    Tcl_SetObjResult(interp, posobj);
    return TCL_OK;
}

static int emc_pos_offset(ClientData clientdata,
			  Tcl_Interp * interp, int objc,
			  Tcl_Obj * CONST objv[])
{
    char string[256];
    Tcl_Obj *posobj;

    if (objc != 2) {
	Tcl_SetResult(interp,
		      "emc_pos_offset: need exactly 1 non-negative integer",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    if (emcUpdateType == EMC_UPDATE_AUTO) {
	updateStatus();
    }

    strcpy(string, Tcl_GetStringFromObj(objv[1], 0));

    if (string[0] == 'X') {
	posobj =
	    Tcl_NewDoubleObj(convertLinearUnits
			     (emcStatus->task.origin.tran.x));
    } else if (string[0] == 'Y') {
	posobj =
	    Tcl_NewDoubleObj(convertLinearUnits
			     (emcStatus->task.origin.tran.y));
    } else if (string[0] == 'Z') {
	posobj =
	    Tcl_NewDoubleObj(convertLinearUnits
			     (emcStatus->task.origin.tran.z));
    } else if (string[0] == 'A') {
	posobj =
	    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->task.origin.a));
    } else if (string[0] == 'B') {
	posobj =
	    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->task.origin.b));
    } else if (string[0] == 'C') {
	posobj =
	    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->task.origin.c));
    } else if (string[0] == 'U') {
	posobj =
	    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->task.origin.u));
    } else if (string[0] == 'V') {
	posobj =
	    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->task.origin.v));
    } else if (string[0] == 'W') {
	posobj =
	    Tcl_NewDoubleObj(convertLinearUnits(emcStatus->task.origin.w));
    } else {
	Tcl_SetResult(interp, "emc_pos_offset: bad integer argument",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    Tcl_SetObjResult(interp, posobj);
    return TCL_OK;
}

static int emc_joint_limit(ClientData clientdata,
			   Tcl_Interp * interp, int objc,
			   Tcl_Obj * CONST objv[])
{
    int joint;

    if (objc != 2) {
	Tcl_SetResult(interp,
		      "emc_joint_limit: need exactly 1 non-negative integer",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    if (emcUpdateType == EMC_UPDATE_AUTO) {
	updateStatus();