diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 77fdf6f16dd..381a4959b7c 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -341,6 +341,14 @@ static void handle_kinematicsSwitch(void) { } #endif tpSetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); + + /* Also synch teleop mode to avoid change in joint cmd */ + for (int axis_num = 0; axis_num < MAX_AXIS_AMOUNT; axis_num++) { + emcmot_axis_t *axis; + axis = &axes[axis_num]; + axis->teleop_tp.curr_pos = *pcmd_p[axis_num]; + } + } //handle_kinematicsSwitch() static void process_inputs(void) @@ -1471,6 +1479,11 @@ static void get_pos_cmds(long period) for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { /* point to joint data */ joint = &joints[joint_num]; + + /* Zero values */ + joint_limit[joint_num][0] = 0; + joint_limit[joint_num][1] = 0; + /* skip inactive or unhomed axes */ if ((!GET_JOINT_ACTIVE_FLAG(joint)) || (!get_homed(joint_num))) { continue;