static void _homing_axis_start(int8_t axis);
+typedef enum { // applies to mach.homing_state
+ HOMING_NOT_HOMED, // machine is not homed
+ HOMING_HOMED, // machine is homed
+ HOMING_WAITING, // machine waiting to be homed
+} homingState_t;
+
+
/// persistent homing runtime variables
struct hmHomingSingleton {
+ homingState_t state; // homing cycle sub-state machine
+ bool homed[AXES]; // individual axis homing flags
+
// controls for homing cycle
int8_t axis; // axis currently being homed
float saved_feed_rate; // F setting
float saved_jerk; // saved and restored for each axis homed
};
-static struct hmHomingSingleton hm;
+
+
+static struct hmHomingSingleton hm = {0,};
// G28.2 homing cycle
static void _homing_axis_set_zero(int8_t axis) {
if (hm.set_coordinates) {
mach_set_position(axis, 0);
- mach.homed[axis] = true;
+ hm.homed[axis] = true;
} else // do not set axis if in G28.4 cycle
mach_set_position(axis, mp_get_runtime_work_position(axis));
// get the first or next axis
if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error
if (axis == -1) { // -1 is done
- mach.homing_state = HOMING_HOMED;
+ hm.state = HOMING_HOMED;
_homing_finalize_exit();
return;
}
// clear the homed flag for axis to move w/o triggering soft limits
- mach.homed[axis] = false;
+ hm.homed[axis] = false;
// trap axis mis-configurations
if (fp_ZERO(mach.a[axis].search_velocity))
void mach_set_not_homed() {
// TODO save homed to EEPROM
for (int axis = 0; axis < AXES; axis++)
- mach.homed[axis] = false;
+ hm.homed[axis] = false;
+}
+
+
+bool mach_get_homed(int axis) {
+ return hm.homed[axis];
+}
+
+
+void mach_set_homed(int axis, bool homed) {
+ hm.homed[axis] = homed;
}
hm.axis = -1; // set to retrieve initial axis
hm.func = _homing_axis_start; // bind initial processing function
mp_set_cycle(CYCLE_HOMING);
- mach.homing_state = HOMING_NOT_HOMED;
+ hm.state = HOMING_NOT_HOMED;
}
bool mach_is_homing();
void mach_set_not_homed();
+bool mach_get_homed(int axis);
+void mach_set_homed(int axis, bool homed);
void mach_homing_cycle_start();
void mach_homing_cycle_start_no_set();
void mach_homing_callback();
#include "usart.h" // for serial queue flush
#include "estop.h"
#include "report.h"
+#include "homing.h"
#include "plan/planner.h"
#include "plan/buffer.h"
#include <stdio.h>
+#define DISABLE_SOFT_LIMIT -1000000
+
+
machine_t mach = {
// Offsets
.offset = {
// Machine State functions
uint32_t mach_get_line() {return mach.gm.line;}
-machHomingState_t mach_get_homing_state() {return mach.homing_state;}
machMotionMode_t mach_get_motion_mode() {return mach.gm.motion_mode;}
machCoordSystem_t mach_get_coord_system() {return mach.gm.coord_system;}
machUnitsMode_t mach_get_units_mode() {return mach.gm.units_mode;}
}
-/// Set endpoint position from final runtime position
-void mach_update_model_position_from_runtime() {
- copy_vector(mach.position, mr.ms.target);
-}
-
-
/* Set target vector in GM model
*
* This is a core routine. It handles:
stat_t mach_test_soft_limits(float target[]) {
#ifdef SOFT_LIMIT_ENABLE
for (int axis = 0; axis < AXES; axis++) {
- if (!mach.homed[axis]) continue; // don't test axes that are not homed
+ if (!mach_get_homed(axis)) continue; // don't test axes that arent homed
if (fp_EQ(mach.a[axis].travel_min, mach.a[axis].travel_max)) continue;
for (int axis = 0; axis < AXES; axis++)
if (fp_TRUE(flag[axis])) {
mp_set_runtime_position(axis, value[axis]);
- mach.homed[axis] = true; // G28.3 is not considered homed until here
+ mach_set_homed(axis, true); // G28.3 is not considered homed until here
}
mp_set_steps_to_runtime_position();
#define TO_MILLIMETERS(a) (mach.gm.units_mode == INCHES ? (a) * MM_PER_INCH : a)
-#define DISABLE_SOFT_LIMIT -1000000
-
-
-
-typedef enum { // applies to mach.homing_state
- HOMING_NOT_HOMED, // machine is not homed
- HOMING_HOMED, // machine is homed
- HOMING_WAITING, // machine waiting to be homed
-} machHomingState_t;
-
-
-typedef enum { // applies to mach.probe_state
- PROBE_FAILED, // probe reached endpoint without triggering
- PROBE_SUCCEEDED, // probe was triggered, mach.probe_results has position
- PROBE_WAITING, // probe is waiting to be started
-} machProbeState_t;
-
/* The difference between machNextAction_t and machMotionMode_ is that
* machNextAction_t is used by the current block, and may carry non-modal
float g28_position[AXES]; // stored machine position for G28
float g30_position[AXES]; // stored machine position for G30
- // settings for axes X,Y,Z,A B,C
- AxisConfig_t a[AXES];
-
- machHomingState_t homing_state; // home: homing cycle sub-state machine
- bool homed[AXES]; // individual axis homing flags
-
- machProbeState_t probe_state;
- float probe_results[AXES]; // probing results
+ AxisConfig_t a[AXES]; // settings for axes
// Model states
MoveState_t ms;
// Model state getters and setters
uint32_t mach_get_line();
-machHomingState_t mach_get_homing_state();
machMotionMode_t mach_get_motion_mode();
machCoordSystem_t mach_get_coord_system();
machUnitsMode_t mach_get_units_mode();
// Critical helpers
void mach_calc_move_time(const float axis_length[], const float axis_square[]);
-void mach_update_model_position_from_runtime();
void mach_finalize_move();
stat_t mach_deferred_write_callback();
void mach_set_model_target(float target[], float flag[]);
// Machining Functions (4.3.6)
stat_t mach_straight_feed(float target[], float flags[]);
-stat_t mach_arc_feed(float target[], float flags[],
- float i, float j, float k,
- float radius, uint8_t motion_mode);
+stat_t mach_arc_feed(float target[], float flags[], float i, float j, float k,
+ float radius, uint8_t motion_mode);
stat_t mach_dwell(float seconds);
// Spindle Functions (4.3.7) see spindle.h
* approximated by generating a large number of tiny, linear arc_segments.
*/
stat_t mach_arc_feed(float target[], float flags[], // arc endpoints
- float i, float j, float k, // raw arc offsets
- float radius, // non-zero radius implies radius mode
- uint8_t motion_mode) { // defined motion mode
+ float i, float j, float k, // raw arc offsets
+ float radius, // non-zero radius implies radius mode
+ uint8_t motion_mode) { // defined motion mode
// Set axis plane and trap arc specification errors
// trap missing feed rate
/// Set planner position for a single axis
-void mp_set_planner_position(uint8_t axis, const float position) {
+void mp_set_planner_position(int axis, const float position) {
mm.position[axis] = position;
}
* the minimums.
*/
stat_t mp_aline(MoveState_t *ms) {
- 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];
// 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
+ // will 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
//
// 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])
+ //
+ // T = 2 * sqrt((S - E) / J)
//
// Since E is always zero in this calculation, we can simplify:
- // T = 2 * sqrt(S / J[n])
+ //
+ // T = 2 * sqrt(S / J)
//
// 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*,
+ // The final value we want to know is 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
// 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. 0Using that, we
+ // 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
// 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)
+ // For each axis n:
//
- // Keeping in mind that we only need a rank compared to the other
- // axes, we can further optimize the calculations::
+ // C[n] = sqrt(1 / J[n]) * (D[n] / L)
//
- // 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.)
+ // Keeping in mind that we only need a rank compared to the other
+ // axes, we can further optimize the calculations:
//
- // 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
+ // Square the expression to remove the square root:
//
- // Also note that we already have 1 / J[n] calculated for each axis,
- // which simplifies it further.
+ // C[n]^2 = (1 / J[n]) * (D[n] / L)^2
+ //
+ // We don't care the C is squared, so 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
// finish up the current block variables
// exact stop cases already zeroed
+ float exact_stop = 0;
if (mach_get_path_control() != PATH_EXACT_STOP) {
bf->replannable = true;
exact_stop = 8675309; // an arbitrarily large floating point number
}
+ float junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit);
bf->cruise_vmax = bf->length / bf->ms.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->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
+ // NOTE: these next lines must remain in exact order. Position must update
// before committing the buffer.
+ uint8_t mr_flag = false;
mp_plan_block_list(bf, &mr_flag); // replan block list
copy_vector(mm.position, bf->ms.target); // set the planner position
// commit current block (must follow the position update)
#pragma once
-#include "buffer.h"
+#include "machine.h"
-void mp_set_planner_position(uint8_t axis, const float position);
+void mp_set_planner_position(int axis, const float position);
stat_t mp_aline(MoveState_t *ms);
void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag);
float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf);
float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf);
+inline int32_t mp_get_line() {return mr.ms.line;}
#define MINIMUM_PROBE_TRAVEL 0.254
+typedef enum { // applies to mach.probe_state
+ PROBE_FAILED, // probe reached endpoint without triggering
+ PROBE_SUCCEEDED, // probe was triggered, pb.results has position
+ PROBE_WAITING, // probe is waiting to be started
+} probeState_t;
+
+
struct pbProbingSingleton { // persistent probing runtime variables
+ probeState_t state;
+ float results[AXES]; // probing results
+
void (*func)(); // binding for callback function state machine
// state saved from gcode model
float flags[AXES];
};
-static struct pbProbingSingleton pb;
+
+static struct pbProbingSingleton pb = {0,};
/* Note: When coding a cycle (like this one) you get to perform one
static void _probing_finish() {
bool closed = switch_is_active(SW_PROBE);
- mach.probe_state = closed ? PROBE_SUCCEEDED : PROBE_FAILED;
+ pb.state = closed ? PROBE_SUCCEEDED : PROBE_FAILED;
for (int axis = 0; axis < AXES; axis++) {
// if we got here because of a feed hold keep the model position correct
mach_set_position(axis, mp_get_runtime_work_position(axis));
// store the probe results
- mach.probe_results[axis] = mach_get_absolute_position(axis);
+ pb.results[axis] = mach_get_absolute_position(axis);
}
_probe_restore_settings();
// NOTE: it is *not* an error condition for the probe not to trigger.
// it is an error for the limit or homing switches to fire, or for some other
// configuration error.
- mach.probe_state = PROBE_FAILED;
+ pb.state = PROBE_FAILED;
mp_set_cycle(CYCLE_PROBING);
// initialize the axes - save the jerk settings & switch to the jerk_homing
bool mach_is_probing() {
- return mp_get_cycle() == CYCLE_PROBING || mach.probe_state == PROBE_WAITING;
+ return mp_get_cycle() == CYCLE_PROBING || pb.state == PROBE_WAITING;
}
// set probe move endpoint
copy_vector(pb.target, target); // set probe move endpoint
copy_vector(pb.flags, flags); // set axes involved on the move
- clear_vector(mach.probe_results); // clear the old probe position.
- // NOTE: relying on probe_results will not detect a probe to (0, 0, 0).
+ clear_vector(pb.results); // clear the old probe position.
+ // NOTE: relying on pb.results will not detect a probe to (0, 0, 0).
// wait until planner queue empties before completing initialization
- mach.probe_state = PROBE_WAITING;
+ pb.state = PROBE_WAITING;
pb.func = _probing_init; // bind probing initialization function
return STAT_OK;
bool get_echo() {return usart_is_set(USART_ECHO);}
void set_echo(bool value) {return usart_set(USART_ECHO, value);}
uint16_t get_queue() {return mp_get_planner_buffer_room();}
-int32_t get_line() {return mr.ms.line;}
+int32_t get_line() {return mp_get_line();}
PGM_P get_state() {return mp_get_state_pgmstr(mp_get_state());}
PGM_P get_cycle() {return mp_get_cycle_pgmstr(mp_get_cycle());}
projects / bbctrl-firmware / commitdiff