component m_spindle_transm "MAHO MH700 spindle and transmission"; author "Dominik Braun"; license "GPL"; option singleton yes; pin in float spindle_speed; pin in bit reducer_left; pin in bit reducer_right; pin in bit reducer_center; pin in bit reducer_left_center; pin in bit middle_left; pin in bit middle_right; pin in bit middle_center; pin in bit middle_left_center; pin in bit input_left; pin in bit input_right; pin in bit input_center; pin in bit input_left_center; pin in bit auto_mode; pin in bit estop; pin in bit manual_change_start; pin in bit gearswitch_bit0; pin in bit gearswitch_bit1; pin in bit gearswitch_bit2; pin in bit gearswitch_bit3; pin in bit gearswitch_bit4; pin in bit spindle_left_enable; pin in bit spindle_right_enable; pin in bit spindle_brake; pin out bit reducer_motor; pin out bit middle_motor; pin out bit input_motor; pin out bit motor_turn_right; pin out bit motor_center_pos; pin out bit spindle_small_left; pin out bit spindle_small_right; pin out bit spindle_brake_transm; pin out bit spindle_turn_left; pin out bit spindle_turn_right; pin out bit spindle_star; pin out bit spindle_triangle; pin out bit spindle_at_speed; pin out bit transmission_lamp; pin out bit spindle_lamp; pin out bit toolclamb_enable; pin out bit transm_spindle_inhibit; pin out bit ready; pin out bit error; //debug pin out bit gearswitch_auto; pin out bit gearswitch_manual; pin out signed gearswitch_gear; pin out signed pos_actual; pin out signed pos_optimal; pin out unsigned pos_state; pin out float debug1; pin out float debug2; pin out float debug3; pin out float debug4; pin out float debug_rpm; pin out float debug_rpm_old; pin out bit dbg_transm_switch; pin out bit dbg_spindle_triangle; pin out bit dbg_lock; pin out bit dbg_pos_ok; pin out float dbg_speed; pin out bit dbg_enable_auto_switch; param rw float stop_timeout = 10.0 "timeout for spindle stop"; param rw float switch_timeout = 10.0 "timeout for switching the transmission"; param rw float overrun_time = 0.01 "overrun time for switching motor"; param rw float setup_time = 0.5 "setup time for relays"; param rw float setup_time_brake = 1.8 "setup time for spindle brake"; param rw float star_triangle_time = 5.0 "time for star delta start"; param rw float spindle_brake_time = 1.5 "time for mechanical spindle brake"; param rw float max_rpm_1 = 0 "maximum spindle speed position 1"; param rw float max_rpm_2 = 50 "maximum spindle speed position 2"; param rw float max_rpm_3 = 63 "maximum spindle speed position 3"; param rw float max_rpm_4 = 80 "maximum spindle speed position 4"; param rw float max_rpm_5 = 100 "maximum spindle speed position 5"; param rw float max_rpm_6 = 125 "maximum spindle speed position 6"; param rw float max_rpm_7 = 160 "maximum spindle speed position 7"; param rw float max_rpm_8 = 200 "maximum spindle speed position 8"; param rw float max_rpm_9 = 250 "maximum spindle speed position 9"; param rw float max_rpm_10 = 315 "maximum spindle speed position 10"; param rw float max_rpm_11 = 400 "maximum spindle speed position 11"; param rw float max_rpm_12 = 500 "maximum spindle speed position 12"; param rw float max_rpm_13 = 630 "maximum spindle speed position 13"; param rw float max_rpm_14 = 800 "maximum spindle speed position 14"; param rw float max_rpm_15 = 1000 "maximum spindle speed position 15"; param rw float max_rpm_16 = 1250 "maximum spindle speed position 16"; param rw float max_rpm_17 = 1600 "maximum spindle speed position 17"; param rw float max_rpm_18 = 2000 "maximum spindle speed position 18"; param rw float max_rpm_19 = 2500 "maximum spindle speed position 19"; param rw float spindle_max_rpm = 2500 "maximum spindle speed rpm"; param rw float spindle_toggle_pause = 1.75 "spindle toogle cycle pause time"; param rw float spindle_toggle_enable = 0.250 "Spindle toogle cycle enable time"; //pin in unsigned POSITION_COUNT; //variable transm_pos pos[POSITION_COUNT]; variable int last_spindle_brake; variable int enable_auto_switch; variable int target_pos; variable double timer; variable double lamp_timer; variable bool pos_ok; variable bool transm_switch; variable bool spindle_stopped; variable bool spindle_run; variable int spindle_state; variable bool manual_change_start_old; variable int toggle_state; variable bool toggle_last_right; variable bool toggle_start_transm; variable bool toggle_run; variable double toggle_timer; variable double spindle_run_timer; variable bool init; option data transm_data; option extra_setup; function _; ;; #define POSITION_COUNT 19 #define STATE_IDLE 0 #define STATE_WAIT_STOP 1 #define STATE_INPUT_SET 2 #define STATE_INPUT_DIR 3 #define STATE_INPUT_MOVE 4 #define STATE_INPUT_OVERRUN 5 #define STATE_INPUT_WAIT 6 #define STATE_MIDDLE_SET 7 #define STATE_MIDDLE_DIR 8 #define STATE_MIDDLE_MOVE 9 #define STATE_MIDDLE_OVERRUN 10 #define STATE_MIDDLE_WAIT 11 #define STATE_REDUCER_SET 12 #define STATE_REDUCER_DIR 13 #define STATE_REDUCER_MOVE 14 #define STATE_REDUCER_OVERRUN 15 #define STATE_REDUCER_WAIT 16 #define STATE_ERROR 17 #define STATE_ERROR_FINAL 18 #define TOGGLE_STATE_INIT 0 #define TOGGLE_STATE_START 1 #define TOGGLE_WAIT_TURN 2 #define TOGGLE_WAIT_PAUSE 3 #define SPINDLE_STATE_INIT 0 #define SPINDLE_STATE_WAIT_SETUP 1 #define SPINDLE_STATE_STAR 2 #define SPINDLE_STATE_TRIANGLE 3 #define SPINDLE_STATE_BRAKING 4 #define SPINDLE_STATE_STOP 5 static int count; RTAPI_MP_INT(count, "number stations"); typedef enum {POS_LEFT, POS_RIGHT, POS_CENTER}transm_state_t; static bool CHECK_SWITCH_POS (bool in_left, bool in_right, bool in_center, transm_state_t state) { switch (state) { case POS_LEFT: return (in_left && !in_right && !in_center); case POS_RIGHT: return (!in_left && in_right && !in_center); case POS_CENTER: return (!in_left && !in_right && in_center); } return false; } typedef struct { double max_rpm; double max_rpm_old; transm_state_t reducer_state; transm_state_t middle_state; transm_state_t input_state; } transm_pos; typedef struct { transm_pos pos[POSITION_COUNT]; } transm_data; EXTRA_SETUP() { data.pos[0].reducer_state = POS_CENTER; data.pos[0].middle_state = POS_CENTER; data.pos[0].input_state = POS_CENTER; data.pos[1].reducer_state = POS_LEFT; data.pos[1].middle_state = POS_CENTER; data.pos[1].input_state = POS_CENTER; data.pos[2].reducer_state = POS_LEFT; data.pos[2].middle_state = POS_CENTER; data.pos[2].input_state = POS_LEFT; data.pos[3].reducer_state = POS_LEFT; data.pos[3].middle_state = POS_CENTER; data.pos[3].input_state = POS_RIGHT; data.pos[4].reducer_state = POS_LEFT; data.pos[4].middle_state = POS_LEFT; data.pos[4].input_state = POS_CENTER; data.pos[5].reducer_state = POS_LEFT; data.pos[5].middle_state = POS_LEFT; data.pos[5].input_state = POS_LEFT; data.pos[6].reducer_state = POS_LEFT; data.pos[6].middle_state = POS_LEFT; data.pos[6].input_state = POS_RIGHT; data.pos[7].reducer_state = POS_LEFT; data.pos[7].middle_state = POS_RIGHT; data.pos[7].input_state = POS_CENTER; data.pos[8].reducer_state = POS_LEFT; data.pos[8].middle_state = POS_RIGHT; data.pos[8].input_state = POS_LEFT; data.pos[9].reducer_state = POS_LEFT; data.pos[9].middle_state = POS_RIGHT; data.pos[9].input_state = POS_RIGHT; data.pos[10].reducer_state = POS_RIGHT; data.pos[10].middle_state = POS_CENTER; data.pos[10].input_state = POS_CENTER; data.pos[11].reducer_state = POS_RIGHT; data.pos[11].middle_state = POS_CENTER; data.pos[11].input_state = POS_LEFT; data.pos[12].reducer_state = POS_RIGHT; data.pos[12].middle_state = POS_CENTER; data.pos[12].input_state = POS_RIGHT; data.pos[13].reducer_state = POS_RIGHT; data.pos[13].middle_state = POS_LEFT; data.pos[13].input_state = POS_CENTER; data.pos[14].reducer_state = POS_RIGHT; data.pos[14].middle_state = POS_LEFT; data.pos[14].input_state = POS_LEFT; data.pos[15].reducer_state = POS_RIGHT; data.pos[15].middle_state = POS_LEFT; data.pos[15].input_state = POS_RIGHT; data.pos[16].reducer_state = POS_RIGHT; data.pos[16].middle_state = POS_RIGHT; data.pos[16].input_state = POS_CENTER; data.pos[17].reducer_state = POS_RIGHT; data.pos[17].middle_state = POS_RIGHT; data.pos[17].input_state = POS_LEFT; data.pos[18].reducer_state = POS_RIGHT; data.pos[18].middle_state = POS_RIGHT; data.pos[18].input_state = POS_RIGHT; return 0; } FUNCTION(_) { double period_sec; double spindle_speed_abs; transm_pos *pos; int i; int curr_pos, opti_pos; int selected; double min_rpm =0; int lock; transm_state_t req_state; int gearswitch_pos; // get period in seconds period_sec = (double)period * 1e-9; // ############# // Spindle // ############# //stop if ((spindle_run && spindle_brake) || (spindle_run && spindle_turn_left && !spindle_left_enable) || (spindle_run && spindle_turn_right && !spindle_right_enable)) { spindle_run = 0; spindle_state = SPINDLE_STATE_BRAKING; } //wait for start ok if (!spindle_brake && !transm_switch && spindle_stopped ) { if (spindle_left_enable) { spindle_turn_left =1; spindle_run = 1; } else if (spindle_right_enable) { spindle_turn_right =1; spindle_run =1; } } //spindle run state machine if (spindle_run_timer > 0) { spindle_run_timer -= period_sec; } switch (spindle_state) { case SPINDLE_STATE_INIT: spindle_at_speed = 0; spindle_star = 0; dbg_spindle_triangle = 0; if (spindle_run) { spindle_run_timer = setup_time; spindle_state = SPINDLE_STATE_WAIT_SETUP; } break; case SPINDLE_STATE_WAIT_SETUP: if (spindle_run_timer <= 0) { spindle_state = SPINDLE_STATE_STAR; } break; case SPINDLE_STATE_STAR: spindle_star = 1; spindle_run_timer = star_triangle_time; spindle_state = SPINDLE_STATE_TRIANGLE; break; case SPINDLE_STATE_TRIANGLE: if (spindle_run_timer <= 0) { //spindle_star = 0; dbg_spindle_triangle = 1; spindle_at_speed =1; } break; case SPINDLE_STATE_BRAKING: spindle_turn_left = 0; spindle_turn_right = 0; spindle_at_speed = 0; spindle_run_timer = spindle_brake_time; spindle_state = SPINDLE_STATE_STOP; break; case SPINDLE_STATE_STOP: if (spindle_run_timer <= 0) { spindle_state = SPINDLE_STATE_INIT; } break; default: spindle_state = SPINDLE_STATE_INIT; } spindle_stopped = !spindle_turn_left && !spindle_turn_right && !spindle_star && (!spindle_triangle || transm_switch); spindle_lamp = !spindle_stopped; // ############# // Transmission // ############# // copy max rpm values if (!init) { data.pos[0].max_rpm = max_rpm_1; data.pos[1].max_rpm = max_rpm_2; data.pos[2].max_rpm = max_rpm_3; data.pos[3].max_rpm = max_rpm_4; data.pos[4].max_rpm = max_rpm_5; data.pos[5].max_rpm = max_rpm_6; data.pos[6].max_rpm = max_rpm_7; data.pos[7].max_rpm = max_rpm_8; data.pos[8].max_rpm = max_rpm_9; data.pos[9].max_rpm = max_rpm_10; data.pos[10].max_rpm = max_rpm_11; data.pos[11].max_rpm = max_rpm_12; data.pos[12].max_rpm = max_rpm_13; data.pos[13].max_rpm = max_rpm_14; data.pos[14].max_rpm = max_rpm_15; data.pos[15].max_rpm = max_rpm_16; data.pos[16].max_rpm = max_rpm_17; data.pos[17].max_rpm = max_rpm_18; data.pos[18].max_rpm = max_rpm_19; init = true; } // Gearswitch gearswitch_manual =0; gearswitch_auto =0; gearswitch_gear =0; gearswitch_pos = gearswitch_bit4 << 4 | gearswitch_bit3 << 3 | gearswitch_bit2 << 2 | gearswitch_bit1 << 1 | gearswitch_bit0; if (gearswitch_pos == 1) { gearswitch_manual =1; } if (gearswitch_pos == 2) { gearswitch_auto =1; } // get absolute speed spindle_speed_abs = spindle_speed >= 0 ? spindle_speed : -spindle_speed; //Panel override if (gearswitch_pos >2) { gearswitch_gear = gearswitch_pos -3; spindle_speed_abs = data.pos[gearswitch_gear].max_rpm; } if (spindle_speed_abs > spindle_max_rpm) { spindle_speed_abs = spindle_max_rpm; } // check for best matching position pos_ok = 0; opti_pos = -1; curr_pos = -1; for (i=0; ireducer_state) && CHECK_SWITCH_POS(middle_left, middle_right, middle_center, pos->middle_state) && CHECK_SWITCH_POS(input_left, input_right, input_center, pos->input_state); if (selected) { curr_pos = i; } // check max_rpm value if (pos->max_rpm < 1.0) { pos->max_rpm = 1.0; } // position range matches if (spindle_speed_abs > min_rpm && spindle_speed_abs <= pos->max_rpm) { // check if current position is ok if (selected) { pos_ok = 1; } opti_pos = i; } min_rpm = pos->max_rpm; } // update debug data pos_actual = (double)(curr_pos + 1); pos_optimal = (double)(opti_pos + 1); // force pos ok, if optimal pos is selected // (needed for min/max speed fallback) if (curr_pos == opti_pos) { pos_ok = 1; } // position is ok, if spindle is stopped if (spindle_speed == 0 && gearswitch_auto) { pos_ok = 1; } // check for brake transition if (!last_spindle_brake && spindle_brake) { enable_auto_switch = 0; } if (last_spindle_brake && !spindle_brake) { enable_auto_switch = 1; } last_spindle_brake = spindle_brake; // first speed command after loosen brake -> force switch // allow switch in automatic mode only after brake lock = auto_mode; if (spindle_speed != 0 && enable_auto_switch) { enable_auto_switch = 0; lock = 0; if (curr_pos != opti_pos) { pos_ok = 0; } } //transm lamp blink if pos not ok if (lamp_timer > 0) { lamp_timer -= period_sec; } if (!pos_ok) { if (lamp_timer <=0) { lamp_timer = 0.8; transmission_lamp = !transmission_lamp; } } else { transmission_lamp = 0; } if (transm_switch) { transmission_lamp = 1; //gearchange- full on } // Start for Manual and Auto Transmission select if (!gearswitch_manual) { lock =1; if (!manual_change_start_old && manual_change_start && spindle_stopped) { lock =0; } } manual_change_start_old = manual_change_start; // Full Manual Mode, transmission off if (gearswitch_manual) { pos_ok = 1; lock = 1; } dbg_pos_ok = pos_ok; dbg_transm_switch = transm_switch; dbg_lock = lock; dbg_speed = spindle_speed_abs; dbg_enable_auto_switch = enable_auto_switch; //Spindle Toggle if (!transm_switch) { spindle_small_left = false; spindle_small_right = false; spindle_brake_transm = false; toggle_state = TOGGLE_STATE_INIT; toggle_start_transm = false; toggle_last_right = false; if (toggle_run) { spindle_triangle = false; toggle_run = false; } } else if (transm_switch && spindle_stopped) { toggle_run = true; spindle_brake_transm = true; spindle_triangle = true; if (toggle_timer > 0) { toggle_timer -= period_sec; } switch (toggle_state) { case TOGGLE_STATE_INIT: toggle_timer = spindle_toggle_pause; toggle_state = TOGGLE_STATE_START; break; case TOGGLE_STATE_START: if ( toggle_timer <= 0.05) { toggle_start_transm = true; } if (toggle_timer <= 0) { spindle_small_right = true; toggle_last_right = spindle_small_right; toggle_timer = spindle_toggle_enable; toggle_state = TOGGLE_WAIT_TURN; } break; case TOGGLE_WAIT_TURN: if (toggle_timer <=0) { spindle_small_left = false; spindle_small_right = false; toggle_timer = spindle_toggle_pause; toggle_state = TOGGLE_WAIT_PAUSE; } break; case TOGGLE_WAIT_PAUSE: if (toggle_timer <= 0) { if (toggle_last_right) { spindle_small_left =true; } else { spindle_small_right =true; } toggle_last_right = spindle_small_right; toggle_timer = spindle_toggle_enable; toggle_state = TOGGLE_WAIT_TURN; } break; default: toggle_state = TOGGLE_STATE_INIT; } } // update timer if (timer > 0) { timer -= period_sec; } // state machine for switching switch (pos_state) { case STATE_IDLE: reducer_motor = 0; input_motor = 0; middle_motor = 0; //motor_center_pos = 0; transm_switch = 0; ready = 1; error = 0; if (!lock && !pos_ok) { ready = 0; target_pos = opti_pos; timer = stop_timeout; pos_state = STATE_WAIT_STOP; } break; case STATE_WAIT_STOP: transm_switch = 1; if (timer <= 0) { rtapi_print_msg(RTAPI_MSG_ERR, "mh700-transm: Spindle stop timeout\n"); pos_state = STATE_ERROR; } if (spindle_stopped & toggle_start_transm) { //transm_switch = 1; pos_state = STATE_INPUT_SET; } break; //input gear case STATE_INPUT_SET: req_state = data.pos[target_pos].input_state; if (CHECK_SWITCH_POS(input_left, input_right, input_center, req_state)) { pos_state = STATE_MIDDLE_SET; break; } if (req_state == POS_LEFT) { motor_turn_right = 0; } else if (req_state == POS_RIGHT) { motor_turn_right = 1; } if (req_state == POS_CENTER) { motor_turn_right = input_left_center; motor_center_pos = 1; } else { motor_center_pos = 0; } timer = setup_time; pos_state = STATE_INPUT_DIR; break; case STATE_INPUT_DIR: if (timer <= 0) { timer = switch_timeout; pos_state = STATE_INPUT_MOVE; } break; case STATE_INPUT_MOVE: input_motor = 1; if (timer <= 0) { rtapi_print_msg(RTAPI_MSG_ERR, "mh700c-transm: Input stage switch timeout\n"); pos_state = STATE_ERROR; break; } req_state = data.pos[target_pos].input_state; if (CHECK_SWITCH_POS(input_left, input_right, input_center, req_state)) { timer = overrun_time; pos_state = STATE_INPUT_OVERRUN; } break; case STATE_INPUT_OVERRUN: if (timer <= 0) { input_motor = 0; timer = setup_time; pos_state = STATE_INPUT_WAIT; } break; case STATE_INPUT_WAIT: if (timer <= 0) { pos_state = STATE_MIDDLE_SET; } break; // Middle Gear case STATE_MIDDLE_SET: req_state = data.pos[target_pos].middle_state; if (CHECK_SWITCH_POS(middle_left, middle_right, middle_center, req_state)) { pos_state = STATE_REDUCER_SET; break; } if (req_state == POS_LEFT) { motor_turn_right = 0; } else if (req_state == POS_RIGHT) { motor_turn_right = 1; } if (req_state == POS_CENTER) { motor_turn_right = middle_left_center; motor_center_pos = 1; } else { motor_center_pos = 0; } timer = setup_time; pos_state = STATE_MIDDLE_DIR; break; case STATE_MIDDLE_DIR: if (timer <= 0) { timer = switch_timeout; pos_state = STATE_MIDDLE_MOVE; } break; case STATE_MIDDLE_MOVE: middle_motor = 1; if (timer <= 0) { rtapi_print_msg(RTAPI_MSG_ERR, "mh700-transm: MIDDLE stage switch timeout\n"); pos_state = STATE_ERROR; break; } req_state = data.pos[target_pos].middle_state; if (CHECK_SWITCH_POS(middle_left, middle_right, middle_center, req_state)) { timer = overrun_time; pos_state = STATE_MIDDLE_OVERRUN; } break; case STATE_MIDDLE_OVERRUN: if (timer <= 0) { middle_motor = 0; timer = setup_time; pos_state = STATE_MIDDLE_WAIT; } break; case STATE_MIDDLE_WAIT: if (timer <= 0) { pos_state = STATE_REDUCER_SET; } break; // Reducer Gear case STATE_REDUCER_SET: req_state = data.pos[target_pos].reducer_state; if (CHECK_SWITCH_POS(reducer_left, reducer_right, reducer_center, req_state)) { pos_state = STATE_IDLE; break; } if (req_state == POS_LEFT) { motor_turn_right = 0; } else if (req_state == POS_RIGHT) { motor_turn_right = 1; } if (req_state == POS_CENTER) { motor_turn_right = reducer_left_center; motor_center_pos = 1; } else { motor_center_pos = 0; } timer = setup_time; pos_state = STATE_REDUCER_DIR; break; case STATE_REDUCER_DIR: if (timer <= 0) { timer = switch_timeout; pos_state = STATE_REDUCER_MOVE; } break; case STATE_REDUCER_MOVE: reducer_motor = 1; if (timer <= 0) { rtapi_print_msg(RTAPI_MSG_ERR, "mh700-transm: REDUCER stage switch timeout\n"); pos_state = STATE_ERROR; break; } req_state = data.pos[target_pos].reducer_state; if (CHECK_SWITCH_POS(reducer_left, reducer_right, reducer_center, req_state)) { timer = overrun_time; pos_state = STATE_REDUCER_OVERRUN; } break; case STATE_REDUCER_OVERRUN: if (timer <= 0) { reducer_motor = 0; timer = setup_time; pos_state = STATE_REDUCER_WAIT; } break; case STATE_REDUCER_WAIT: if (timer <= 0) { pos_state = STATE_IDLE; } break; case STATE_ERROR: reducer_motor = 0; middle_motor = 0; input_motor = 0; transm_switch = 0; timer = setup_time; pos_state = STATE_ERROR_FINAL; break; case STATE_ERROR_FINAL: if (timer <= 0) { motor_turn_right = 0; motor_center_pos = 0; ready = 0; error = 1; } break; default: pos_state = STATE_IDLE; } transm_spindle_inhibit = transm_switch; //toolclamb enable toolclamb_enable = spindle_stopped & !transm_switch; // reset state machine if estop is pressed if (!estop) { pos_state = STATE_IDLE; toggle_state = TOGGLE_STATE_INIT; spindle_state = SPINDLE_STATE_INIT; reducer_motor = 0; input_motor = 0; middle_motor =0; motor_center_pos = 0; motor_turn_right = 0; transm_switch = 0; spindle_run = 0; spindle_stopped = 1; ready = 0; error = 0; spindle_small_left = 0; spindle_small_right = 0; spindle_brake_transm = 0; spindle_turn_left = 0; spindle_turn_right = 0; spindle_star = 0; spindle_triangle = 0; transmission_lamp =0; toolclamb_enable = 0; } }