From: Joseph Coffland Date: Sun, 11 Sep 2016 09:07:05 +0000 (-0700) Subject: Removed plan/command.c, Moved CM_ALARM X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=7e6eadf4e0f556749aae56e8793cc0095ff6798a;p=bbctrl-firmware Removed plan/command.c, Moved CM_ALARM --- diff --git a/src/machine.c b/src/machine.c index 5d3aa6e..0a758f5 100644 --- a/src/machine.c +++ b/src/machine.c @@ -44,16 +44,14 @@ * - Call the mach_xxx_xxx() function which will do any input validation and * return an error if it detects one. * - * - The mach_ function calls mp_command_queue(). Arguments are a callback to + * - The mach_ function calls mp_queue_push(). Arguments are a callback to * the _exec_...() function, which is the runtime execution routine, and any - * arguments that are needed by the runtime. See typedef for *exec in - * planner.h for details + * arguments that are needed by the runtime. * - * - mp_command_queue() stores the callback and the args in a planner buffer. + * - mp_queue_push() stores the callback and the args in a planner buffer. * * - When planner execution reaches the buffer it executes the callback w/ the - * args. Take careful note that the callback executes under an interrupt, - * so beware of variables that may need to be volatile. + * args. Take careful note that the callback executes under an interrupt. * * Note: The synchronous command execution mechanism uses 2 vectors in the bf * buffer to store and return values for the callback. It's obvious, but @@ -71,14 +69,12 @@ #include "switch.h" #include "hardware.h" #include "util.h" -#include "estop.h" #include "report.h" #include "homing.h" #include "plan/planner.h" #include "plan/runtime.h" #include "plan/dwell.h" -#include "plan/command.h" #include "plan/arc.h" #include "plan/line.h" #include "plan/state.h" @@ -212,32 +208,32 @@ void mach_set_motion_mode(motion_mode_t motion_mode) { /// Spindle speed callback from planner queue -static void _exec_spindle_speed(float *value, float *flag) { - float speed = value[0]; - mach.gm.spindle_speed = speed; - spindle_set(mach.gm.spindle_mode, speed); +static stat_t _exec_spindle_speed(mp_buffer_t *bf) { + spindle_set_speed(bf->value); + return STAT_NOOP; // No move queued } /// Queue the S parameter to the planner buffer void mach_set_spindle_speed(float speed) { - float value[AXES] = {speed}; - mp_command_queue(_exec_spindle_speed, value, value); + mp_buffer_t *bf = mp_queue_get_tail(); + bf->value = speed; + mp_queue_push(_exec_spindle_speed, mach_get_line()); } /// execute the spindle command (called from planner) -static void _exec_spindle_mode(float *value, float *flag) { - spindle_mode_t mode = value[0]; - mach.gm.spindle_mode = mode; - spindle_set(mode, mach.gm.spindle_speed); +static stat_t _exec_spindle_mode(mp_buffer_t *bf) { + spindle_set_mode(bf->value); + return STAT_NOOP; // No move queued } /// Queue the spindle command to the planner buffer void mach_set_spindle_mode(spindle_mode_t mode) { - float value[AXES] = {mode}; - mp_command_queue(_exec_spindle_mode, value, value); + mp_buffer_t *bf = mp_queue_get_tail(); + bf->value = mode; + mp_queue_push(_exec_spindle_mode, mach_get_line()); } @@ -327,7 +323,7 @@ void mach_update_work_offsets() { if (!same) { mp_buffer_t *bf = mp_queue_get_tail(); copy_vector(bf->target, work_offset); - mp_queue_push(_exec_update_work_offsets, mach.gm.line); + mp_queue_push(_exec_update_work_offsets, mach_get_line()); } } @@ -479,8 +475,6 @@ static float _calc_ABC(uint8_t axis, float target[], float flag[]) { void mach_set_model_target(float target[], float flag[]) { - float tmp = 0; - // process XYZABC for lower modes for (int axis = AXIS_X; axis <= AXIS_Z; axis++) { if (fp_FALSE(flag[axis]) || axes[axis].axis_mode == AXIS_DISABLED) @@ -500,7 +494,7 @@ void mach_set_model_target(float target[], float flag[]) { if (fp_FALSE(flag[axis]) || axes[axis].axis_mode == AXIS_DISABLED) continue; // skip axis if not flagged for update or its disabled - tmp = _calc_ABC(axis, target, flag); + float tmp = _calc_ABC(axis, target, flag); if (mach.gm.distance_mode == ABSOLUTE_MODE) // sacidu93's fix to Issue #22 @@ -569,14 +563,6 @@ void machine_init() { } -/// Alarm state; send an exception report and stop processing input -stat_t mach_alarm(const char *location, stat_t code) { - status_message_P(location, STAT_LEVEL_ERROR, code, 0); - estop_trigger(ESTOP_ALARM); - return code; -} - - // Representation (4.3.3) // // Affect the Gcode model only (asynchronous) @@ -633,7 +619,7 @@ void mach_set_coord_system(coord_system_t coord_system) { * the planner and that all motion has stopped. */ void mach_set_axis_position(unsigned axis, float position) { - //if (!mp_is_quiescent()) CM_ALARM(STAT_MACH_NOT_QUIESCENT); + //if (!mp_is_quiescent()) ALARM(STAT_MACH_NOT_QUIESCENT); if (AXES <= axis) return; mach.position[axis] = position; @@ -665,14 +651,19 @@ stat_t mach_zero_axis(unsigned axis) { // G28.3 functions and support -static void _exec_absolute_origin(float *value, float *flag) { +static stat_t _exec_absolute_origin(mp_buffer_t *bf) { + const float *origin = bf->target; + const float *flags = bf->unit; + for (int axis = 0; axis < AXES; axis++) - if (fp_TRUE(flag[axis])) { - mp_runtime_set_axis_position(axis, value[axis]); + if (fp_TRUE(flags[axis])) { + mp_runtime_set_axis_position(axis, origin[axis]); mach_set_homed(axis, true); // G28.3 is not considered homed until here } mp_runtime_set_steps_from_position(); + + return STAT_NOOP; // No move queued } @@ -680,7 +671,7 @@ static void _exec_absolute_origin(float *value, float *flag) { * * Takes a vector of origins (presumably 0's, but not necessarily) and * applies them to all axes where the corresponding position in the - * flag vector is true (1). + * flags vector is true (1). * * This is a 2 step process. The model and planner contexts are set * immediately, the runtime command is queued and synchronized with @@ -688,18 +679,21 @@ static void _exec_absolute_origin(float *value, float *flag) { * recording done by the encoders. At that point any axis that is set * is also marked as homed. */ -void mach_set_absolute_origin(float origin[], float flag[]) { +void mach_set_absolute_origin(float origin[], float flags[]) { float value[AXES]; for (int axis = 0; axis < AXES; axis++) - if (fp_TRUE(flag[axis])) { + if (fp_TRUE(flags[axis])) { value[axis] = TO_MILLIMETERS(origin[axis]); mach.position[axis] = value[axis]; // set model position mach.gm.target[axis] = value[axis]; // reset model target mp_set_axis_position(axis, value[axis]); // set mm position } - mp_command_queue(_exec_absolute_origin, value, flag); + mp_buffer_t *bf = mp_queue_get_tail(); + copy_vector(bf->target, origin); + copy_vector(bf->unit, flags); + mp_queue_push(_exec_absolute_origin, mach_get_line()); } @@ -747,7 +741,7 @@ stat_t mach_rapid(float target[], float flags[]) { // test soft limits stat_t status = mach_test_soft_limits(mach.gm.target); - if (status != STAT_OK) return CM_ALARM(status); + if (status != STAT_OK) return ALARM(status); // prep and plan the move mach_update_work_offsets(); // update fully resolved offsets to state @@ -839,7 +833,7 @@ stat_t mach_feed(float target[], float flags[]) { // test soft limits stat_t status = mach_test_soft_limits(mach.gm.target); - if (status != STAT_OK) return CM_ALARM(status); + if (status != STAT_OK) return ALARM(status); // prep and plan the move mach_update_work_offsets(); // update fully resolved offsets to state @@ -863,28 +857,32 @@ void mach_change_tool(uint8_t tool) {mach.gm.tool = tool;} // Miscellaneous Functions (4.3.9) -static void _exec_mist_coolant_control(float *value, float *flag) { - coolant_set_mist(value[0]); +static stat_t _exec_mist_coolant(mp_buffer_t *bf) { + coolant_set_mist(bf->value); + return STAT_NOOP; // No move queued } /// M7 void mach_mist_coolant_control(bool mist_coolant) { - float value[AXES] = {mist_coolant}; - mp_command_queue(_exec_mist_coolant_control, value, value); + mp_buffer_t *bf = mp_queue_get_tail(); + bf->value = mist_coolant; + mp_queue_push(_exec_mist_coolant, mach_get_line()); } -static void _exec_flood_coolant_control(float *value, float *flag) { - coolant_set_flood(value[0]); - if (!value[0]) coolant_set_mist(false); // M9 special function +static stat_t _exec_flood_coolant(mp_buffer_t *bf) { + coolant_set_flood(bf->value); + if (!bf->value) coolant_set_mist(false); // M9 special function + return STAT_NOOP; // No move queued } /// M8, M9 void mach_flood_coolant_control(bool flood_coolant) { - float value[AXES] = {flood_coolant}; - mp_command_queue(_exec_flood_coolant_control, value, value); + mp_buffer_t *bf = mp_queue_get_tail(); + bf->value = flood_coolant; + mp_queue_push(_exec_flood_coolant, mach_get_line()); } diff --git a/src/machine.h b/src/machine.h index 05fc7a0..2a51b3e 100644 --- a/src/machine.h +++ b/src/machine.h @@ -102,12 +102,6 @@ stat_t mach_test_soft_limits(float target[]); // Initialization and termination (4.3.2) void machine_init(); -/// enter alarm state. returns same status code -stat_t mach_alarm(const char *location, stat_t status); - -#define CM_ALARM(CODE) mach_alarm(STATUS_LOCATION, CODE) -#define CM_ASSERT(COND) \ - do {if (!(COND)) CM_ALARM(STAT_INTERNAL_ERROR);} while (0) // Representation (4.3.3) void mach_set_plane(plane_t plane); diff --git a/src/motor.c b/src/motor.c index 9209998..2dda5f7 100644 --- a/src/motor.c +++ b/src/motor.c @@ -34,6 +34,7 @@ #include "stepper.h" #include "tmc2660.h" #include "estop.h" +#include "gcode_state.h" #include "util.h" #include "plan/runtime.h" @@ -317,7 +318,7 @@ void motor_error_callback(int motor, motor_flags_t errors) { motors[motor].flags |= errors; report_request(); - if (motor_error(motor)) CM_ALARM(STAT_MOTOR_ERROR); + if (motor_error(motor)) ALARM(STAT_MOTOR_ERROR); } @@ -439,7 +440,7 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, uint32_t clocks = seg_clocks >> (m->timer_clock - 1); // Motor timer clocks float steps = (float)clocks / m->timer_period; float diff = fabs(fabs(travel_steps) - steps); - if (10 < diff) CM_ALARM(STAT_STEP_CHECK_FAILED); + if (10 < diff) ALARM(STAT_STEP_CHECK_FAILED); } // Setup the direction, compensating for polarity. diff --git a/src/plan/buffer.c b/src/plan/buffer.c index 7ed8cf3..e2f76bc 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -62,7 +62,7 @@ static void _clear_buffer(mp_buffer_t *bf) { static void _push() { if (!mb.space) { - CM_ALARM(STAT_INTERNAL_ERROR); + ALARM(STAT_INTERNAL_ERROR); return; } @@ -73,7 +73,7 @@ static void _push() { static void _pop() { if (mb.space == PLANNER_BUFFER_POOL_SIZE) { - CM_ALARM(STAT_INTERNAL_ERROR); + ALARM(STAT_INTERNAL_ERROR); return; } diff --git a/src/plan/buffer.h b/src/plan/buffer.h index 8c708c2..90e1f99 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -45,7 +45,6 @@ typedef enum { // Callbacks struct mp_buffer_t; typedef stat_t (*buffer_cb_t)(struct mp_buffer_t *bf); -typedef void (*mach_cb_t)(float[], float[]); typedef struct mp_buffer_t { // See Planning Velocity Notes @@ -59,8 +58,7 @@ typedef struct mp_buffer_t { // See Planning Velocity Notes run_state_t run_state; // run state machine sequence bool replannable; // true if move can be re-planned - mach_cb_t mach_cb; // callback to machine - float dwell; + float value; // used in dwell and other callbacks float target[AXES]; // XYZABC where the move should go float unit[AXES]; // unit vector for axis scaling & planning diff --git a/src/plan/command.c b/src/plan/command.c deleted file mode 100644 index c4137d4..0000000 --- a/src/plan/command.c +++ /dev/null @@ -1,69 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -/* How this works: - * - A command is called by the Gcode interpreter (mach_, - * e.g. M code) - * - mach_ function calls mp_command_queue which puts it in the planning queue - * (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 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 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, - * and makes keeping the queue full and avoiding starvation much easier. - */ - -#include "command.h" - -#include "buffer.h" -#include "machine.h" -#include "stepper.h" -#include "util.h" - - -/// Callback to execute command -static stat_t _exec_command(mp_buffer_t *bf) { - st_prep_command(bf->mach_cb, bf->target, bf->unit); - return STAT_OK; // Done -} - - -/// Queue a synchronous Mcode, program control, or other command -void mp_command_queue(mach_cb_t mach_cb, float values[], float flags[]) { - mp_buffer_t *bf = mp_queue_get_tail(); - bf->mach_cb = mach_cb; - copy_vector(bf->target, values); - copy_vector(bf->unit, flags); - mp_queue_push(_exec_command, mach_get_line()); -} diff --git a/src/plan/command.h b/src/plan/command.h deleted file mode 100644 index 9a5c432..0000000 --- a/src/plan/command.h +++ /dev/null @@ -1,32 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "plan/buffer.h" - -void mp_command_queue(mach_cb_t mach_exec, float *value, float *flag); diff --git a/src/plan/dwell.c b/src/plan/dwell.c index 0093b12..07bba61 100644 --- a/src/plan/dwell.c +++ b/src/plan/dwell.c @@ -39,7 +39,7 @@ /// Dwell execution static stat_t _exec_dwell(mp_buffer_t *bf) { - st_prep_dwell(bf->dwell); // in seconds + st_prep_dwell(bf->value); // in seconds return STAT_OK; // Done } @@ -47,7 +47,7 @@ static stat_t _exec_dwell(mp_buffer_t *bf) { /// Queue a dwell stat_t mp_dwell(float seconds, int32_t line) { mp_buffer_t *bf = mp_queue_get_tail(); - bf->dwell = seconds; // in seconds, not minutes + bf->value = seconds; // in seconds, not minutes mp_queue_push(_exec_dwell, line); return STAT_OK; diff --git a/src/plan/line.c b/src/plan/line.c index 608a407..c3a2003 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -51,22 +51,22 @@ * (Sungeun K. Jeon's) explanation of what's going on: * * "First let's assume that at a junction we only look a centripetal - * acceleration to simply things. At a junction of two lines, let's place a - * circle such that both lines are tangent to the circle. The circular segment - * joining the lines represents the path for constant centripetal - * acceleration. This creates a deviation from the path (let's call this delta), + * acceleration to simplify things. At a junction of two lines, let's place a + * circle such that both lines are tangent to the circle. The circular segment + * joining the lines represents the path for constant centripetal acceleration. + * This creates a deviation from the path (let's call this delta), * which is the distance from the junction to the edge of the circular - * segment. Delta needs to be defined, so let's replace the term max_jerk (see - * note 1) with max_junction_deviation, or "delta". This indirectly sets the + * segment. Delta needs to be defined, so let's replace the term max_jerk (see + * note 1) with max_junction_deviation, or "delta". This indirectly sets the * radius of the circle, and hence limits the velocity by the centripetal - * acceleration. Think of the this as widening the race track. If a race car is + * acceleration. Think of the this as widening the race track. If a race car is * driving on a track only as wide as a car, it'll have to slow down a lot to - * turn corners. If we widen the track a bit, the car can start to use the track - * to go into the turn. The wider it is, the faster through the corner it can - * go. + * turn corners. If we widen the track a bit, the car can start to use the + * track to go into the turn. The wider it is, the faster through the corner + * it can go. * - * (Note 1: "max_jerk" refers to the old grbl/marlin max_jerk" approximation - * term, not the TinyG jerk terms) + * Note 1: "max_jerk" refers to the old grbl/marlin "max_jerk" approximation + * term, not the TinyG jerk terms. * * If you do the geometry in terms of the known variables, you get: * @@ -78,7 +78,7 @@ * * Theta is the angle between line segments given by: * - * cos(theta) = dot(a, b) / (norm(a) * norm(b)). + * cos(theta) = dot(a, b) / (norm(a) * norm(b)) * * Most of these calculations are already done in the planner. * To remove the acos() and sin() computations, use the trig half @@ -86,10 +86,12 @@ * * sin(theta/2) = +/-sqrt((1 - cos(theta)) / 2) * - * For our applications, this should always be positive. Now just plug the - * equations into the centripetal acceleration equation: v_c = - * sqrt(a_max*R). You'll see that there are only two sqrt computations and no - * sine/cosines." + * For our applications, this should always be positive. Now just plug the + * equations into the centripetal acceleration equation: + * + * v_c = sqrt(a_max * R) + * + * You'll see that there are only two sqrt computations and no sine/cosines. * * How to compute the radius using brute-force trig: * @@ -97,17 +99,17 @@ * float radius = delta * sin(theta/2) / (1 - sin(theta/2)); * * This version extends Chamnit's algorithm by computing a value for delta that - * takes the contributions of the individual axes in the move into account. This - * allows the control radius to vary by axis. This is necessary to support axes - * that have different dynamics; such as a Z axis that doesn't move as fast as X - * and Y (such as a screw driven Z axis on machine with a belt driven XY - like - * a Shapeoko), or rotary axes ABC that have completely different dynamics than - * their linear counterparts. + * takes the contributions of the individual axes in the move into account. + * This allows the control radius to vary by axis. This is necessary to + * support axes that have different dynamics; such as a Z axis that doesn't + * move as fast as X and Y (such as a screw driven Z axis on machine with a belt + * driven XY - like a Shapeoko), or rotary axes ABC that have completely + * different dynamics than their linear counterparts. * * The function takes the absolute values of the sum of the unit vector * components as a measure of contribution to the move, then scales the delta * values from the non-zero axes into a composite delta to be used for the - * move. Shown for an XY vector: + * move. Shown for an XY vector: * * U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i]) * Usum Length of sums Ux + Uy diff --git a/src/plan/planner.c b/src/plan/planner.c index 14d87b4..18d3545 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -242,8 +242,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { // F case: Block is too short - run time < minimum segment time // Force block into a single segment body with limited velocities // Accept the entry velocity, limit the cruise, and go for the best exit - // velocity you can get given the delta_vmax (maximum velocity slew) - // supportable. + // velocity you can get given the delta_vmax (maximum velocity slew). float naive_move_time = 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average @@ -256,8 +255,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { bf->head_length = 0; bf->tail_length = 0; - // We are violating the jerk value but since it's a single segment move we - // don't use it. + // Violating jerk but since it's a single segment move we don't use it. return; } @@ -278,8 +276,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { bf->head_length = 0; bf->tail_length = 0; - // We are violating the jerk value but since it's a single segment move we - // don't use it. + // Violating jerk but since it's a single segment move we don't use it. return; } diff --git a/src/spindle.c b/src/spindle.c index c08f972..59012af 100644 --- a/src/spindle.c +++ b/src/spindle.c @@ -32,8 +32,6 @@ #include "pwm_spindle.h" #include "huanyang.h" -#include "plan/command.h" - typedef enum { SPINDLE_TYPE_PWM, @@ -68,6 +66,26 @@ void spindle_set(spindle_mode_t mode, float speed) { } +void spindle_set_mode(spindle_mode_t mode) { + spindle.mode = mode; + + switch (spindle.type) { + case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, spindle.speed); break; + case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, spindle.speed); break; + } +} + + +void spindle_set_speed(float speed) { + spindle.speed = speed; + + switch (spindle.type) { + case SPINDLE_TYPE_PWM: pwm_spindle_set(spindle.mode, speed); break; + case SPINDLE_TYPE_HUANYANG: huanyang_set(spindle.mode, speed); break; + } +} + + spindle_mode_t spindle_get_mode() {return spindle.mode;} float spindle_get_speed() {return spindle.speed;} diff --git a/src/spindle.h b/src/spindle.h index ef15e86..4542651 100644 --- a/src/spindle.h +++ b/src/spindle.h @@ -32,7 +32,9 @@ void spindle_init(); -void spindle_set(spindle_mode_t spindle_mode, float speed); +void spindle_set(spindle_mode_t mode, float speed); +void spindle_set_mode(spindle_mode_t mode); +void spindle_set_speed(float speed); spindle_mode_t spindle_get_mode(); float spindle_get_speed(); void spindle_estop(); diff --git a/src/status.c b/src/status.c index 2ec1e66..ed09d6d 100644 --- a/src/status.c +++ b/src/status.c @@ -26,6 +26,7 @@ \******************************************************************************/ #include "status.h" +#include "estop.h" #include #include @@ -107,3 +108,11 @@ void status_help() { putchar('}'); putchar('\n'); } + + +/// Alarm state; send an exception report and stop processing input +stat_t status_alarm(const char *location, stat_t code) { + status_message_P(location, STAT_LEVEL_ERROR, code, 0); + estop_trigger(ESTOP_ALARM); + return code; +} diff --git a/src/status.h b/src/status.h index 24f222b..2615003 100644 --- a/src/status.h +++ b/src/status.h @@ -60,6 +60,9 @@ stat_t status_message_P(const char *location, status_level_t level, stat_t code, const char *msg, ...); void status_help(); +/// Enter alarm state. returns same status code +stat_t status_alarm(const char *location, stat_t status); + #define TO_STRING(x) _TO_STRING(x) #define _TO_STRING(x) #x @@ -79,3 +82,6 @@ void status_help(); #define STATUS_ERROR(CODE, MSG, ...) \ STATUS_MESSAGE(STAT_LEVEL_ERROR, CODE, MSG, ##__VA_ARGS__) + +#define ALARM(CODE) status_alarm(STATUS_LOCATION, CODE) +#define ASSERT(COND) do {if (!(COND)) ALARM(STAT_INTERNAL_ERROR);} while (0) diff --git a/src/stepper.c b/src/stepper.c index f8ce6bc..026a435 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -33,7 +33,6 @@ #include "machine.h" #include "plan/runtime.h" #include "plan/exec.h" -#include "plan/command.h" #include "motor.h" #include "hardware.h" #include "estop.h" @@ -48,7 +47,6 @@ typedef enum { MOVE_TYPE_NULL, // null move - does a no-op MOVE_TYPE_ALINE, // acceleration planned line MOVE_TYPE_DWELL, // delay with no movement - MOVE_TYPE_COMMAND, // general command } move_type_t; @@ -63,9 +61,6 @@ typedef struct { move_type_t move_type; uint16_t seg_period; uint32_t prep_dwell; - mach_cb_t mach_cb; // used for command moves - float values[AXES]; - float flags[AXES]; } stepper_t; @@ -105,10 +100,10 @@ ISR(ADCB_CH0_vect) { case STAT_EAGAIN: continue; // No command executed, try again case STAT_OK: // Move executed - if (!st.move_ready) CM_ALARM(STAT_EXPECTED_MOVE); // No move was queued + if (!st.move_ready) ALARM(STAT_EXPECTED_MOVE); // No move was queued break; - default: CM_ALARM(status); break; + default: ALARM(status); break; } break; @@ -167,9 +162,7 @@ ISR(STEP_TIMER_ISR) { // Start dwell st.dwell = st.prep_dwell; - - } else if (st.move_type == MOVE_TYPE_COMMAND) - st.mach_cb(st.values, st.flags); // Execute command + } // We are done with this move st.move_type = MOVE_TYPE_NULL; @@ -202,10 +195,10 @@ ISR(STEP_TIMER_ISR) { */ stat_t st_prep_line(float travel_steps[], float error[], float seg_time) { // Trap conditions that would prevent queueing the line - if (st.move_ready) return CM_ALARM(STAT_INTERNAL_ERROR); + if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR); if (isinf(seg_time)) - return CM_ALARM(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE); - if (isnan(seg_time)) return CM_ALARM(STAT_PREP_LINE_MOVE_TIME_IS_NAN); + return ALARM(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE); + if (isnan(seg_time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_IS_NAN); if (seg_time < EPSILON) return STAT_MINIMUM_TIME_MOVE; // Setup segment parameters @@ -223,20 +216,9 @@ stat_t st_prep_line(float travel_steps[], float error[], float seg_time) { } -/// Stage command to execution -void st_prep_command(mach_cb_t mach_cb, float values[], float flags[]) { - if (st.move_ready) CM_ALARM(STAT_INTERNAL_ERROR); - st.move_type = MOVE_TYPE_COMMAND; - st.mach_cb = mach_cb; - copy_vector(st.values, values); - copy_vector(st.flags, flags); - st.move_ready = true; // signal prep buffer ready -} - - /// Add a dwell to the move buffer void st_prep_dwell(float seconds) { - if (st.move_ready) CM_ALARM(STAT_INTERNAL_ERROR); + if (st.move_ready) ALARM(STAT_INTERNAL_ERROR); st.move_type = MOVE_TYPE_DWELL; st.seg_period = STEP_TIMER_FREQ * 0.001; // 1 ms st.prep_dwell = seconds * 1000; // convert to ms diff --git a/src/stepper.h b/src/stepper.h index ad1cdac..6ee709a 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -30,7 +30,6 @@ #pragma once #include "status.h" -#include "plan/buffer.h" #include @@ -40,5 +39,4 @@ void st_shutdown(); bool st_is_busy(); stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time); -void st_prep_command(mach_cb_t mach_cb, float values[], float flags[]); void st_prep_dwell(float seconds);