\******************************************************************************/
+#include "homing.h"
#include "machine.h"
#include "switch.h"
#include "util.h"
mach_set_feed_rate_mode(hm.saved_feed_rate_mode);
mach.gm.feed_rate = hm.saved_feed_rate;
mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
- mp_set_cycle(CYCLE_MACHINING);
}
void mach_set_not_homed() {
- // TODO save homed to EEPROM
for (int axis = 0; axis < AXES; axis++)
- hm.homed[axis] = false;
+ mach_set_homed(axis, false);
}
void mach_set_homed(int axis, bool homed) {
+ // TODO save homed to EEPROM
hm.homed[axis] = homed;
}
// set working values
mach_set_units_mode(MILLIMETERS);
mach_set_distance_mode(INCREMENTAL_MODE);
- mach_set_coord_system(ABSOLUTE_COORDS); // in machine coordinates
+ mach_set_coord_system(ABSOLUTE_COORDS); // in machine coordinates
mach_set_feed_rate_mode(UNITS_PER_MINUTE_MODE);
hm.set_coordinates = true;
mach.position[axis] = position;
mach.ms.target[axis] = position;
mp_set_planner_position(axis, position);
- mp_runtime_set_position(axis, position);
+ mp_runtime_set_axis_position(axis, position);
mp_runtime_set_steps_from_position();
}
static void _exec_absolute_origin(float *value, float *flag) {
for (int axis = 0; axis < AXES; axis++)
if (fp_TRUE(flag[axis])) {
- mp_runtime_set_position(axis, value[axis]);
+ mp_runtime_set_axis_position(axis, value[axis]);
mach_set_homed(axis, true); // G28.3 is not considered homed until here
}
// issues.
float disc = 4 * square(arc.radius) - (square(x) + square(y));
- float h_x2_div_d = (disc > 0) ? -sqrt(disc) / hypotf(x, y) : 0;
+ float h_x2_div_d = disc > 0 ? -sqrt(disc) / hypotf(x, y) : 0;
// Invert the sign of h_x2_div_d if circle is counter clockwise (see header
// notes)
// g18_correction is used to invert G18 XZ plane arcs for proper CW
// orientation
- float g18_correction = (mach.gm.select_plane == PLANE_XZ) ? -1 : 1;
+ float g18_correction = mach.gm.select_plane == PLANE_XZ ? -1 : 1;
if (arc.full_circle) {
// angular travel always starts as zero for full circles
// Add in travel for rotations
if (mach.gm.motion_mode == MOTION_MODE_CW_ARC)
- arc.angular_travel += (2*M_PI * arc.rotations * g18_correction);
- else arc.angular_travel -= (2*M_PI * arc.rotations * g18_correction);
+ arc.angular_travel += 2 * M_PI * arc.rotations * g18_correction;
+ else arc.angular_travel -= 2 * M_PI * arc.rotations * g18_correction;
// Calculate travel in the depth axis of the helix and compute the time it
// should take to perform the move arc.length is the total mm of travel of
mb.r = mb.r->nx; // advance to next run buffer
mb.buffers_available++;
report_request();
-
- if (mp_queue_empty()) mp_state_idle(); // if queue empty
}
typedef enum {
MOVE_OFF, // move inactive (MUST BE ZERO)
MOVE_NEW, // initial value
+ MOVE_INIT, // first run
MOVE_RUN, // general run state (for non-acceleration moves)
+ MOVE_RESTART, // restart buffer when done
} run_state_t;
// Callbacks
-typedef void (*mach_exec_t)(float[], float[]);
+typedef void (*mach_func_t)(float[], float[]);
struct mp_buffer_t;
typedef stat_t (*bf_func_t)(struct mp_buffer_t *bf);
struct mp_buffer_t *nx; // pointer to next buffer
bf_func_t bf_func; // callback to buffer exec function
- mach_exec_t mach_func; // callback to machine
+ mach_func_t mach_func; // callback to machine
buffer_state_t buffer_state; // used to manage queuing/dequeuing
move_type_t move_type; // used to dispatch to run routine
static stat_t _exec_calibrate(mp_buffer_t *bf) {
- if (bf->run_state == MOVE_NEW) bf->run_state = MOVE_RUN;
-
const float time = MIN_SEGMENT_TIME; // In minutes
const float max_delta_v = JOG_ACCELERATION * time;
STATUS_DEBUG("%"PRIi32" steps %0.2f mm", steps, mm);
tmc2660_set_stallguard_threshold(cal.motor, 63);
- mp_free_run_buffer(); // Release buffer
- mp_set_cycle(CYCLE_MACHINING);
- return STAT_OK;
+
+ return STAT_OK; // Done
} else {
motor_set_encoder(cal.motor, 0);
break;
}
- if (!cal.velocity) return STAT_OK;
+ if (!cal.velocity) return STAT_EAGAIN;
// Compute travel
float travel[AXES] = {0}; // In mm
float error[MOTORS] = {0};
st_prep_line(steps, error, time);
- return STAT_OK;
+ return STAT_EAGAIN;
}
* (bf buffer) which sets some parameters and registers a callback to the
* execution function in the machine.
* - When the planning queue gets to the function it calls _exec_command()
- * which loads a pointer to the bf buffer in stepper.c's next move.
+ * which loads a pointer to the function and its args in stepper.c's next
+ * move.
* - When the runtime gets to the end of the current activity (sending steps,
- * counting a dwell) it executes mp_command_callback which uses the callback
- * function in the bf and the saved parameters in the vectors.
- * - To finish up mp_command_callback() frees the bf buffer.
+ * counting a dwell) it executes the callback function passing the args.
*
* Doing it this way instead of synchronizing on queue empty simplifies the
* handling of feedholds, feed overrides, buffer flushes, and thread blocking,
/// Callback to execute command
static stat_t _exec_command(mp_buffer_t *bf) {
- st_prep_command(bf);
- return STAT_OK;
+ st_prep_command(bf->mach_func, bf->ms.target, bf->unit);
+ return STAT_OK; // Done
}
/// Queue a synchronous Mcode, program control, or other command
-void mp_queue_command(mach_exec_t mach_exec, float *value, float *flag) {
+void mp_queue_command(mach_func_t mach_func, float values[], float flags[]) {
mp_buffer_t *bf = mp_get_write_buffer();
if (!bf) {
}
bf->bf_func = _exec_command; // callback to planner queue exec function
- bf->mach_func = mach_exec; // callback to machine exec function
+ bf->mach_func = mach_func; // callback to machine exec function
// Store values and flags in planner buffer
for (int axis = 0; axis < AXES; axis++) {
- bf->ms.target[axis] = value[axis];
- bf->unit[axis] = flag[axis]; // flag vector in unit
+ bf->ms.target[axis] = values[axis];
+ bf->unit[axis] = flags[axis]; // flag vector in unit
}
// Must be final operation before exit
mp_commit_write_buffer(mach_get_line(), MOVE_TYPE_COMMAND);
}
-
-
-void mp_command_callback(mp_buffer_t *bf) {
- // Use values & flags stored in mp_queue_command()
- bf->mach_func(bf->ms.target, bf->unit);
-
- // Free buffer & perform cycle_end if planner is empty
- mp_free_run_buffer();
-}
#include "plan/buffer.h"
-void mp_queue_command(mach_exec_t mach_exec, float *value, float *flag);
-void mp_command_callback(mp_buffer_t *bf);
+void mp_queue_command(mach_func_t mach_exec, float *value, float *flag);
/// Dwell execution
static stat_t _exec_dwell(mp_buffer_t *bf) {
st_prep_dwell(bf->ms.move_time); // in seconds
-
- // free buffer & perform cycle_end if planner is empty
- mp_free_run_buffer();
-
- return STAT_OK;
+ return STAT_OK; // Done
}
bf->bf_func = _exec_dwell; // register callback to dwell start
bf->ms.move_time = seconds; // in seconds, not minutes
- bf->run_state = MOVE_NEW;
// must be final operation before exit
mp_commit_write_buffer(line, MOVE_TYPE_DWELL);
// waypoint, synchronize to the head, body or tail end. Otherwise, if not at
// a section waypoint compute target from segment time and velocity. Don't
// do waypoint correction if you are going into a hold.
- if (!--ex.segment_count && !ex.section_new && mp_get_state() == STATE_RUNNING)
+ if (!--ex.segment_count && !ex.section_new && !ex.hold_planned)
copy_vector(target, ex.waypoint[ex.section]);
else {
float segment_length = ex.segment_velocity * ex.segment_time;
for (int i = 0; i < AXES; i++)
- target[i] = mp_runtime_get_position(i) + ex.unit[i] * segment_length;
+ target[i] = mp_runtime_get_axis_position(i) + ex.unit[i] * segment_length;
}
- RITORNO(mp_runtime_move_to_target
- (target, ex.segment_velocity, ex.segment_time));
+ mp_runtime_set_velocity(ex.segment_velocity);
+ RITORNO(mp_runtime_move_to_target(target, ex.segment_time));
// Return EAGAIN to continue or OK if this segment is done
return ex.segment_count ? STAT_EAGAIN : STAT_OK;
static float _compute_next_segment_velocity() {
- if (ex.section == SECTION_BODY) return ex.segment_velocity;
- return ex.segment_velocity + ex.forward_diff[4];
+ if (ex.section_new) {
+ if (ex.section == SECTION_HEAD) return ex.entry_velocity;
+ else return ex.cruise_velocity;
+ }
+
+ return ex.segment_velocity +
+ (ex.section == SECTION_BODY ? 0 : ex.forward_diff[4]);
}
* speed.
*/
static void _plan_hold() {
- mp_buffer_t *bp = mp_get_run_buffer(); // working buffer pointer
- if (!bp) return; // Oops! nothing's running
+ mp_buffer_t *bf = mp_get_run_buffer(); // working buffer pointer
+ if (!bf) return; // Oops! nothing's running
// Examine and process current buffer and compute length left for decel
float available_length =
- get_axis_vector_length(ex.final_target, mp_runtime_get_position_vector());
+ get_axis_vector_length(ex.final_target, mp_runtime_get_position());
// Compute next_segment velocity, velocity left to shed to brake to zero
float braking_velocity = _compute_next_segment_velocity();
- // Distance to brake to zero from braking_velocity, bp is OK to use here
- float braking_length = mp_get_target_length(braking_velocity, 0, bp);
-
- // Hack to prevent Case 2 moves for perfect-fit decels. Happens in
- // homing situations. The real fix: The braking velocity cannot
- // simply be the ex.segment_velocity as this is the velocity of the
- // last segment, not the one that's going to be executed next. The
- // braking_velocity needs to be the velocity of the next segment
- // that has not yet been computed. In the mean time, this hack will work.
- if (available_length < braking_length && fp_ZERO(bp->exit_velocity))
+ // Distance to brake to zero from braking_velocity, bf is OK to use here
+ float braking_length = mp_get_target_length(braking_velocity, 0, bf);
+
+ // Hack to prevent Case 2 moves for perfect-fit decels. Happens when homing.
+ if (available_length < braking_length && fp_ZERO(bf->exit_velocity))
braking_length = available_length;
// Replan to decelerate
ex.exit_velocity = 0;
ex.tail_length = braking_length;
- // Re-use bp+0 to be the hold point and to run the remaining block length
- bp->length = available_length - braking_length;
- bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp);
- bp->entry_vmax = 0; // set bp+0 as hold point
- bp->run_state = MOVE_NEW; // tell _exec to re-use the bf buffer
+ // Re-use bf to run the remaining block length
+ bf->length = available_length - braking_length;
+ bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf);
+ bf->entry_vmax = 0;
+ bf->run_state = MOVE_RESTART; // Restart the buffer when done
} else { // Case 2: deceleration exceeds length remaining in buffer
// Replan to minimum (but non-zero) exit velocity
ex.tail_length = available_length;
ex.exit_velocity =
- braking_velocity - mp_get_target_velocity(0, available_length, bp);
+ braking_velocity - mp_get_target_velocity(0, available_length, bf);
}
}
/// Initializes move runtime with a new planner buffer
static stat_t _exec_aline_init(mp_buffer_t *bf) {
// Remove zero length lines. Short lines have already been removed.
- if (fp_ZERO(bf->length)) {
- mp_runtime_set_busy(false);
- bf->nx->replannable = false; // prevent overplanning (Note 2)
- mp_free_run_buffer(); // free buffer
-
- return STAT_NOOP;
- }
-
- // Take control of buffer
- bf->run_state = MOVE_RUN;
- bf->replannable = false;
+ if (fp_ZERO(bf->length)) return STAT_NOOP;
// Initialize move
copy_vector(ex.unit, bf->unit);
ex.hold_planned = false;
// Update runtime
- mp_runtime_set_busy(true);
mp_runtime_set_work_offsets(bf->ms.work_offset);
// Generate waypoints for position correction at section ends. This helps
// negate floating point errors in the forward differencing code.
for (int axis = 0; axis < AXES; axis++) {
ex.waypoint[SECTION_HEAD][axis] =
- mp_runtime_get_position(axis) + ex.unit[axis] * ex.head_length;
+ mp_runtime_get_axis_position(axis) + ex.unit[axis] * ex.head_length;
- ex.waypoint[SECTION_BODY][axis] = mp_runtime_get_position(axis) +
+ ex.waypoint[SECTION_BODY][axis] = mp_runtime_get_axis_position(axis) +
ex.unit[axis] * (ex.head_length + ex.body_length);
ex.waypoint[SECTION_TAIL][axis] = ex.final_target[axis];
* Period 3 V = Vi - Jm * (T^2) / 2
* Period 4 V = Vh + As * T + Jm * (T^2) / 2
*
- * These routines play some games with the acceleration and move timing
- * to make sure this actually work out. move_time is the actual time of
- * the move, accel_time is the time value needed to compute the velocity -
- * taking the initial velocity into account (move_time does not need to).
- *
- * State transitions - hierarchical state machine:
- *
- * bf->run_state transitions:
- *
- * from _NEW to _RUN on first call (sub_state set to _OFF)
- * from _RUN to _OFF on final call or just remain _OFF
+ * move_time is the actual time of the move, accel_time is the time value
+ * needed to compute the velocity taking the initial velocity into account.
+ * move_time does not need to.
*/
stat_t mp_exec_aline(mp_buffer_t *bf) {
- if (bf->run_state == MOVE_OFF) return STAT_NOOP; // No more moves
-
stat_t status = STAT_OK;
// Start a new move
- if (!mp_runtime_is_busy()) {
+ if (bf->run_state == MOVE_INIT) {
+ bf->run_state = MOVE_RUN;
status = _exec_aline_init(bf);
if (status != STAT_OK) return status;
}
+ if (mp_get_state() == STATE_STOPPING && !ex.hold_planned) _plan_hold();
+
// Main segment dispatch. From this point on the contents of the bf buffer
// do not affect execution.
switch (ex.section) {
case SECTION_TAIL: status = _exec_aline_tail(); break;
}
- // There are 3 things that can happen here depending on return conditions:
- //
- // status bf->run_state Description
- // ----------- -------------- ----------------------------------
- // STAT_EAGAIN <don't care> buffer has more segments to run
- // STAT_OK MOVE_RUN ex and bf buffers are done
- // STAT_OK MOVE_NEW ex done; bf must be run again
- // (it's been reused)
- if (status != STAT_EAGAIN) {
- mp_runtime_set_busy(false);
- bf->nx->replannable = false; // prevent overplanning (Note 2)
- if (fp_ZERO(ex.exit_velocity)) ex.segment_velocity = 0;
- // Note, _plan_hold() may change bf->run_state to reuse this buffer so it
- // can plan the deceleration.
- if (bf->run_state == MOVE_RUN) mp_free_run_buffer();
- }
-
- if (mp_get_state() == STATE_STOPPING &&
- (status == STAT_OK || status == STAT_EAGAIN)) {
- if (!mp_runtime_is_busy() && !ex.segment_velocity) mp_state_holding();
- else if (mp_runtime_is_busy() && !ex.hold_planned) _plan_hold();
- }
+ if (status != STAT_EAGAIN) mp_runtime_set_velocity(ex.exit_velocity);
return status;
}
if (mp_get_state() == STATE_HOLDING) return STAT_NOOP;
mp_buffer_t *bf = mp_get_run_buffer();
- if (!bf) return STAT_NOOP; // nothing running
- if (!bf->bf_func) return CM_ALARM(STAT_INTERNAL_ERROR);
+ if (!bf) return STAT_NOOP; // Nothing running
+ if (!bf->bf_func) return STAT_INTERNAL_ERROR; // Should never happen
- // Update runtime
- mp_runtime_set_line(bf->ms.line);
+ if (bf->run_state == MOVE_NEW) {
+ // Take control of buffer
+ bf->run_state = MOVE_INIT;
+ bf->replannable = false;
+
+ // Update runtime
+ mp_runtime_set_busy(true);
+ mp_runtime_set_line(bf->ms.line);
+ }
+
+ stat_t status = bf->bf_func(bf); // Move callback
+
+ if (status != STAT_EAGAIN) {
+ bool idle = false;
+
+ // Enter HOLDING state
+ if (mp_get_state() == STATE_STOPPING &&
+ fp_ZERO(mp_runtime_get_velocity())) {
+ mp_state_holding();
+ idle = true;
+ }
- return bf->bf_func(bf); // move callback
+ // Handle buffer run state
+ if (bf->run_state == MOVE_RESTART) bf->run_state = MOVE_NEW;
+ else {
+ bf->nx->replannable = false; // Prevent overplanning (Note 2)
+ mp_free_run_buffer(); // Free buffer
+
+ // Enter READY state
+ if (mp_queue_empty()) {
+ mp_state_idle();
+ idle = true;
+ }
+
+ mp_set_cycle(CYCLE_MACHINING); // Default cycle
+ }
+
+ // Queue idle
+ if (idle) {
+ mp_runtime_set_velocity(0);
+ mp_runtime_set_busy(false);
+ }
+ }
+
+ return status;
}
static stat_t _exec_jog(mp_buffer_t *bf) {
- if (bf->run_state == MOVE_OFF) return STAT_NOOP;
- if (bf->run_state == MOVE_NEW) bf->run_state = MOVE_RUN;
-
// Load next velocity
if (!jr.writing)
for (int axis = 0; axis < AXES; axis++)
// Compute target from axis velocity
target[axis] =
- mp_runtime_get_position(axis) + time * jr.current_velocity[axis];
+ mp_runtime_get_axis_position(axis) + time * jr.current_velocity[axis];
// Accumulate velocities squared
velocity_sqr += square(jr.current_velocity[axis]);
for (int axis = 0; axis < AXES; axis++)
mach_set_position(axis, mp_runtime_get_work_position(axis));
- // Release buffer
- mp_free_run_buffer();
-
- mp_set_cycle(CYCLE_MACHINING);
-
- return STAT_NOOP;
+ return STAT_NOOP; // Done
}
- return mp_runtime_move_to_target(target, sqrt(velocity_sqr), time);
+ mp_runtime_set_velocity(sqrt(velocity_sqr));
+ stat_t status = mp_runtime_move_to_target(target, time);
+ if (status != STAT_OK) return status;
+
+ return STAT_EAGAIN;
}
if (move_time < MIN_BLOCK_TIME) return STAT_MINIMUM_TIME_MOVE;
}
- // Get a *cleared* buffer and setup move variables
- // Note, mp_free_run_buffer() initializes all buffer variables to zero
+ // Get a buffer. Note, new buffers are initialized to zero.
mp_buffer_t *bf = mp_get_write_buffer(); // current move pointer
if (!bf) return CM_ALARM(STAT_BUFFER_FULL_FATAL); // never fails
#include <stdio.h>
-void planner_init() {
- mp_init_buffers();
-}
+void planner_init() {mp_init_buffers();}
/*** Flush all moves in the planner
* during a hold to reset the planner. This function should not usually
* be directly called. Call mp_request_flush() instead.
*/
-void mp_flush_planner() {
- mp_init_buffers();
-}
+void mp_flush_planner() {mp_init_buffers();}
/* Performs axis mapping & conversion of length units to steps (and deals
/*** Plans the entire block list
*
* The block list is the circular buffer of planner buffers
- * (bf's). The block currently being planned is the "bf" block. The
+ * (bf's). The block currently being planned is the "bf" block. The
* "first block" is the next block to execute; queued immediately
* behind the currently executing block, aka the "running" block.
* In some cases, there is no first block because the list is empty
void mp_replan_blocks() {
mp_buffer_t *bf = mp_get_run_buffer();
+ if (!bf) return;
+
mp_buffer_t *bp = bf;
// Mark all blocks replanable
while (true) {
- if (bp->run_state != MOVE_OFF || fp_ZERO(bp->exit_velocity)) break;
bp->replannable = true;
-
+ if (bp->nx->run_state == MOVE_OFF || bp->nx == bf) break;
bp = mp_get_next_buffer(bp);
- if (bp == bf) break; // Avoid wrap around
}
// Plan blocks
float mp_runtime_get_velocity() {return rt.velocity;}
+void mp_runtime_set_velocity(float velocity) {
+ rt.velocity = velocity;
+ report_request();
+}
+
+
/// Set encoder counts to the runtime position
void mp_runtime_set_steps_from_position() {
// Convert lengths to steps in floating point
/// Set runtime position for a single axis
-void mp_runtime_set_position(uint8_t axis, const float position) {
+void mp_runtime_set_axis_position(uint8_t axis, const float position) {
rt.position[axis] = position;
+ report_request();
}
/// Returns current axis position in machine coordinates
-float mp_runtime_get_position(uint8_t axis) {return rt.position[axis];}
-float *mp_runtime_get_position_vector() {return rt.position;}
+float mp_runtime_get_axis_position(uint8_t axis) {return rt.position[axis];}
+float *mp_runtime_get_position() {return rt.position;}
+
+
+void mp_runtime_set_position(float position[]) {
+ copy_vector(rt.position, position);
+ report_request();
+}
/// Returns axis position in work coordinates that were in effect at plan time
* 90 100 -10 encoder is 10 steps behind commanded steps
* -100 -90 -10 encoder is 10 steps behind commanded steps
*/
-stat_t mp_runtime_move_to_target(float target[], float velocity, float time) {
+stat_t mp_runtime_move_to_target(float target[], float time) {
// Bucket-brigade old target down the chain before getting new target
- for (int i = 0; i < MOTORS; i++) {
+ for (int motor = 0; motor < MOTORS; motor++) {
// previous segment's position, delayed by 1 segment
- rt.steps.commanded[i] = rt.steps.position[i];
+ rt.steps.commanded[motor] = rt.steps.position[motor];
// previous segment's target becomes position
- rt.steps.position[i] = rt.steps.target[i];
+ rt.steps.position[motor] = rt.steps.target[motor];
// current encoder position (aligns to commanded_steps)
- rt.steps.encoder[i] = motor_get_encoder(i);
- rt.steps.error[i] = rt.steps.encoder[i] - rt.steps.commanded[i];
+ rt.steps.encoder[motor] = motor_get_encoder(motor);
+ rt.steps.error[motor] = rt.steps.encoder[motor] - rt.steps.commanded[motor];
}
// Convert target position to steps.
// works for Cartesian kinematics. Other kinematics may require
// transforming travel distance as opposed to simply subtracting steps.
float travel_steps[MOTORS];
- for (int i = 0; i < MOTORS; i++)
- travel_steps[i] = rt.steps.target[i] - rt.steps.position[i];
+ for (int motor = 0; motor < MOTORS; motor++)
+ travel_steps[motor] = rt.steps.target[motor] - rt.steps.position[motor];
// Call the stepper prep function
RITORNO(st_prep_line(travel_steps, rt.steps.error, time));
- // Update position and velocity
- copy_vector(rt.position, target);
- rt.velocity = velocity;
-
- report_request(); // Position and possibly velocity have changed
+ // Update position
+ mp_runtime_set_position(target);
return STAT_OK;
}
void mp_runtime_set_line(int32_t line);
float mp_runtime_get_velocity();
+void mp_runtime_set_velocity(float velocity);
void mp_runtime_set_steps_from_position();
-void mp_runtime_set_position(uint8_t axis, const float position);
-float mp_runtime_get_position(uint8_t axis);
-float *mp_runtime_get_position_vector();
+
+void mp_runtime_set_axis_position(uint8_t axis, const float position);
+float mp_runtime_get_axis_position(uint8_t axis);
+
+float *mp_runtime_get_position();
+void mp_runtime_set_position(float position[]);
+
float mp_runtime_get_work_position(uint8_t axis);
void mp_runtime_set_work_offsets(float offset[]);
-stat_t mp_runtime_move_to_target(float target[], float velocity, float time);
+stat_t mp_runtime_move_to_target(float target[], float time);
void mp_set_cycle(mp_cycle_t cycle) {
if (ps.cycle == cycle) return; // No change
- if (ps.state != STATE_READY) {
+ if (ps.state != STATE_READY && cycle != CYCLE_MACHINING) {
STATUS_ERROR(STAT_INTERNAL_ERROR, "Cannot transition to %S while %S",
mp_get_cycle_pgmstr(cycle),
mp_get_state_pgmstr(ps.state));
// Reset to actual machine position. Otherwise machine is set to the
// position of the last queued move.
for (int axis = 0; axis < AXES; axis++)
- mach_set_position(axis, mp_runtime_get_position(axis));
+ mach_set_position(axis, mp_runtime_get_axis_position(axis));
}
// Resume
if (mp_get_state() == STATE_HOLDING) {
// Check if any moves are buffered
- if (mp_get_run_buffer()) {
+ if (!mp_queue_empty()) {
mp_replan_blocks();
_set_state(STATE_RUNNING);
// update the model with actual position
mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
- mp_set_cycle(CYCLE_MACHINING);
}
move_type_t move_type;
uint16_t seg_period;
uint32_t prep_dwell;
- mp_buffer_t *bf; // used for command moves
+ mach_func_t mach_func; // used for command moves
+ float values[AXES];
+ float flags[AXES];
} stepper_t;
/// Return true if motors or dwell are running
-uint8_t st_is_busy() {return st.busy;}
+bool st_is_busy() {return st.busy;}
/// Interrupt handler for calling move exec function.
/// ADC channel 0 triggered by load ISR as a "software" interrupt.
ISR(ADCB_CH0_vect) {
- mp_exec_move();
+ stat_t status = mp_exec_move();
+
+ switch (status) {
+ case STAT_OK: case STAT_NOOP: case STAT_EAGAIN: break; // Ok
+ default: CM_ALARM(status); break;
+ }
+
ADCB_CH0_INTCTRL = 0;
st.requesting = false;
}
st.dwell = st.prep_dwell;
} else if (st.move_type == MOVE_TYPE_COMMAND)
- mp_command_callback(st.bf); // Execute command
+ st.mach_func(st.values, st.flags); // Execute command
// We are done with this move
st.move_type = MOVE_TYPE_NULL;
/// Stage command to execution
-void st_prep_command(mp_buffer_t *bf) {
+void st_prep_command(mach_func_t mach_func, float values[], float flags[]) {
if (st.move_ready) CM_ALARM(STAT_INTERNAL_ERROR);
st.move_type = MOVE_TYPE_COMMAND;
- st.bf = bf;
+ st.mach_func = mach_func;
+ copy_vector(st.values, values);
+ copy_vector(st.flags, flags);
st.move_ready = true; // signal prep buffer ready
}
void stepper_init();
void st_shutdown();
-uint8_t st_is_busy();
+bool st_is_busy();
stat_t st_prep_line(float travel_steps[], float following_error[],
float segment_time);
-void st_prep_command(mp_buffer_t *bf);
+void st_prep_command(mach_func_t mach_func, float values[], float flags[]);
void st_prep_dwell(float seconds);
\******************************************************************************/
-/* util contains:
- * - math and min/max utilities and extensions
- * - vector manipulation utilities
- */
-
#include "util.h"
-#include <avr/pgmspace.h>
-
-#include <math.h>
-#include <stdbool.h>
-#include <string.h>
-#include <ctype.h>
-#include <stdio.h>
-
-float vector[AXES]; // statically allocated global for vector utilities
-
-
-/// Test if vectors are equal
-uint8_t vector_equal(const float a[], const float b[]) {
- return
- fp_EQ(a[AXIS_X], b[AXIS_X]) && fp_EQ(a[AXIS_Y], b[AXIS_Y]) &&
- fp_EQ(a[AXIS_Z], b[AXIS_Z]) && fp_EQ(a[AXIS_A], b[AXIS_A]) &&
- fp_EQ(a[AXIS_B], b[AXIS_B]) && fp_EQ(a[AXIS_C], b[AXIS_C]);
-}
-
-/// Return the length of an axis vector
float get_axis_vector_length(const float a[], const float b[]) {
- return sqrt(square(a[AXIS_X] - b[AXIS_X] +
- square(a[AXIS_Y] - b[AXIS_Y]) +
- square(a[AXIS_Z] - b[AXIS_Z]) +
- square(a[AXIS_A] - b[AXIS_A]) +
- square(a[AXIS_B] - b[AXIS_B]) +
- square(a[AXIS_C] - b[AXIS_C])));
-}
-
-
-/// Load values into vector form
-float *set_vector(float x, float y, float z, float a, float b, float c) {
- vector[AXIS_X] = x;
- vector[AXIS_Y] = y;
- vector[AXIS_Z] = z;
- vector[AXIS_A] = a;
- vector[AXIS_B] = b;
- vector[AXIS_C] = c;
-
- return vector;
-}
-
-
-/// load a single value into a zero vector
-float *set_vector_by_axis(float value, uint8_t axis) {
- clear_vector(vector);
-
- switch (axis) {
- case AXIS_X: vector[AXIS_X] = value; break;
- case AXIS_Y: vector[AXIS_Y] = value; break;
- case AXIS_Z: vector[AXIS_Z] = value; break;
- case AXIS_A: vector[AXIS_A] = value; break;
- case AXIS_B: vector[AXIS_B] = value; break;
- case AXIS_C: vector[AXIS_C] = value; break;
- }
-
- return vector;
-}
-
-
-/**** Math and other general purpose functions ****/
-
-/* Slightly faster (*) multi-value min and max functions
- * min3() - return minimum of 3 numbers
- * min4() - return minimum of 4 numbers
- * max3() - return maximum of 3 numbers
- * max4() - return maximum of 4 numbers
- *
- * Implementation tip: Order the min and max values from most to least likely
- * in the calling args
- *
- * (*) Macro min4 is about 20usec, inline function version is closer to 10
- * usec (Xmega 32 MHz)
- * #define min3(a,b,c) (min(min(a,b),c))
- * #define min4(a,b,c,d) (min(min(a,b),min(c,d)))
- * #define max3(a,b,c) (max(max(a,b),c))
- * #define max4(a,b,c,d) (max(max(a,b),max(c,d)))
- */
-float min3(float x1, float x2, float x3) {
- float min = x1;
- if (x2 < min) min = x2;
- if (x3 < min) return x3;
- return min;
-}
-
-
-float min4(float x1, float x2, float x3, float x4) {
- float min = x1;
- if (x2 < min) min = x2;
- if (x3 < min) min = x3;
- if (x4 < min) return x4;
- return min;
-}
-
-
-float max3(float x1, float x2, float x3) {
- float max = x1;
- if (x2 > max) max = x2;
- if (x3 > max) return x3;
- return max;
-}
-
-
-float max4(float x1, float x2, float x3, float x4) {
- float max = x1;
- if (x2 > max) max = x2;
- if (x3 > max) max = x3;
- if (x4 > max) return x4;
- return max;
-}
-
-
-/// isdigit that also accepts plus, minus, and decimal point
-uint8_t isnumber(char c) {
- return c == '.' || c == '-' || c == '+' || isdigit(c);
-}
-
-
-/// Add escapes to a string - currently for quotes only
-char *escape_string(char *dst, char *src) {
- char c;
- char *start_dst = dst;
-
- while ((c = *(src++)) != 0) { // 0
- if (c == '"') *(dst++) = '\\';
- *(dst++) = c;
- }
-
- return start_dst;
-}
-
-
-/*
- * Return ASCII string given a float and a decimal precision value
- *
- * Returns length of string, less the terminating 0 character
- */
-char fntoa(char *str, float n, uint8_t precision) {
- // handle special cases
- if (isnan(n)) {
- strcpy(str, "nan");
- return 3;
- }
-
- if (isinf(n)) {
- strcpy(str, "inf");
- return 3;
- }
-
- switch (precision) {
- case 0: return (char)sprintf((char *)str, "%0.0f", (double)n);
- case 1: return (char)sprintf((char *)str, "%0.1f", (double)n);
- case 2: return (char)sprintf((char *)str, "%0.2f", (double)n);
- case 3: return (char)sprintf((char *)str, "%0.3f", (double)n);
- case 4: return (char)sprintf((char *)str, "%0.4f", (double)n);
- case 5: return (char)sprintf((char *)str, "%0.5f", (double)n);
- case 6: return (char)sprintf((char *)str, "%0.6f", (double)n);
- case 7: return (char)sprintf((char *)str, "%0.7f", (double)n);
- default: return (char)sprintf((char *)str, "%f", (double)n);
- }
-}
-
-
-/*
- * Calculate the checksum for a string
- *
- * Stops calculation on null termination or length value if non-zero.
- *
- * This is based on the the Java hashCode function.
- * See http://en.wikipedia.org/wiki/Java_hashCode()
- */
-#define HASHMASK 9999
-
-uint16_t compute_checksum(char const *string, const uint16_t length) {
- uint32_t h = 0;
- uint16_t len = strlen(string);
-
- if (length) len = min(len, length);
-
- for (uint16_t i = 0; i < len; i++)
- h = 31 * h + string[i];
-
- return h % HASHMASK;
+ return sqrt(square(a[AXIS_X] - b[AXIS_X]) + square(a[AXIS_Y] - b[AXIS_Y]) +
+ square(a[AXIS_Z] - b[AXIS_Z]) + square(a[AXIS_A] - b[AXIS_A]) +
+ square(a[AXIS_B] - b[AXIS_B]) + square(a[AXIS_C] - b[AXIS_C]));
}
\******************************************************************************/
-/* Supporting functions including:
- *
- * - math and min/max utilities and extensions
- * - vector manipulation utilities
- * - support for debugging routines
- */
-
#pragma once
#include <stdint.h>
#include <math.h>
+#include <string.h>
-// Vector utilities
-extern float vector[AXES]; // vector of axes for passing to subroutines
+// Vector utilities
#define clear_vector(a) memset(a, 0, sizeof(a))
#define copy_vector(d, s) memcpy(d, s, sizeof(d))
-
float get_axis_vector_length(const float a[], const float b[]);
-uint8_t vector_equal(const float a[], const float b[]);
-float *set_vector(float x, float y, float z, float a, float b, float c);
-float *set_vector_by_axis(float value, uint8_t axis);
// Math utilities
-float min3(float x1, float x2, float x3);
-float min4(float x1, float x2, float x3, float x4);
-float max3(float x1, float x2, float x3);
-float max4(float x1, float x2, float x3, float x4);
-
-
-// String utilities
-uint8_t isnumber(char c);
-char *escape_string(char *dst, char *src);
-char fntoa(char *str, float n, uint8_t precision);
-uint16_t compute_checksum(char const *string, const uint16_t length);
-
-
-// side-effect safe forms of min and max
-#ifndef max
-#define max(a, b) \
- ({ __typeof__ (a) termA = (a); \
- __typeof__ (b) termB = (b); \
- termA > termB ? termA : termB;})
-#endif
-
-#ifndef min
-#define min(a, b) \
- ({ __typeof__ (a) term1 = (a); \
- __typeof__ (b) term2 = (b); \
- term1 < term2 ? term1 : term2;})
-#endif
-
-#ifndef avg
-#define avg(a, b) ((a + b) / 2)
-#endif
-
-// allowable rounding error for floats
-#ifndef EPSILON
-#define EPSILON 0.00001
-#endif
-
-#ifndef fp_EQ
+inline float min(float a, float b) {return a < b ? a : b;}
+inline float max(float a, float b) {return a < b ? b : a;}
+inline float min3(float a, float b, float c) {return min(min(a, b), c);}
+inline float max3(float a, float b, float c) {return max(max(a, b), c);}
+inline float min4(float a, float b, float c, float d)
+{return min(min(a, b), min(c, d));}
+inline float max4(float a, float b, float c, float d)
+{return max(max(a, b), max(c, d));}
+
+// Floating-point utilities
+#define EPSILON 0.00001 // allowable rounding error for floats
#define fp_EQ(a, b) (fabs(a - b) < EPSILON)
-#endif
-#ifndef fp_NE
#define fp_NE(a, b) (fabs(a - b) > EPSILON)
-#endif
-#ifndef fp_ZERO
#define fp_ZERO(a) (fabs(a) < EPSILON)
-#endif
-#ifndef fp_NOT_ZERO
#define fp_NOT_ZERO(a) (fabs(a) > EPSILON)
-#endif
-/// float is interpreted as FALSE (equals zero)
-#ifndef fp_FALSE
-#define fp_FALSE(a) (a < EPSILON)
-#endif
-/// float is interpreted as TRUE (not equal to zero)
-#ifndef fp_TRUE
-#define fp_TRUE(a) (a > EPSILON)
-#endif
+#define fp_FALSE(a) fp_ZERO(a)
+#define fp_TRUE(a) fp_NOT_ZERO(a)
// Constants
-#define MAX_LONG 2147483647
-#define MAX_ULONG 4294967295
#define MM_PER_INCH 25.4
#define INCHES_PER_MM (1 / 25.4)
#define MICROSECONDS_PER_MINUTE 60000000.0
#define usec(a) ((a) * MICROSECONDS_PER_MINUTE)
-
-#define RADIAN 57.2957795
-#ifndef M_SQRT3
-#define M_SQRT3 1.73205080756888
-#endif
#include "plan/buffer.h"
-float get_position(int index) {return mp_runtime_get_position(index);}
+float get_position(int index) {return mp_runtime_get_axis_position(index);}
float get_velocity() {return mp_runtime_get_velocity();}
float get_speed() {return mach_get_spindle_speed();}
float get_feed() {return mach_get_feed_rate();}
projects / bbctrl-firmware / commitdiff