halui.cc

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

	    maxSpindleOverride =  d;
	}
	else {
    	    // error-- no value provided, use 100% as max
	    maxSpindleOverride =  1.0;
	}
    }
    else {
	// no line at all
        maxSpindleOverride =  1.0;
    }
    
    if (NULL != (inistring = inifile.Find("AXES", "TRAJ"))) {
	if (1 == sscanf(inistring, "%d", &i) && i > 0) {
	    num_axes =  i;
	}
	else {
    	    // error-- no value provided, use 100% as max
	    num_axes =  0;
	}
    }
    else {
	// no line at all
        num_axes =  0;
    }

    if (NULL != (inistring = inifile.Find("LINEAR_UNITS", "DISPLAY"))) {
	if (!strcmp(inistring, "AUTO")) {
	    linearUnitConversion = LINEAR_UNITS_AUTO;
	} else if (!strcmp(inistring, "INCH")) {
	    linearUnitConversion = LINEAR_UNITS_INCH;
	} else if (!strcmp(inistring, "MM")) {
	    linearUnitConversion = LINEAR_UNITS_MM;
	} else if (!strcmp(inistring, "CM")) {
	    linearUnitConversion = LINEAR_UNITS_CM;
	}
    } else {
	// not found, leave default alone
    }

    if (NULL != (inistring = inifile.Find("ANGULAR_UNITS", "DISPLAY"))) {
	if (!strcmp(inistring, "AUTO")) {
	    angularUnitConversion = ANGULAR_UNITS_AUTO;
	} else if (!strcmp(inistring, "DEG")) {
	    angularUnitConversion = ANGULAR_UNITS_DEG;
	} else if (!strcmp(inistring, "RAD")) {
	    angularUnitConversion = ANGULAR_UNITS_RAD;
	} else if (!strcmp(inistring, "GRAD")) {
	    angularUnitConversion = ANGULAR_UNITS_GRAD;
	}
    } else {
	// not found, leave default alone
    }

    const char *mc;
    while(num_mdi_commands < MDI_MAX && (mc = inifile.Find("MDI_COMMAND", "HALUI", num_mdi_commands+1))) {
        mdi_commands[num_mdi_commands++] = strdup(mc);
        printf("halui found mdi command: %s\n", mc);
    }

    // close it
    inifile.Close();

    return 0;
}

static void hal_init_pins()
{
    int joint;

    old_halui_data.machine_on = *(halui_data->machine_on) = 0;
    old_halui_data.machine_off = *(halui_data->machine_off) = 0;

    old_halui_data.estop_activate = *(halui_data->estop_activate) = 0;
    old_halui_data.estop_reset = *(halui_data->estop_reset) = 0;

    
    for (joint=0; joint < num_axes; joint++) {
	*(halui_data->joint_home[joint]) = old_halui_data.joint_home[joint] = 0;
	*(halui_data->joint_nr_select[joint]) = old_halui_data.joint_nr_select[joint] = 0;
	*(halui_data->jog_minus[joint]) = old_halui_data.jog_minus[joint] = 0;
	*(halui_data->jog_plus[joint]) = old_halui_data.jog_plus[joint] = 0;
	*(halui_data->jog_analog[joint]) = old_halui_data.jog_analog[joint] = 0;
    }

    *(halui_data->joint_home[num_axes]) = old_halui_data.joint_home[num_axes] = 0;
    *(halui_data->jog_minus[num_axes]) = old_halui_data.jog_minus[num_axes] = 0;
    *(halui_data->jog_plus[num_axes]) = old_halui_data.jog_plus[num_axes] = 0;
    *(halui_data->jog_deadband) = 0.2;
    *(halui_data->jog_speed) = 0;

    *(halui_data->joint_selected) = 0; // select joint 0 by default
    
    *(halui_data->fo_scale) = old_halui_data.fo_scale = 0.1; //sane default
    *(halui_data->so_scale) = old_halui_data.so_scale = 0.1; //sane default
}

static int check_bit_changed(hal_bit_t *halpin, hal_bit_t *oldpin) {
    hal_bit_t bit;
    
    bit = *(halpin);
    if (bit != *(oldpin)) {
	*(oldpin) = bit;
	if (bit != 0) //if transition to 1
	    return 1;
    }
    return 0;
}

// this function looks if any of the hal pins has changed
// and sends appropiate messages if so
static void check_hal_changes()
{
    hal_s32_t counts;
    int select_changed, joint;
    hal_bit_t bit, js;
    hal_float_t floatt;
    
    //check if machine_on pin has changed (the rest work exactly the same)
    if (check_bit_changed(halui_data->machine_on, &(old_halui_data.machine_on)) != 0)
	sendMachineOn();                //send MachineOn NML command
    
    if (check_bit_changed(halui_data->machine_off, &(old_halui_data.machine_off)) != 0)
	sendMachineOff();

    if (check_bit_changed(halui_data->estop_activate, &(old_halui_data.estop_activate)) != 0)
	sendEstop();

    if (check_bit_changed(halui_data->estop_reset, &(old_halui_data.estop_reset)) != 0)
	sendEstopReset();

    if (check_bit_changed(halui_data->mode_manual, &(old_halui_data.mode_manual)) != 0)
	sendManual();

    if (check_bit_changed(halui_data->mode_auto, &(old_halui_data.mode_auto)) != 0)
	sendAuto();

    if (check_bit_changed(halui_data->mode_mdi, &(old_halui_data.mode_mdi)) != 0)
	sendMdi();

    if (check_bit_changed(halui_data->mode_teleop, &(old_halui_data.mode_teleop)) != 0)
	sendTeleop();

    if (check_bit_changed(halui_data->mode_joint, &(old_halui_data.mode_joint)) != 0)
	sendJoint();

    if (check_bit_changed(halui_data->mist_on, &(old_halui_data.mist_on)) != 0)
	sendMistOn();

    if (check_bit_changed(halui_data->mist_off, &(old_halui_data.mist_off)) != 0)
	sendMistOff();

    if (check_bit_changed(halui_data->flood_on, &(old_halui_data.flood_on)) != 0)
	sendFloodOn();

    if (check_bit_changed(halui_data->flood_off, &(old_halui_data.flood_off)) != 0)
	sendFloodOff();

    if (check_bit_changed(halui_data->lube_on, &(old_halui_data.lube_on)) != 0)
	sendLubeOn();

    if (check_bit_changed(halui_data->lube_off, &(old_halui_data.lube_off)) != 0)
	sendLubeOff();

    if (check_bit_changed(halui_data->program_run, &(old_halui_data.program_run)) != 0)
	sendProgramRun(0);

    if (check_bit_changed(halui_data->program_pause, &(old_halui_data.program_pause)) != 0)
	sendProgramPause();

    if (check_bit_changed(halui_data->program_os_on, &(old_halui_data.program_os_on)) != 0)
	sendSetOptionalStop(ON);

    if (check_bit_changed(halui_data->program_os_off, &(old_halui_data.program_os_off)) != 0)
	sendSetOptionalStop(OFF);

    if (check_bit_changed(halui_data->program_bd_on, &(old_halui_data.program_bd_on)) != 0)
	sendSetBlockDelete(ON);

    if (check_bit_changed(halui_data->program_bd_off, &(old_halui_data.program_bd_off)) != 0)
	sendSetBlockDelete(OFF);

    if (check_bit_changed(halui_data->program_resume, &(old_halui_data.program_resume)) != 0)
	sendProgramResume();

    if (check_bit_changed(halui_data->program_step, &(old_halui_data.program_step)) != 0)
	sendProgramStep();

    if (check_bit_changed(halui_data->program_stop, &(old_halui_data.program_stop)) != 0)
	sendAbort();

    //feed-override stuff
    counts = *halui_data->fo_counts;
    if(counts != old_halui_data.fo_counts) {
        sendFeedOverride( *halui_data->fo_value + (counts - old_halui_data.fo_counts) *
                *halui_data->fo_scale);
        old_halui_data.fo_counts = counts;
    }

    //spindle-override stuff
    counts = *halui_data->so_counts;
    if(counts != old_halui_data.so_counts) {
        sendSpindleOverride( *halui_data->so_value + (counts - old_halui_data.so_counts) *
                *halui_data->so_scale);
        old_halui_data.so_counts = counts;
    }
    
    if (check_bit_changed(halui_data->fo_increase, &(old_halui_data.fo_increase)) != 0)
        sendFeedOverride(*halui_data->fo_value + *halui_data->fo_scale);
    if (check_bit_changed(halui_data->fo_decrease, &(old_halui_data.fo_decrease)) != 0)
        sendFeedOverride(*halui_data->fo_value - *halui_data->fo_scale);

    if (check_bit_changed(halui_data->so_increase, &(old_halui_data.so_increase)) != 0)
        sendSpindleOverride(*halui_data->so_value + *halui_data->so_scale);
    if (check_bit_changed(halui_data->so_decrease, &(old_halui_data.so_decrease)) != 0)
        sendSpindleOverride(*halui_data->so_value - *halui_data->so_scale);

//spindle stuff
    if (check_bit_changed(halui_data->spindle_start, &(old_halui_data.spindle_start)) != 0)
	sendSpindleForward();

    if (check_bit_changed(halui_data->spindle_stop, &(old_halui_data.spindle_stop)) != 0)
	sendSpindleOff();

    if (check_bit_changed(halui_data->spindle_forward, &(old_halui_data.spindle_forward)) != 0)
	sendSpindleForward();

    if (check_bit_changed(halui_data->spindle_reverse, &(old_halui_data.spindle_reverse)) != 0)
	sendSpindleReverse();

    bit = *(halui_data->spindle_increase);
    if (bit != old_halui_data.spindle_increase) {
	if (bit != 0)
	    sendSpindleIncrease();
	if (bit == 0)
	    sendSpindleConstant();
	old_halui_data.spindle_increase = bit;
    }

    bit = *(halui_data->spindle_decrease);
    if (bit != old_halui_data.spindle_decrease) {
	if (bit != 0)
	    sendSpindleDecrease();
	if (bit == 0)
	    sendSpindleConstant();
	old_halui_data.spindle_decrease = bit;
    }

    if (check_bit_changed(halui_data->spindle_brake_on, &(old_halui_data.spindle_brake_on)) != 0)
	sendBrakeEngage();

    if (check_bit_changed(halui_data->spindle_brake_off, &(old_halui_data.spindle_brake_off)) != 0)
	sendBrakeRelease();
    
    if (check_bit_changed(halui_data->abort, &(old_halui_data.abort)) != 0)
	sendAbort();
    
// joint stuff (selection, homing..)
    select_changed = -1; // flag to see if the selected joint changed
    
    for (joint=0; joint < num_axes; joint++) {
	if (check_bit_changed(halui_data->joint_home[joint], &(old_halui_data.joint_home[joint])) != 0)
	    sendHome(joint);

	bit = *(halui_data->jog_minus[joint]);
	if (bit != old_halui_data.jog_minus[joint]) {
	    if (bit != 0)
		sendJogCont(joint,-*(halui_data->jog_speed));
	    else
		sendJogStop(joint);
	    old_halui_data.jog_minus[joint] = bit;
	}

	bit = *(halui_data->jog_plus[joint]);
	if (bit != old_halui_data.jog_plus[joint]) {
	    if (bit != 0)
		sendJogCont(joint,*(halui_data->jog_speed));
	    else
		sendJogStop(joint);
	    old_halui_data.jog_plus[joint] = bit;
	}

	floatt = *(halui_data->jog_analog[joint]);
	if (floatt != old_halui_data.jog_analog[joint]) {
	    if (fabs(floatt) > *(halui_data->jog_deadband))
		sendJogCont(joint,*(halui_data->jog_speed) * *(halui_data->jog_analog[joint]));
	    else
		sendJogStop(joint);
	    old_halui_data.jog_analog[joint] = floatt;
	}
	
	// check to see if another joint has been selected
	bit = *(halui_data->joint_nr_select[joint]);
	if (bit != old_halui_data.joint_nr_select[joint]) {
	    if (bit != 0) {
		*(halui_data->joint_selected) = joint;
		select_changed = joint; // flag that we changed the selected joint
	    } 
	    old_halui_data.joint_home[joint] = bit;
	}
    }
    
    if (select_changed >= 0) {
	for (joint = 0; joint < num_axes; joint++) {
	    if (joint != select_changed) {
		*(halui_data->joint_is_selected[joint]) = 0;
    	    } else {
		*(halui_data->joint_is_selected[joint]) = 1;
	    }
	}
    }

    if (check_bit_changed(halui_data->joint_home[num_axes], &(old_halui_data.joint_home[num_axes])) != 0)
	sendHome(*(halui_data->joint_selected));

    bit = *(halui_data->jog_minus[num_axes]);
    js = *(halui_data->joint_selected);
    if (bit != old_halui_data.jog_minus[num_axes]) {
        if (bit != 0)
	    sendJogCont(js, -*(halui_data->jog_speed));
	else
	    sendJogStop(js);
	old_halui_data.jog_minus[num_axes] = bit;
    }

    bit = *(halui_data->jog_plus[num_axes]);
    js = *(halui_data->joint_selected);
    if (bit != old_halui_data.jog_plus[num_axes]) {
        if (bit != 0)
    	    sendJogCont(js,*(halui_data->jog_speed));
	else
	    sendJogStop(js);
	old_halui_data.jog_plus[num_axes] = bit;
    }

    for(int n = 0; n < num_mdi_commands; n++) {
        if (check_bit_changed(halui_data->mdi_commands[n], &(old_halui_data.mdi_commands[n])) != 0)
            sendMdiCommand(n);
    }
}

// this function looks at the received NML status message
// and modifies the appropiate HAL pins
static void modify_hal_pins()
{
    int joint;
    
    if (emcStatus->task.state == EMC_TASK_STATE_ON) {
	*(halui_data->machine_is_on)=1;
    } else {
	*(halui_data->machine_is_on)=0;
    }

    if (emcStatus->task.state == EMC_TASK_STATE_ESTOP) {
	*(halui_data->estop_is_activated)=1;
    } else {