iocontrol.cc

Source code for an Numeric Cmputer

	    emcioStatus.command_type = EMC_IO_STAT_TYPE;
	    emcioStatus.echo_serial_number =
		emcioCommand->serial_number+1; //need for different serial number, because we are pushing a new message
	    emcioStatus.heartbeat++;
	    emcioStatusBuffer->write(&emcioStatus);
	}
	;
	if ( (tool_status = read_tool_inputs() ) > 0) { // in case of tool prep (or change) update, we only need to change the state (from RCS_EXEC
	    emcioStatus.command_type = EMC_IO_STAT_TYPE; // to RCS_DONE, no need for different serial_number
	    emcioStatus.echo_serial_number =
		emcioCommand->serial_number;
	    emcioStatus.heartbeat++;
	    emcioStatusBuffer->write(&emcioStatus);
	}

	/* read NML, run commands */
	if (-1 == emcioCommandBuffer->read()) {
	    /* bad command, wait until next cycle */
	    esleep(EMC_IO_CYCLE_TIME);
	    /* and repeat */
	    continue;
	}

	if (0 == emcioCommand ||	// bad command pointer
	    0 == emcioCommand->type ||	// bad command type
	    emcioCommand->serial_number == emcioStatus.echo_serial_number) {	// command already finished
	    /* wait until next cycle */
	    esleep(EMC_IO_CYCLE_TIME);
	    /* and repeat */
	    continue;
	}

	type = emcioCommand->type;
	emcioStatus.status = RCS_DONE;

	switch (type) {
	case 0:
	    break;

	case EMC_IO_INIT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_IO_INIT\n");
	    hal_init_pins();
	    break;

	case EMC_TOOL_INIT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_TOOL_INIT\n");
	    loadToolTable(TOOL_TABLE_FILE, emcioStatus.tool.toolTable);
	    break;

	case EMC_TOOL_HALT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_TOOL_HALT\n");
	    break;

	case EMC_TOOL_ABORT_TYPE:
	    // this gets sent on any Task Abort, so it might be safer to stop
	    // the spindle  and coolant
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_TOOL_ABORT\n");

	    emcioStatus.spindle.speed = 0.0;
	    emcioStatus.spindle.direction = 0;
	    emcioStatus.spindle.brake = 1;
	    emcioStatus.coolant.mist = 0;
	    emcioStatus.coolant.flood = 0;
	    *(iocontrol_data->coolant_mist)=0;		/* coolant mist output pin */
    	    *(iocontrol_data->coolant_flood)=0;		/* coolant flood output pin */
	    *(iocontrol_data->spindle_on)=0;		/* spindle spin output */
	    *(iocontrol_data->spindle_forward)=0;	/* spindle spin-forward output */
	    *(iocontrol_data->spindle_reverse)=0;	/* spindle spin-reverse output */
	    *(iocontrol_data->spindle_incr_speed)=0;	/* spindle spin-increase output */
	    *(iocontrol_data->spindle_decr_speed)=0;	/* spindle spin-decrease output */
	    *(iocontrol_data->spindle_brake)=0;		/* spindle brake output */
	    *(iocontrol_data->spindle_speed_out)=0.0;	/* spindle speed output */    
	    break;

	case EMC_TOOL_PREPARE_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_TOOL_PREPARE\n");
	    *(iocontrol_data->tool_prepare) = 1;
	    *(iocontrol_data->tool_prep_number) = ((EMC_TOOL_PREPARE *) emcioCommand)->tool;
	    // the feedback logic is done inside read_hal_inputs()
	    // we only need to set RCS_EXEC if RCS_DONE is not already set by the above logic
	    if (tool_status != 10) //set above to 10 in case PREP already finished (HAL loopback machine)
		emcioStatus.status = RCS_EXEC;
	    break;

	case EMC_TOOL_LOAD_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_TOOL_LOAD\n");
	    if (emcioStatus.tool.toolPrepped != -1) {
		*(iocontrol_data->tool_change) = 1; //notify HW for toolchange
	    }
	    // the feedback logic is done inside read_hal_inputs()
	    // we only need to set RCS_EXEC if RCS_DONE is not already set by the above logic
	    if (tool_status != 11) //set above to 11 in case LOAD already finished (HAL loopback machine)
		emcioStatus.status = RCS_EXEC;
	    break;

	case EMC_TOOL_UNLOAD_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_TOOL_UNLOAD\n");
	    emcioStatus.tool.toolInSpindle = 0;
	/*! \todo FIXME  - not sure about this NML message, does it get sent? when and why? */
	    break;

	case EMC_TOOL_LOAD_TOOL_TABLE_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_TOOL_LOAD_TOOL_TABLE\n");
	    if (0 != loadToolTable(((EMC_TOOL_LOAD_TOOL_TABLE *) emcioCommand)->
			      file, emcioStatus.tool.toolTable))
		emcioStatus.status = RCS_ERROR;
	    break;

	case EMC_TOOL_SET_OFFSET_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG,
			    "EMC_TOOL_SET_OFFSET length=%lf diameter=%lf\n",
			    ((EMC_TOOL_SET_OFFSET *) emcioCommand)->length,
			    ((EMC_TOOL_SET_OFFSET *) emcioCommand)->diameter);
	    emcioStatus.tool.
		toolTable[((EMC_TOOL_SET_OFFSET *) emcioCommand)->tool].
		length = ((EMC_TOOL_SET_OFFSET *) emcioCommand)->length;
	    emcioStatus.tool.
		toolTable[((EMC_TOOL_SET_OFFSET *) emcioCommand)->tool].
		diameter = ((EMC_TOOL_SET_OFFSET *) emcioCommand)->diameter;
	    if (0 != saveToolTable(TOOL_TABLE_FILE, emcioStatus.tool.toolTable))
		emcioStatus.status = RCS_ERROR;
	    break;

	case EMC_SPINDLE_INIT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_INIT\n");
	    emcioStatus.spindle.speed = 0.0;
	    emcioStatus.spindle.direction = 0;
	    emcioStatus.spindle.brake = 1;
	    emcioStatus.spindle.increasing = 0;
	    *(iocontrol_data->spindle_speed_out) = 0;
	    *(iocontrol_data->spindle_on) = 0;
	    *(iocontrol_data->spindle_forward) = 0;
	    *(iocontrol_data->spindle_reverse) = 0;
	    *(iocontrol_data->spindle_brake) = 1;
	    break;

	case EMC_SPINDLE_HALT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_HALT\n");
	    emcioStatus.spindle.speed = 0.0;
	    emcioStatus.spindle.direction = 0;
	    emcioStatus.spindle.brake = 1;
	    emcioStatus.spindle.increasing = 0;
	    *(iocontrol_data->spindle_speed_out) = 0;
	    *(iocontrol_data->spindle_on) = 0;
	    *(iocontrol_data->spindle_forward) = 0;
	    *(iocontrol_data->spindle_reverse) = 0;
	    *(iocontrol_data->spindle_brake) = 1;
	    break;

	case EMC_SPINDLE_ABORT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_ABORT\n");
	    emcioStatus.spindle.speed = 0.0;
	    emcioStatus.spindle.direction = 0;
	    emcioStatus.spindle.brake = 1;
	    emcioStatus.spindle.increasing = 0;
	    *(iocontrol_data->spindle_speed_out) = 0;
	    *(iocontrol_data->spindle_on) = 0;
	    *(iocontrol_data->spindle_forward) = 0;
	    *(iocontrol_data->spindle_reverse) = 0;
	    *(iocontrol_data->spindle_brake) = 1;
	    break;

	case EMC_SPINDLE_ON_TYPE:
	    emcioStatus.spindle.speed =
		((EMC_SPINDLE_ON *) emcioCommand)->speed;
	    emcioStatus.spindle.brake = 0;
	    emcioStatus.spindle.increasing = 0;
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_ON %f RPM\n",
			    emcioStatus.spindle.speed);
	    if (((EMC_SPINDLE_ON *) emcioCommand)->speed >= 0) {
		emcioStatus.spindle.direction = 1;
		*(iocontrol_data->spindle_on) = 1;
		*(iocontrol_data->spindle_forward) = 1;
		*(iocontrol_data->spindle_reverse) = 0;
	    } else {
		emcioStatus.spindle.direction = -1;
		*(iocontrol_data->spindle_on) = 1;
		*(iocontrol_data->spindle_forward) = 0;
		*(iocontrol_data->spindle_reverse) = 1;
	    }
	    *(iocontrol_data->spindle_speed_out) =
		    ((EMC_SPINDLE_ON *) emcioCommand)->speed;
	    *(iocontrol_data->spindle_brake) = 0;
	    break;

	case EMC_SPINDLE_OFF_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_OFF\n");
	    emcioStatus.spindle.speed = 0.0;
	    emcioStatus.spindle.direction = 0;
	    emcioStatus.spindle.brake = 1;
	    emcioStatus.spindle.increasing = 0;
	    *(iocontrol_data->spindle_speed_out) = 0;
	    *(iocontrol_data->spindle_on) = 0;
	    *(iocontrol_data->spindle_forward) = 0;
	    *(iocontrol_data->spindle_reverse) = 0;
	    *(iocontrol_data->spindle_brake) = 1;
	    break;

	case EMC_SPINDLE_FORWARD_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_FORWARD %f RPM\n",
			    emcioStatus.spindle.speed);
	    emcioStatus.spindle.direction = 1;
	    emcioStatus.spindle.brake = 0;
	    *(iocontrol_data->spindle_forward) = 1;
	    break;

	case EMC_SPINDLE_REVERSE_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_REVERSE %f RPM\n",
			    emcioStatus.spindle.speed);
	    emcioStatus.spindle.direction = -1;
	    emcioStatus.spindle.brake = 0;
	    *(iocontrol_data->spindle_reverse) = 1;
	    break;

	case EMC_SPINDLE_STOP_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_STOP\n");
	    emcioStatus.spindle.direction = 0;
	    emcioStatus.spindle.brake = 1;
	    *(iocontrol_data->spindle_speed_out) = 0;
	    *(iocontrol_data->spindle_on) = 0;
	    *(iocontrol_data->spindle_forward) = 0;
	    *(iocontrol_data->spindle_reverse) = 0;
	    *(iocontrol_data->spindle_brake) = 1;
	    break;

	case EMC_SPINDLE_INCREASE_TYPE:
	    emcioStatus.spindle.speed++;
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_INCREASE %f RPM\n",
			    emcioStatus.spindle.speed);
	    emcioStatus.spindle.increasing = 1;
	    emcioStatus.spindle.brake = 0;
	    if ( *(iocontrol_data->spindle_speed_out) > 0 )
		*(iocontrol_data->spindle_speed_out) += 1.0;
	    else
		*(iocontrol_data->spindle_speed_out) -= 1.0;
	    *(iocontrol_data->spindle_incr_speed) = 1;
	    break;

	case EMC_SPINDLE_DECREASE_TYPE:
	    emcioStatus.spindle.speed--;
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_DECREASE %f RPM\n",
			    emcioStatus.spindle.speed);
	    emcioStatus.spindle.increasing = -1;
	    emcioStatus.spindle.brake = 0;
	    if ( *(iocontrol_data->spindle_speed_out) > 0 )
		*(iocontrol_data->spindle_speed_out) -= 1.0;
	    else
		*(iocontrol_data->spindle_speed_out) += 1.0;
	    *(iocontrol_data->spindle_decr_speed) = 1;
	    break;

	case EMC_SPINDLE_CONSTANT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_CONSTANT %f RPM\n",
			    emcioStatus.spindle.speed);
	    emcioStatus.spindle.increasing = 0;
	    emcioStatus.spindle.brake = 0;
	    *(iocontrol_data->spindle_decr_speed) = 0;
	    *(iocontrol_data->spindle_incr_speed) = 0;
	    break;

	case EMC_SPINDLE_BRAKE_RELEASE_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_BRAKE_RELEASE\n");
	    emcioStatus.spindle.brake = 0;
	    *(iocontrol_data->spindle_brake) = 0;
	    break;

	case EMC_SPINDLE_BRAKE_ENGAGE_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SPINDLE_BRAKE_ENGAGE\n");
	    emcioStatus.spindle.brake = 1;
	    *(iocontrol_data->spindle_brake) = 1;
	    break;

	case EMC_COOLANT_INIT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_COOLANT_INIT\n");
	    emcioStatus.coolant.mist = 0;
	    emcioStatus.coolant.flood = 0;
	    *(iocontrol_data->coolant_mist) = 0;
	    *(iocontrol_data->coolant_flood) = 0;
	    break;

	case EMC_COOLANT_HALT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_COOLANT_HALT\n");
	    emcioStatus.coolant.mist = 0;
	    emcioStatus.coolant.flood = 0;
	    *(iocontrol_data->coolant_mist) = 0;
	    *(iocontrol_data->coolant_flood) = 0;
	    break;

	case EMC_COOLANT_ABORT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_COOLANT_ABORT\n");
	    emcioStatus.coolant.mist = 0;
	    emcioStatus.coolant.flood = 0;
	    *(iocontrol_data->coolant_mist) = 0;
	    *(iocontrol_data->coolant_flood) = 0;
	    break;

	case EMC_COOLANT_MIST_ON_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_COOLANT_MIST_ON\n");
	    emcioStatus.coolant.mist = 1;
	    *(iocontrol_data->coolant_mist) = 1;
	    break;

	case EMC_COOLANT_MIST_OFF_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_COOLANT_MIST_OFF\n");
	    emcioStatus.coolant.mist = 0;
	    *(iocontrol_data->coolant_mist) = 0;
	    break;

	case EMC_COOLANT_FLOOD_ON_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_COOLANT_FLOOD_ON\n");
	    emcioStatus.coolant.flood = 1;
	    *(iocontrol_data->coolant_flood) = 1;
	    break;

	case EMC_COOLANT_FLOOD_OFF_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_COOLANT_FLOOD_OFF\n");
	    emcioStatus.coolant.flood = 0;
	    *(iocontrol_data->coolant_flood) = 0;
	    break;

	case EMC_AUX_INIT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_AUX_INIT\n");
	    hal_init_pins(); //init default (safe) pin values
	    emcioStatus.aux.estop = 1;	// this should get modified by the loopback
	    *(iocontrol_data->user_enable_out) = 0; //don't enable on AUX_INIT
	    break;

	case EMC_AUX_HALT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_AUX_HALT\n");
	    emcioStatus.aux.estop = 1;  // this should get modified by the loopback
	    *(iocontrol_data->user_enable_out) = 0; //disable on AUX_HALT
	    break;

	case EMC_AUX_ABORT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_AUX_ABORT\n");
	    emcioStatus.aux.estop = 1;  // this should get modified by the loopback
	    *(iocontrol_data->user_enable_out) = 0; //disable on AUX_ABORT
	    break;

	case EMC_AUX_ESTOP_ON_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_AUX_ESTOP_ON\n");
	    /* assert an ESTOP to the outside world (thru HAL) */
	    *(iocontrol_data->user_enable_out) = 0; //disable on ESTOP_ON
	    hal_init_pins(); //resets all HAL pins to safe value
	    break;

	case EMC_AUX_ESTOP_OFF_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_AUX_ESTOP_OFF\n");
	    /* remove ESTOP */
	    *(iocontrol_data->user_enable_out) = 1; //we're good to enable on ESTOP_OFF
	    /* generate a rising edge to reset optional HAL latch */
	    *(iocontrol_data->user_request_enable) = 1;
	    break;
	    
	case EMC_AUX_ESTOP_RESET_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_AUX_ESTOP_RESET\n");
	    // doesn't do anything right now, this will need to come from GUI
	    // but that means task needs to be rewritten/rethinked
	    break;
	    
	case EMC_LUBE_INIT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_LUBE_INIT\n");
	    emcioStatus.lube.on = 0;
	    //get the lube-level from hal
	    emcioStatus.lube.level = *(iocontrol_data->lube_level);
	    *(iocontrol_data->lube) = 0;
	    break;

	case EMC_LUBE_HALT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_LUBE_HALT\n");
	    emcioStatus.lube.on = 0;
	    //get the lube-level from hal
	    emcioStatus.lube.level = *(iocontrol_data->lube_level);
	    *(iocontrol_data->lube) = 0;
	    break;

	case EMC_LUBE_ABORT_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_LUBE_ABORT\n");
	    emcioStatus.lube.on = 0;
	    emcioStatus.lube.level = 1;
	    //get the lube-level from hal
	    emcioStatus.lube.level = *(iocontrol_data->lube_level);
	    *(iocontrol_data->lube) = 0;
	    break;

	case EMC_LUBE_ON_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_LUBE_ON\n");
	    emcioStatus.lube.on = 1;
	    *(iocontrol_data->lube) = 1;
	    break;

	case EMC_LUBE_OFF_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_LUBE_OFF\n");
	    emcioStatus.lube.on = 0;
	    *(iocontrol_data->lube) = 0;
	    break;

	    /* FIXME - look if it's used, DEBUG level for the iocontroller */
	case EMC_SET_DEBUG_TYPE:
	    rtapi_print_msg(RTAPI_MSG_DBG, "EMC_SET_DEBUG\n");
	    EMC_DEBUG = ((EMC_SET_DEBUG *) emcioCommand)->debug;
	    break;

	default:
	    rtapi_print("IO: unknown command %s\n", emcSymbolLookup(type));
	    break;
	}			/* switch (type) */

	// ack for the received command
	emcioStatus.command_type = type;
	emcioStatus.echo_serial_number = emcioCommand->serial_number;
	//set above, to allow some commands to fail this
	//emcioStatus.status = RCS_DONE;
	emcioStatus.heartbeat++;
	emcioStatusBuffer->write(&emcioStatus);

	esleep(EMC_IO_CYCLE_TIME);
	/* clear reset line to allow for a later rising edge */
	*(iocontrol_data->user_request_enable) = 0;
	
    }	// end of "while (! done)" loop

    // disconnect from the HAL
    hal_exit(comp_id);

    if (emcErrorBuffer != 0) {
	delete emcErrorBuffer;
	emcErrorBuffer = 0;
    }

    if (emcioStatusBuffer != 0) {
	delete emcioStatusBuffer;
	emcioStatusBuffer = 0;
    }

    if (emcioCommandBuffer != 0) {
	delete emcioCommandBuffer;
	emcioCommandBuffer = 0;
    }

    return 0;
}