component mh700c_transm "MAHO MH700C transmission"; author "Sascha Ittner"; license "GPL"; option singleton yes; pin in float spindle_speed; pin in float spindle_voltage_fb; pin in bit spindle_brake; pin in bit spindle_stopped; pin in bit reducer_left; pin in bit reducer_right; pin in bit input_left; pin in bit input_right; pin in bit auto_mode; pin in bit estop; pin out float spindle_speed_fb; pin out float spindle_voltage; pin out bit reducer_motor; pin out bit input_motor; pin out bit motor_turn_right; pin out bit transm_switch; pin out bit ready; pin out float pos_actual; pin out float pos_optimal; pin out u32 pos_state; pin out bit pos_ok; pin out bit error; 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.5 "overrun time for switching motor"; param rw float setup_time = 0.2 "setup time for relays"; param rw float max_rpm_1 = 127 "maximum spindle speed position 1"; param rw float max_rpm_2 = 403 "maximum spindle speed position 2"; param rw float max_rpm_3 = 989 "maximum spindle speed position 3"; param rw float max_rpm_4 = 3135 "maximum spindle speed position 4"; param rw float max_voltage = 9.000 "maximum spindle cmd voltage"; param rw float min_voltage = 1.500 "minimum spindle cmd voltage"; option data transm_data; option extra_setup; function _; ;; #define POSITION_COUNT 4 #define SWITCH_LEFT 0 #define SWITCH_RIGHT 1 #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_REDUCER_SET 7 #define STATE_REDUCER_DIR 8 #define STATE_REDUCER_MOVE 9 #define STATE_REDUCER_OVERRUN 10 #define STATE_REDUCER_WAIT 11 #define STATE_ERROR 12 #define CHECK_SWITCH_POS(in_left, in_right, state) (state == SWITCH_LEFT ? (in_left && !in_right) : (!in_left && in_right)) typedef struct { double max_rpm; double max_rpm_old; double factor; double recip; int reducer_state; int input_state; } transm_pos; typedef struct { transm_pos pos[POSITION_COUNT]; double max_voltage_old; double min_voltage_old; double min_max_ratio; int last_spindle_brake; int enable_auto_switch; int target_pos; double timer; } transm_data; EXTRA_SETUP() { data.pos[0].reducer_state = SWITCH_LEFT; data.pos[0].input_state = SWITCH_RIGHT; data.pos[1].reducer_state = SWITCH_LEFT; data.pos[1].input_state = SWITCH_LEFT; data.pos[2].reducer_state = SWITCH_RIGHT; data.pos[2].input_state = SWITCH_RIGHT; data.pos[3].reducer_state = SWITCH_RIGHT; data.pos[3].input_state = SWITCH_LEFT; return 0; } FUNCTION(_) { double period_sec; double spindle_speed_abs; transm_pos *pos; int voltage_change; int i; int curr_pos, opti_pos; int selected; double min_rpm, all_min_rpm, all_max_rpm; double last_buffer, curr_buffer; int lock; int req_state; // get period in seconds period_sec = (double)period * 1e-9; // get absolute speed spindle_speed_abs = spindle_speed >= 0 ? spindle_speed : -spindle_speed; // copy max rpm values 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; // check for changed voltage parameters and calculate ratio voltage_change = 0; if (max_voltage != data.max_voltage_old || min_voltage != data.min_voltage_old) { voltage_change = 1; data.max_voltage_old = max_voltage; data.min_voltage_old = min_voltage; if (max_voltage == 0) { data.min_max_ratio = 0; } else { data.min_max_ratio = min_voltage / max_voltage; } } // check for best matching position pos_ok = 0; opti_pos = -1; last_buffer = -1; all_min_rpm = -1; all_max_rpm = -1; curr_pos = -1; for (i=0; ireducer_state) && CHECK_SWITCH_POS(input_left, input_right, pos->input_state); if (selected) { curr_pos = i; } // check max_rpm value if (pos->max_rpm < 1.0) { pos->max_rpm = 1.0; } // calculate transmission factor if (pos->max_rpm != pos->max_rpm_old || voltage_change) { pos->max_rpm_old = pos->max_rpm; pos->factor = max_voltage / pos->max_rpm; pos->recip = 1 / pos->factor; } // calculate min_rpm value min_rpm = pos->max_rpm * data.min_max_ratio; // minimum speed fallback if (all_min_rpm < 0 || min_rpm < all_min_rpm) { all_min_rpm = min_rpm; if (spindle_speed_abs < all_min_rpm) { opti_pos = i; } } // maximum speed fallback if (all_max_rpm < 0 || pos->max_rpm > all_max_rpm) { all_max_rpm = pos->max_rpm; if (spindle_speed_abs > all_max_rpm) { opti_pos = i; } } // 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; } // check for boundary buffer curr_buffer = spindle_speed_abs - min_rpm; if (last_buffer < 0 || curr_buffer > last_buffer) { opti_pos = i; last_buffer = pos->max_rpm - spindle_speed_abs; } } } // 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) { pos_ok = 1; } // check for brake transition if (!data.last_spindle_brake && spindle_brake) { data.enable_auto_switch = 0; } if (data.last_spindle_brake && !spindle_brake) { data.enable_auto_switch = 1; } data.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 && data.enable_auto_switch) { data.enable_auto_switch = 0; lock = 0; if (curr_pos != opti_pos) { pos_ok = 0; } } // update timer if (data.timer > 0) { data.timer -= period_sec; } // state machine for switching switch (pos_state) { case STATE_IDLE: reducer_motor = 0; input_motor = 0; if (ready && curr_pos >= 0 && curr_pos < POSITION_COUNT) { spindle_voltage = spindle_speed * data.pos[curr_pos].factor; spindle_speed_fb = spindle_voltage_fb * data.pos[curr_pos].recip; } transm_switch = 0; ready = 1; error = 0; if (!lock && !pos_ok) { spindle_voltage = 0; spindle_speed_fb = 0; ready = 0; data.target_pos = opti_pos; data.timer = stop_timeout; pos_state = STATE_WAIT_STOP; } break; case STATE_WAIT_STOP: if (data.timer <= 0) { rtapi_print_msg(RTAPI_MSG_ERR, "mh700c-transm: Spindle stop timeout\n"); pos_state = STATE_ERROR; } if (spindle_stopped) { transm_switch = 1; pos_state = STATE_INPUT_SET; } break; case STATE_INPUT_SET: req_state = data.pos[data.target_pos].input_state; if (CHECK_SWITCH_POS(input_left, input_right, req_state)) { pos_state = STATE_REDUCER_SET; } if (req_state == SWITCH_LEFT && !motor_turn_right) { data.timer = setup_time; motor_turn_right = 1; pos_state = STATE_INPUT_DIR; } if (req_state == SWITCH_RIGHT && motor_turn_right) { data.timer = setup_time; motor_turn_right = 0; pos_state = STATE_INPUT_DIR; } data.timer = switch_timeout; pos_state = STATE_INPUT_MOVE; break; case STATE_INPUT_DIR: if (data.timer <= 0) { data.timer = switch_timeout; pos_state = STATE_INPUT_MOVE; } break; case STATE_INPUT_MOVE: input_motor = 1; if (data.timer <= 0) { rtapi_print_msg(RTAPI_MSG_ERR, "mh700c-transm: Input stage switch timeout\n"); pos_state = STATE_ERROR; } req_state = data.pos[data.target_pos].input_state; if (CHECK_SWITCH_POS(input_left, input_right, req_state)) { data.timer = overrun_time; pos_state = STATE_INPUT_OVERRUN; } break; case STATE_INPUT_OVERRUN: if (data.timer <= 0) { input_motor = 0; data.timer = setup_time; pos_state = STATE_INPUT_WAIT; } break; case STATE_INPUT_WAIT: if (data.timer <= 0) { pos_state = STATE_REDUCER_SET; } break; case STATE_REDUCER_SET: req_state = data.pos[data.target_pos].reducer_state; if (CHECK_SWITCH_POS(reducer_left, reducer_right, req_state)) { pos_state = STATE_IDLE; } if (req_state == SWITCH_LEFT && motor_turn_right) { data.timer = setup_time; motor_turn_right = 0; pos_state = STATE_REDUCER_DIR; } if (req_state == SWITCH_RIGHT && !motor_turn_right) { data.timer = setup_time; motor_turn_right = 1; pos_state = STATE_REDUCER_DIR; } data.timer = switch_timeout; pos_state = STATE_REDUCER_MOVE; break; case STATE_REDUCER_DIR: if (data.timer <= 0) { data.timer = switch_timeout; pos_state = STATE_REDUCER_MOVE; } break; case STATE_REDUCER_MOVE: reducer_motor = 1; if (data.timer <= 0) { rtapi_print_msg(RTAPI_MSG_ERR, "mh700c-transm: Reducer stage switch timeout\n"); pos_state = STATE_ERROR; } req_state = data.pos[data.target_pos].reducer_state; if (CHECK_SWITCH_POS(reducer_left, reducer_right, req_state)) { data.timer = overrun_time; pos_state = STATE_REDUCER_OVERRUN; } break; case STATE_REDUCER_OVERRUN: if (data.timer <= 0) { reducer_motor = 0; data.timer = setup_time; pos_state = STATE_REDUCER_WAIT; } break; case STATE_REDUCER_WAIT: if (data.timer <= 0) { pos_state = STATE_IDLE; } break; case STATE_ERROR: reducer_motor = 0; input_motor = 0; motor_turn_right = 0; spindle_voltage = 0; spindle_speed_fb = 0; transm_switch = 0; ready = 0; error = 1; break; default: pos_state = STATE_IDLE; } // reset state machine if estop is pressed if (!estop) { pos_state = STATE_IDLE; reducer_motor = 0; input_motor = 0; motor_turn_right = 0; spindle_voltage = 0; spindle_speed_fb = 0; transm_switch = 0; ready = 0; error = 0; } }