/* * _limit_switch_handler() - shut down system if limit switch fired */ static stat_t _limit_switch_handler(void) { if (cm_get_machine_state() == MACHINE_ALARM) { return (STAT_NOOP);} if (get_limit_switch_thrown() == false) return (STAT_NOOP); return(cm_hard_alarm(STAT_LIMIT_SWITCH_HIT)); return (STAT_OK); }
/************************************************************************* * mp_dwell() - queue a dwell * _exec_dwell() - dwell execution * * Dwells are performed by passing a dwell move to the stepper drivers. * When the stepper driver sees a dwell it times the dwell on a separate * timer than the stepper pulse timer. */ stat_t mp_dwell(float seconds) { mpBuf_t *bf; if ((bf = mp_get_write_buffer()) == NULL) // get write buffer or fail return(cm_hard_alarm(STAT_BUFFER_FULL_FATAL)); // not ever supposed to fail bf->bf_func = _exec_dwell; // register callback to dwell start bf->gm.move_time = seconds; // in seconds, not minutes bf->move_state = MOVE_NEW; mp_commit_write_buffer(MOVE_TYPE_DWELL); // must be final operation before exit return (STAT_OK); }
void mp_queue_command(void(*cm_exec)(float[], float[]), float *value, float *flag) { mpBuf_t *bf; // Never supposed to fail as buffer availability was checked upstream in the controller if ((bf = mp_get_write_buffer()) == NULL) { cm_hard_alarm(STAT_BUFFER_FULL_FATAL); return; } bf->move_type = MOVE_TYPE_COMMAND; bf->bf_func = _exec_command; // callback to planner queue exec function bf->cm_func = cm_exec; // callback to canonical machine exec function for (uint8_t axis = AXIS_X; axis < AXES; axis++) { bf->value_vector[axis] = value[axis]; bf->flag_vector[axis] = flag[axis]; } mp_commit_write_buffer(MOVE_TYPE_COMMAND); // must be final operation before exit }
/* #define axis_length bf->body_length #define axis_velocity bf->cruise_velocity #define axis_tail bf->tail_length #define longest_tail bf->head_length */ stat_t mp_aline(GCodeState_t *gm_in) { mpBuf_t *bf; // current move pointer float exact_stop = 0; // preset this value OFF float junction_velocity; uint8_t mr_flag = false; // compute some reusable terms float axis_length[AXES]; float axis_square[AXES]; float length_square = 0; for (uint8_t axis=0; axis<AXES; axis++) { axis_length[axis] = gm_in->target[axis] - mm.position[axis]; axis_square[axis] = square(axis_length[axis]); length_square += axis_square[axis]; } float length = sqrt(length_square); if (fp_ZERO(length)) { // sr_request_status_report(); return (STAT_OK); } // If _calc_move_times() says the move will take less than the minimum move time // get a more accurate time estimate based on starting velocity and acceleration. // The time of the move is determined by its initial velocity (Vi) and how much // acceleration will be occur. For this we need to look at the exit velocity of // the previous block. There are 3 possible cases: // (1) There is no previous block. Vi = 0 // (2) Previous block is optimally planned. Vi = previous block's exit_velocity // (3) Previous block is not optimally planned. Vi <= previous block's entry_velocity + delta_velocity _calc_move_times(gm_in, axis_length, axis_square); // set move time and minimum time in the state if (gm_in->move_time < MIN_BLOCK_TIME) { float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; // max velocity change for this move float entry_velocity = 0; // pre-set as if no previous block if ((bf = mp_get_run_buffer()) != NULL) { if (bf->replannable == true) { // not optimally planned entry_velocity = bf->entry_velocity + bf->delta_vmax; } else { // optimally planned entry_velocity = bf->exit_velocity; } } float move_time = (2 * length) / (2*entry_velocity + delta_velocity);// compute execution time for this move if (move_time < MIN_BLOCK_TIME) return (STAT_MINIMUM_TIME_MOVE); } // get a cleared buffer and setup move variables if ((bf = mp_get_write_buffer()) == NULL) return(cm_hard_alarm(STAT_BUFFER_FULL_FATAL)); // never supposed to fail bf->bf_func = mp_exec_aline; // register the callback to the exec function bf->length = length; memcpy(&bf->gm, gm_in, sizeof(GCodeState_t)); // copy model state into planner buffer // Compute the unit vector and find the right jerk to use (combined operations) // To determine the jerk value to use for the block we want to find the axis for which // the jerk cannot be exceeded - the 'jerk-limit' axis. This is the axis for which // the time to decelerate from the target velocity to zero would be the longest. // // We can determine the "longest" deceleration in terms of time or distance. // // The math for time-to-decelerate T from speed S to speed E with constant // jerk J is: // // T = 2*sqrt((S-E)/J[n]) // // Since E is always zero in this calculation, we can simplify: // T = 2*sqrt(S/J[n]) // // The math for distance-to-decelerate l from speed S to speed E with constant // jerk J is: // // l = (S+E)*sqrt((S-E)/J) // // Since E is always zero in this calculation, we can simplify: // l = S*sqrt(S/J) // // The final value we want is to know which one is *longest*, compared to the others, // so we don't need the actual value. This means that the scale of the value doesn't // matter, so for T we can remove the "2 *" and For L we can remove the "S*". // This leaves both to be simply "sqrt(S/J)". Since we don't need the scale, // it doesn't matter what speed we're coming from, so S can be replaced with 1. // // However, we *do* need to compensate for the fact that each axis contributes // differently to the move, so we will scale each contribution C[n] by the // proportion of the axis movement length D[n] to the total length of the move L. // Using that, we construct the following, with these definitions: // // J[n] = max_jerk for axis n // D[n] = distance traveled for this move for axis n // L = total length of the move in all axes // C[n] = "axis contribution" of axis n // // For each axis n: C[n] = sqrt(1/J[n]) * (D[n]/L) // // Keeping in mind that we only need a rank compared to the other axes, we can further // optimize the calculations:: // // Square the expression to remove the square root: // C[n]^2 = (1/J[n]) * (D[n]/L)^2 (We don't care the C is squared, we'll use it that way.) // // Re-arranged to optimize divides for pre-calculated values, we create a value // M that we compute once: // M = (1/(L^2)) // Then we use it in the calculations for every axis: // C[n] = (1/J[n]) * D[n]^2 * M // // Also note that we already have (1/J[n]) calculated for each axis, which simplifies it further. // // Finally, the selected jerk term needs to be scaled by the reciprocal of the absolute value // of the jerk-limit axis's unit vector term. This way when the move is finally decomposed into // its constituent axes for execution the jerk for that axis will be at it's maximum value. float C; // contribution term. C = T * a float maxC = 0; float recip_L2 = 1/length_square; for (uint8_t axis=0; axis<AXES; axis++) { if (fabs(axis_length[axis]) > 0) { // You cannot use the fp_XXX comparisons here! bf->unit[axis] = axis_length[axis] / bf->length; // compute unit vector term (zeros are already zero) C = axis_square[axis] * recip_L2 * cm.a[axis].recip_jerk; // squaring axis_length ensures it's positive if (C > maxC) { maxC = C; bf->jerk_axis = axis; // also needed for junction vmax calculation } } } // set up and pre-compute the jerk terms needed for this round of planning bf->jerk = cm.a[bf->jerk_axis].jerk_max * JERK_MULTIPLIER / fabs(bf->unit[bf->jerk_axis]); // scale the jerk if (fabs(bf->jerk - mm.jerk) > JERK_MATCH_PRECISION) { // specialized comparison for tolerance of delta mm.jerk = bf->jerk; // used before this point next time around mm.recip_jerk = 1/bf->jerk; // compute cached jerk terms used by planning mm.cbrt_jerk = cbrt(bf->jerk); } bf->recip_jerk = mm.recip_jerk; bf->cbrt_jerk = mm.cbrt_jerk; // finish up the current block variables if (cm_get_path_control(MODEL) != PATH_EXACT_STOP) { // exact stop cases already zeroed bf->replannable = true; exact_stop = 8675309; // an arbitrarily large floating point number } bf->cruise_vmax = bf->length / bf->gm.move_time; // target velocity requested junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit); bf->entry_vmax = min3(bf->cruise_vmax, junction_velocity, exact_stop); bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); bf->exit_vmax = min3(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax), exact_stop); bf->braking_velocity = bf->delta_vmax; // Note: these next lines must remain in exact order. Position must update before committing the buffer. _plan_block_list(bf, &mr_flag); // replan block list copy_vector(mm.position, bf->gm.target); // set the planner position mp_commit_write_buffer(MOVE_TYPE_ALINE); // commit current block (must follow the position update) return (STAT_OK); }