From: Joseph Coffland Date: Sat, 25 Mar 2017 01:05:37 +0000 (-0700) Subject: Working on planner numerical stability X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=ab019bc7cb5f19de71f93862b49101e3ab0dd4b3;p=bbctrl-firmware Working on planner numerical stability --- diff --git a/avr/MoveLifecycle.md b/avr/MoveLifecycle.md index f6d781b..edb3101 100644 --- a/avr/MoveLifecycle.md +++ b/avr/MoveLifecycle.md @@ -72,7 +72,7 @@ After move initialization ``mp_exec_aline()`` calls ``_exec_aline_head()``, ``_exec_aline_body()`` and ``exec_aline_tail()`` on successive callbacks. These functions are called repeatedly until each section finishes. If any sections have zero length they are skipped and execution is passed immediately -to the next section. During each section, forward differencing is used to map +to the next section. During each section, forward difference is used to map the trapezoid computed during the planning stage to a fifth-degree Bezier polynomial S-curve. The curve is used to find the appropriate velocity at the next target position. @@ -85,15 +85,15 @@ returns ``STAT_OK`` indicating the next segment should be loaded. When all non-zero segments have been executed, the move is complete. ## Pulse generation -Calls to ``st_prep_line()`` prepare short (~5ms) move fragements for pluse +Calls to ``st_prep_line()`` prepare short (~5ms) move fragments for pulse generation by the stepper driver. Move time in clock ticks is computed from travel in steps and move duration. Then ``motor_prep_move()`` is called for each motor. ``motor_prep_move()`` may perform step correction and enable the motor. It then computes the optimal step clock divisor, clock ticks and sets the move direction, taking the motor's configuration in to account. -The stepper timer ISR, after ending the previous move, calls +The stepper timer interrupt, after ending the previous move, calls ``motor_load_move()`` on each motor. This sets up and starts the motor clocks, sets the motor direction lines and accumulates and resets the step encoders. -After (re)starting the motor clocks the ISR signals a lower level interrupt to -call ``mp_exec_move()`` and load the next move fragment. +After (re)starting the motor clocks the interrupt signals a lower level +interrupt to call ``mp_exec_move()`` and load the next move fragment. diff --git a/avr/src/axis.c b/avr/src/axis.c index 8556b06..3bfa5ed 100644 --- a/avr/src/axis.c +++ b/avr/src/axis.c @@ -41,8 +41,8 @@ typedef struct { float velocity_max; // max velocity in mm/min or deg/min float travel_max; // max work envelope for soft limits float travel_min; // min work envelope for soft limits - float jerk_max; // max jerk (Jm) in mm/min^3 divided by 1 million - float recip_jerk; // reciprocal of current jerk value - with million + float jerk_max; // max jerk (Jm) in km/min^3 + float recip_jerk; // reciprocal of current jerk in min^3/mm float radius; // radius in mm for rotary axes float search_velocity; // homing search velocity float latch_velocity; // homing latch velocity @@ -58,7 +58,8 @@ axis_t axes[MOTORS] = {{0}}; bool axis_is_enabled(int axis) { int motor = axis_get_motor(axis); - return motor != -1 && motor_is_enabled(motor); + return motor != -1 && motor_is_enabled(motor) && + !fp_ZERO(axis_get_velocity_max(axis)); } diff --git a/avr/src/command.c b/avr/src/command.c index 4f2b167..5baca93 100644 --- a/avr/src/command.c +++ b/avr/src/command.c @@ -42,9 +42,11 @@ #include "plan/arc.h" #include "plan/state.h" #include "config.h" +#include "pgmspace.h" -#include +#ifdef __AVR__ #include +#endif #include #include @@ -250,7 +252,7 @@ void command_callback() { // Command functions void static print_command_help(int i) { - static const char fmt[] PROGMEM = " $%-12S %S\n"; + static const char fmt[] PROGMEM = " $%-12"PRPSTR" %"PRPSTR"\n"; const char *name = pgm_read_word(&commands[i].name); const char *help = pgm_read_word(&commands[i].help); @@ -278,7 +280,9 @@ uint8_t command_help(int argc, char *argv[]) { const char *name = pgm_read_word(&commands[i].name); if (!name) break; print_command_help(i); +#ifdef __AVR__ wdt_reset(); +#endif } puts_P(PSTR("\nVariables:")); diff --git a/avr/src/config.h b/avr/src/config.h index 2f89630..daf0a96 100644 --- a/avr/src/config.h +++ b/avr/src/config.h @@ -29,7 +29,9 @@ #include "pins.h" +#ifdef __AVR__ #include +#endif // Pins diff --git a/avr/src/gcode_parser.c b/avr/src/gcode_parser.c index 7156831..09589e2 100644 --- a/avr/src/gcode_parser.c +++ b/avr/src/gcode_parser.c @@ -40,6 +40,7 @@ #include #include #include +#include parser_t parser = {{0}}; @@ -562,6 +563,10 @@ stat_t gc_gcode_parser(char *block) { // For now this is unconditional and will always delete if (block_delete_flag) return STAT_NOOP; +#ifdef DEBUG + printf("GCODE: %s\n", block); +#endif + // queue a "(MSG" response if (*msg) mach_message(msg); // queue the message diff --git a/avr/src/gcode_state.h b/avr/src/gcode_state.h index 9f83fee..c50aaf7 100644 --- a/avr/src/gcode_state.h +++ b/avr/src/gcode_state.h @@ -29,8 +29,7 @@ #pragma once #include "config.h" - -#include +#include "pgmspace.h" #include #include @@ -116,8 +115,8 @@ typedef enum { typedef enum { - INVERSE_TIME_MODE, // G93 UNITS_PER_MINUTE_MODE, // G94 + INVERSE_TIME_MODE, // G93 UNITS_PER_REVOLUTION_MODE, // G95 (unimplemented) } feed_mode_t; diff --git a/avr/src/hardware.c b/avr/src/hardware.c index 8faaead..ec652f8 100644 --- a/avr/src/hardware.c +++ b/avr/src/hardware.c @@ -31,9 +31,9 @@ #include "usart.h" #include "huanyang.h" #include "config.h" +#include "pgmspace.h" #include -#include #include #include diff --git a/avr/src/huanyang.c b/avr/src/huanyang.c index 0d766a2..7b0d09a 100644 --- a/avr/src/huanyang.c +++ b/avr/src/huanyang.c @@ -29,10 +29,10 @@ #include "config.h" #include "rtc.h" #include "report.h" +#include "pgmspace.h" #include #include -#include #include #include diff --git a/avr/src/main.c b/avr/src/main.c index 6d6876c..4e8476a 100644 --- a/avr/src/main.c +++ b/avr/src/main.c @@ -43,12 +43,12 @@ #include "homing.h" #include "home.h" #include "i2c.h" +#include "pgmspace.h" #include "plan/planner.h" #include "plan/arc.h" #include "plan/state.h" -#include #include #include diff --git a/avr/src/motor.c b/avr/src/motor.c index 44789a1..f8ee11f 100644 --- a/avr/src/motor.c +++ b/avr/src/motor.c @@ -37,13 +37,11 @@ #include "gcode_state.h" #include "axis.h" #include "util.h" +#include "pgmspace.h" #include "plan/runtime.h" #include "plan/calibrate.h" -#include -#include - #include #include #include diff --git a/avr/src/pgmspace.h b/avr/src/pgmspace.h new file mode 100644 index 0000000..f0b1cd0 --- /dev/null +++ b/avr/src/pgmspace.h @@ -0,0 +1,50 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 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 + +#ifdef __AVR__ + +#include +#define PRPSTR "S" + +#else // __AVR__ + +#define PRPSTR "s" +#define PROGMEM +#define PGM_P char * +#define PSTR(X) X +#define vfprintf_P vfprintf +#define printf_P printf +#define puts_P puts +#define sprintf_P sprintf +#define strcmp_P strcmp +#define pgm_read_ptr(x) *(x) +#define pgm_read_word(x) *(x) +#define pgm_read_byte(x) *(x) + +#endif // __AVR__ diff --git a/avr/src/pins.h b/avr/src/pins.h index 6151e5a..3426af5 100644 --- a/avr/src/pins.h +++ b/avr/src/pins.h @@ -27,16 +27,16 @@ #pragma once -#include - - enum {PORT_A = 1, PORT_B, PORT_C, PORT_D, PORT_E, PORT_F}; -extern PORT_t *pin_ports[]; - #define PORT(PIN) pin_ports[(PIN >> 3) - 1] #define BM(PIN) (1 << (PIN & 7)) +#ifdef __AVR__ +#include + +extern PORT_t *pin_ports[]; + #define DIRSET_PIN(PIN) PORT(PIN)->DIRSET = BM(PIN) #define DIRCLR_PIN(PIN) PORT(PIN)->DIRCLR = BM(PIN) #define OUTCLR_PIN(PIN) PORT(PIN)->OUTCLR = BM(PIN) @@ -45,3 +45,5 @@ extern PORT_t *pin_ports[]; #define OUT_PIN(PIN) (!!(PORT(PIN)->OUT & BM(PIN))) #define IN_PIN(PIN) (!!(PORT(PIN)->IN & BM(PIN))) #define PINCTRL_PIN(PIN) ((&PORT(PIN)->PIN0CTRL)[PIN & 7]) + +#endif // __AVR__ diff --git a/avr/src/plan/buffer.c b/avr/src/plan/buffer.c index 83f25b4..f264cff 100644 --- a/avr/src/plan/buffer.c +++ b/avr/src/plan/buffer.c @@ -133,6 +133,7 @@ mp_buffer_t *mp_queue_get_tail() { * invalidating its contents */ void mp_queue_push(buffer_cb_t cb, uint32_t line) { + mp_buffer_validate(mb.tail); mp_state_running(); mb.tail->ts = rtc_get_time(); @@ -154,3 +155,87 @@ void mp_queue_pop() { _clear_buffer(mb.head); _pop(); } + + +#ifdef DEBUG +void mp_queue_dump() { + mp_buffer_t *bf = mp_queue_get_head(); + if (!bf) return; + mp_buffer_t *bp = bf; + + do { + if (bp != bf) putchar(','); + mp_buffer_print(bp); + bp = mp_buffer_next(bp); + } while (bp != bf && bp->state != BUFFER_OFF); + + if (bp != bf) mp_buffer_print(bp); + + putchar('\n'); +} + + +void mp_buffer_print(const mp_buffer_t *bf) { + printf("{\n" + " ts: %d,\n" + " line: %d,\n" + " state: %d,\n" + " replannable: %s,\n" + " hold: %s,\n" + " value: %0.2f,\n" + " target: [%0.2f, %0.2f, %0.2f, %0.2f],\n" + " unit: [%0.2f, %0.2f, %0.2f, %0.2f],\n" + " length: %0.2f,\n" + " head_length: %0.2f,\n" + " body_length: %0.2f,\n" + " tail_length: %0.2f,\n" + " entry_velocity: %0.2f,\n" + " cruise_velocity: %0.2f,\n" + " exit_velocity: %0.2f,\n" + " braking_velocity: %0.2f,\n" + " entry_vmax: %0.2f,\n" + " cruise_vmax: %0.2f,\n" + " exit_vmax: %0.2f,\n" + " delta_vmax: %0.2f,\n" + " jerk: %0.2f,\n" + " cbrt_jerk: %0.2f\n" + "}", bf->ts, bf->line, bf->state, bf->replannable ? "true" : "false", + bf->hold ? "true" : "false", bf->value, bf->target[0], bf->target[1], + bf->target[2], bf->target[3], bf->unit[0], bf->unit[1], bf->unit[2], + bf->unit[3], bf->length, bf->head_length, bf->body_length, + bf->tail_length, bf->entry_velocity, bf->cruise_velocity, + bf->exit_velocity, bf->braking_velocity, bf->entry_vmax, + bf->cruise_vmax, bf->exit_vmax, bf->delta_vmax, bf->jerk, + bf->cbrt_jerk); +} +#endif // DEBUG + + +void mp_buffer_validate(const mp_buffer_t *bp) { + ASSERT(bp); + + ASSERT(isfinite(bp->value)); + + ASSERT(isfinite(bp->target[0]) && isfinite(bp->target[1]) && + isfinite(bp->target[2]) && isfinite(bp->target[3])); + ASSERT(isfinite(bp->unit[0]) && isfinite(bp->unit[1]) && + isfinite(bp->unit[2]) && isfinite(bp->unit[3])); + + ASSERT(isfinite(bp->length)); + ASSERT(isfinite(bp->head_length)); + ASSERT(isfinite(bp->body_length)); + ASSERT(isfinite(bp->tail_length)); + + ASSERT(isfinite(bp->entry_velocity)); + ASSERT(isfinite(bp->cruise_velocity)); + ASSERT(isfinite(bp->exit_velocity)); + ASSERT(isfinite(bp->braking_velocity)); + + ASSERT(isfinite(bp->entry_vmax)); + ASSERT(isfinite(bp->cruise_vmax)); + ASSERT(isfinite(bp->exit_vmax)); + ASSERT(isfinite(bp->delta_vmax)); + + ASSERT(isfinite(bp->jerk)); + ASSERT(isfinite(bp->cbrt_jerk)); +} diff --git a/avr/src/plan/buffer.h b/avr/src/plan/buffer.h index 9317629..c962132 100644 --- a/avr/src/plan/buffer.h +++ b/avr/src/plan/buffer.h @@ -61,7 +61,7 @@ typedef struct mp_buffer_t { // See Planning Velocity Notes float value; // used in dwell and other callbacks - float target[AXES]; // XYZABC where the move should go + float target[AXES]; // XYZABC where the move should go in mm float unit[AXES]; // unit vector for axis scaling & planning float length; // total length of line or helix in mm @@ -98,5 +98,9 @@ void mp_queue_push(buffer_cb_t func, uint32_t line); mp_buffer_t *mp_queue_get_head(); void mp_queue_pop(); +void mp_queue_dump(); + +void mp_buffer_print(const mp_buffer_t *bp); +void mp_buffer_validate(const mp_buffer_t *bp); static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->prev;} static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->next;} diff --git a/avr/src/plan/calibrate.c b/avr/src/plan/calibrate.c index 5e17dea..67b368e 100644 --- a/avr/src/plan/calibrate.c +++ b/avr/src/plan/calibrate.c @@ -41,6 +41,7 @@ #include #include #include +#include #define CAL_VELOCITIES 256 diff --git a/avr/src/plan/dwell.h b/avr/src/plan/dwell.h index 9805620..0345084 100644 --- a/avr/src/plan/dwell.h +++ b/avr/src/plan/dwell.h @@ -29,4 +29,7 @@ #include "status.h" +#include + + stat_t mp_dwell(float seconds, int32_t line); diff --git a/avr/src/plan/exec.c b/avr/src/plan/exec.c index fc59e55..945764a 100644 --- a/avr/src/plan/exec.c +++ b/avr/src/plan/exec.c @@ -45,6 +45,14 @@ #include +typedef struct { + float a, b, c, d; + float delta; + float half_delta; + uint16_t count; +} quintic_bezier_t; + + typedef struct { float unit[AXES]; // unit vector for axis scaling & planning float final_target[AXES]; // final target, used to correct rounding errors @@ -62,6 +70,7 @@ typedef struct { float segment_velocity; // computed velocity for segment float segment_time; // actual time increment per segment forward_dif_t fdif; // forward difference levels + quintic_bezier_t qb; bool hold_planned; // true when a feedhold has been planned move_section_t section; // current move section bool section_new; // true if it's a new section @@ -72,6 +81,28 @@ typedef struct { static mp_exec_t ex = {{0}}; +static float _quintic_bezier_next() { + float t = ex.qb.half_delta + ex.qb.count++ * ex.qb.delta; + float t2 = t * t; + float t3 = t2 * t; + + return (ex.qb.a * t2 + ex.qb.b * t + ex.qb.c) * t3 + ex.qb.d; +} + + +static float _quintic_bezier_init(float Vi, float Vt, float segs) { + ex.qb.a = 6 * (Vt - Vi); + ex.qb.b = -15 * (Vt - Vi); + ex.qb.c = 10 * (Vt - Vi); + ex.qb.d = Vi; + ex.qb.delta = 1 / segs; + ex.qb.half_delta = ex.qb.delta / 2; + ex.qb.count = 0; + + return _quintic_bezier_next(); +} + + static stat_t _exec_aline_segment() { float target[AXES]; @@ -101,8 +132,14 @@ static stat_t _exec_aline_segment() { /// Common code for head and tail sections static stat_t _exec_aline_section(float length, float vin, float vout) { if (ex.section_new) { + ASSERT(isfinite(length)); + if (fp_ZERO(length)) return STAT_NOOP; // end the section + ASSERT(isfinite(vin) && isfinite(vout)); + ASSERT(0 <= vin && 0 <= vout); + ASSERT(vin || vout); + // len / avg. velocity float move_time = 2 * length / (vin + vout); float segments = ceil(move_time / NOM_SEGMENT_TIME); @@ -110,8 +147,14 @@ static stat_t _exec_aline_section(float length, float vin, float vout) { ex.segment_count = round(segments); if (vin == vout) ex.segment_velocity = vin; - else ex.segment_velocity = - mp_init_forward_dif(ex.fdif, vin, vout, segments); + else { +#if 0 + ex.segment_velocity = + mp_init_forward_dif(ex.fdif, vin, vout, segments); +#else + ex.segment_velocity = _quintic_bezier_init(vin, vout, segments); +#endif + } if (ex.segment_time < MIN_SEGMENT_TIME) return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position @@ -125,7 +168,13 @@ static stat_t _exec_aline_section(float length, float vin, float vout) { ex.section_new = false; } else { - if (vin != vout) ex.segment_velocity += mp_next_forward_dif(ex.fdif); + if (vin != vout) { +#if 0 + ex.segment_velocity += mp_next_forward_dif(ex.fdif); +#else + ex.segment_velocity = _quintic_bezier_next(); +#endif + } // Subsequent segments if (_exec_aline_segment() == STAT_OK) return STAT_OK; diff --git a/avr/src/plan/forward_dif.c b/avr/src/plan/forward_dif.c index 258725e..80f3339 100644 --- a/avr/src/plan/forward_dif.c +++ b/avr/src/plan/forward_dif.c @@ -29,15 +29,23 @@ #include "forward_dif.h" +#include "util.h" + #include /// Forward differencing math /// /// We are using a quintic (fifth-degree) Bezier polynomial for the velocity -/// curve. This gives us a "linear pop" velocity curve; with pop being the -/// sixth derivative of position: velocity - 1st, acceleration - 2nd, jerk - -/// 3rd, snap - 4th, crackle - 5th, pop - 6th +/// curve. This gives us a constant "pop"; with pop being the sixth derivative +/// of position: +/// +/// 1st - velocity +/// 2nd - acceleration +/// 3rd - jerk +/// 4th - snap +/// 5th - crackle +/// 6th - pop /// /// The Bezier curve takes the form: /// @@ -74,10 +82,12 @@ /// E = - 5 * P_0 + 5 * P_1 /// F = P_0 /// -/// Now, since we will (currently) *always* want the initial acceleration and -/// jerk values to be 0, We set P_i = P_0 = P_1 = P_2 (initial velocity), and -/// P_t = P_3 = P_4 = P_5 (target velocity), which, after simplification, -/// resolves to: +/// Since we want the initial acceleration and jerk values to be 0, We set +/// +/// P_i = P_0 = P_1 = P_2 (initial velocity) +/// P_t = P_3 = P_4 = P_5 (target velocity) +/// +/// which, after simplification, resolves to: /// /// A = - 6 * P_i + 6 * P_t /// B = 15 * P_i - 15 * P_t @@ -86,10 +96,14 @@ /// E = 0 /// F = P_i /// -/// Given an interval count of I to get from P_i to P_t, we get the parametric -/// "step" size of h = 1/I. We need to calculate the initial value of forward -/// differences (F_0 - F_5) such that the inital velocity V = P_i, then we -/// iterate over the following I times: +/// Given an interval count of segments to get from P_i to P_t, we get the +/// parametric "step" size: +/// +/// h = 1 / segs +/// +/// We need to calculate the initial value of forward differences (F_0 - F_5) +/// such that the inital velocity V = P_i, then we iterate over the following +/// segs times: /// /// V += F_5 /// F_5 += F_4 @@ -137,7 +151,7 @@ /// Normally, we could then assign t = 0, use the A-F values from above, and get /// out initial F_* values. However, for the sake of "averaging" the velocity /// of each segment, we actually want to have the initial V be be at t = h/2 and -/// iterate I-1 times. So, the resulting F_* values are (steps not shown): +/// iterate segs-1 times. So, the resulting F_* values are (steps not shown): /// /// F_5 = 121Ah^5 / 16 + 5Bh^4 + 13Ch^3 / 4 + 2Dh^2 + Eh /// F_4 = 165Ah^5 / 2 + 29Bh^4 + 9Ch^3 + 2Dh^2 @@ -156,22 +170,22 @@ /// F_2 = ( - 360 * s + 1800.0 )(Vt - Vi) * h^5 /// F_1 = ( 720.0 )(Vt - Vi) * h^5 /// -float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float s) { - const float h = 1 / s; - const float s2 = square(s); +float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float segs) { + const float h = 1 / segs; + const float segs2 = square(segs); const float Vdxh5 = (Vt - Vi) * h * h * h * h * h; - fdif[4] = (32.5 * s2 - 75.0 * s + 45.375) * Vdxh5; - fdif[3] = (90.0 * s2 - 435.0 * s + 495.0 ) * Vdxh5; - fdif[2] = (60.0 * s2 - 720.0 * s + 1530.0 ) * Vdxh5; - fdif[1] = ( - 360.0 * s + 1800.0 ) * Vdxh5; - fdif[0] = ( 720.0 ) * Vdxh5; + fdif[4] = (32.5 * segs2 - 75.0 * segs + 45.375) * Vdxh5; + fdif[3] = (90.0 * segs2 - 435.0 * segs + 495.0 ) * Vdxh5; + fdif[2] = (60.0 * segs2 - 720.0 * segs + 1530.0 ) * Vdxh5; + fdif[1] = ( - 360.0 * segs + 1800.0 ) * Vdxh5; + fdif[0] = ( 720.0 ) * Vdxh5; // Calculate the initial velocity by calculating: // // V(h / 2) = // - // ( -6Vi + 6Vt) / (2^5 * s^8) + + // ( -6Vi + 6Vt) / (2^5 * s^8) + // ( 15Vi - 15Vt) / (2^4 * s^8) + // (-10Vi + 10Vt) / (2^3 * s^8) + Vi = // diff --git a/avr/src/plan/line.c b/avr/src/plan/line.c index 19666f7..a58f876 100644 --- a/avr/src/plan/line.c +++ b/avr/src/plan/line.c @@ -34,6 +34,8 @@ #include "exec.h" #include "buffer.h" +#include + /* Sonny's algorithm - simple * @@ -238,6 +240,8 @@ static void _calc_and_cache_jerk_values(mp_buffer_t *bf) { static void _calc_max_velocities(mp_buffer_t *bf, float move_time, bool exact_stop) { + ASSERT(0 < move_time && isfinite(move_time)); + float junction_velocity = _get_junction_vmax(mp_buffer_prev(bf)->unit, bf->unit); @@ -310,6 +314,7 @@ static float _calc_move_time(const float axis_length[], const float axis_square[], bool rapid, bool inverse_time, float feed_rate, float feed_override) { + ASSERT(0 < feed_override); float max_time = 0; // Compute times for feed motion @@ -363,6 +368,11 @@ static float _calc_move_time(const float axis_length[], stat_t mp_aline(const float target[], bool rapid, bool inverse_time, bool exact_stop, float feed_rate, float feed_override, int32_t line) { + DEBUG_CALL("(%f, %f, %f, %f), %s, %s, %s, %f, %f, %d", target[0], target[1], + target[2], target[3], rapid ? "true" : "false", + inverse_time ? "true" : "false", exact_stop ? "true" : "false", + feed_rate, feed_override, line); + // Compute axis and move lengths float axis_length[AXES]; float axis_square[AXES]; diff --git a/avr/src/plan/line.h b/avr/src/plan/line.h index 3acb11b..84d1be6 100644 --- a/avr/src/plan/line.h +++ b/avr/src/plan/line.h @@ -30,6 +30,7 @@ #include "status.h" #include +#include stat_t mp_aline(const float target[], bool rapid, bool inverse_time, diff --git a/avr/src/plan/planner.c b/avr/src/plan/planner.c index d3ef6dd..6a256c5 100644 --- a/avr/src/plan/planner.c +++ b/avr/src/plan/planner.c @@ -231,8 +231,6 @@ void mp_kinematics(const float travel[], float steps[]) { * tons of pathologically short Gcode blocks are being thrown at you. */ void mp_calculate_trapezoid(mp_buffer_t *bf) { - // RULE #1 of mp_calculate_trapezoid(): Don't change bf->length - if (!bf->length) return; // F case: Block is too short - run time < minimum segment time @@ -259,7 +257,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { if (naive_move_time <= NOM_SEGMENT_TIME) { bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity; - if (fp_NOT_ZERO(bf->entry_velocity)) { + if (!fp_ZERO(bf->entry_velocity)) { bf->cruise_velocity = bf->entry_velocity; bf->exit_velocity = bf->entry_velocity; @@ -334,8 +332,8 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk); bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk); - if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;} - if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;} + if (bf->head_length < MIN_HEAD_LENGTH) bf->head_length = 0; + if (bf->tail_length < MIN_TAIL_LENGTH) bf->tail_length = 0; // Rate-limited HT and HT' cases if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited @@ -343,7 +341,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { // Symmetric rate-limited case (HT) if (fabs(bf->entry_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) { - bf->head_length = bf->length/2; + bf->head_length = bf->length / 2; bf->tail_length = bf->head_length; bf->cruise_velocity = min(bf->cruise_vmax, @@ -356,7 +354,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { bf->tail_length = 0; // Average the entry speed and computed best cruise-speed - bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity)/2; + bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity) / 2; bf->entry_velocity = bf->cruise_velocity; bf->exit_velocity = bf->cruise_velocity; } @@ -375,7 +373,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk); - if (bf->head_length > bf->tail_length) { + if (bf->tail_length < bf->head_length) { bf->head_length = (bf->head_length / (bf->head_length + bf->tail_length)) * bf->length; computed_velocity = @@ -415,23 +413,23 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { bf->body_length = bf->length - bf->head_length - bf->tail_length; // If a non-zero body is < minimum length distribute it to the head and/or - // tail. This will generate small (acceptable) velocity errors in runtime + // tail. This will generate small (acceptable) velocity errors in runtime // execution but preserve correct distance, which is more important. - if ((bf->body_length < MIN_BODY_LENGTH) && (fp_NOT_ZERO(bf->body_length))) { - if (fp_NOT_ZERO(bf->head_length)) { - if (fp_NOT_ZERO(bf->tail_length)) { // HBT reduces to HT + if (bf->body_length < MIN_BODY_LENGTH && !fp_ZERO(bf->body_length)) { + if (!fp_ZERO(bf->head_length)) { + if (!fp_ZERO(bf->tail_length)) { // HBT reduces to HT bf->head_length += bf->body_length / 2; bf->tail_length += bf->body_length / 2; - } else bf->head_length += bf->body_length; // HB reduces to H - } else bf->tail_length += bf->body_length; // BT reduces to T + } else bf->head_length += bf->body_length; // HB reduces to H + } else bf->tail_length += bf->body_length; // BT reduces to T bf->body_length = 0; // If the body is a standalone make the cruise velocity match the entry - // velocity. This removes a potential velocity discontinuity at the expense + // velocity. This removes a potential velocity discontinuity at the expense // of top speed - } else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length))) + } else if (fp_ZERO(bf->head_length) && fp_ZERO(bf->tail_length)) bf->cruise_velocity = bf->entry_velocity; } @@ -448,11 +446,11 @@ void mp_print_queue(mp_buffer_t *bf) { int i = 0; mp_buffer_t *bp = bf; while (bp) { - printf_P(PSTR("{\"msg\":\"%d,%d,0x%04x," + printf_P(PSTR("{\"msg\":\"%d,%d,0x%04lx," "%0.2f,%0.2f,%0.2f,%0.2f," "%0.2f,%0.2f,%0.2f,%0.2f," "%0.2f,%0.2f,%0.2f\"}\n"), - i++, bp->replannable, bp->cb, + i++, bp->replannable, (intptr_t)bp->cb, bp->length, bp->head_length, bp->body_length, bp->tail_length, bp->entry_velocity, bp->cruise_velocity, bp->exit_velocity, bp->braking_velocity, @@ -664,6 +662,8 @@ void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line) { * PLANNER_VELOCITY_TOLERANCE necessitating the introduction of fabs() */ float mp_get_target_length(float Vi, float Vf, float jerk) { + ASSERT(0 <= Vi && 0 <= Vf); + ASSERT(isfinite(jerk)); return fp_EQ(Vi, Vf) ? 0 : fabs(Vi - Vf) * invsqrt(jerk / fabs(Vi - Vf)); } @@ -735,10 +735,15 @@ float mp_get_target_length(float Vi, float Vf, float jerk) { * J'(x) = (2 * Vi * x - Vi^2 + 3 * x^2) / L^2 */ float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf) { + ASSERT(0 <= Vi && 0 <= L); + ASSERT(isfinite(bf->cbrt_jerk) && isfinite(bf->jerk)); + + if (!L) return Vi; + // 0 iterations (a reasonable estimate) float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate -#if (GET_VELOCITY_ITERATIONS > 0) +#if GET_VELOCITY_ITERATIONS const float L2 = L * L; const float Vi2 = Vi * Vi; diff --git a/avr/src/plan/runtime.c b/avr/src/plan/runtime.c index 0b9374d..f4183b5 100644 --- a/avr/src/plan/runtime.c +++ b/avr/src/plan/runtime.c @@ -154,6 +154,8 @@ void mp_runtime_set_work_offsets(float offset[]) { /// Segment runner stat_t mp_runtime_move_to_target(float target[], float time) { + ASSERT(isfinite(time)); + // Convert target position to steps. float steps[MOTORS]; mp_kinematics(target, steps); diff --git a/avr/src/plan/state.h b/avr/src/plan/state.h index 9b0ea46..df9a9d9 100644 --- a/avr/src/plan/state.h +++ b/avr/src/plan/state.h @@ -29,7 +29,7 @@ #pragma once -#include +#include "pgmspace.h" #include diff --git a/avr/src/probing.c b/avr/src/probing.c index 1c17891..21ff1cb 100644 --- a/avr/src/probing.c +++ b/avr/src/probing.c @@ -31,13 +31,12 @@ #include "spindle.h" #include "switch.h" #include "util.h" +#include "pgmspace.h" #include "plan/planner.h" #include "plan/runtime.h" #include "plan/state.h" -#include - #include #include #include diff --git a/avr/src/report.c b/avr/src/report.c index f49a593..b403965 100644 --- a/avr/src/report.c +++ b/avr/src/report.c @@ -30,8 +30,7 @@ #include "usart.h" #include "rtc.h" #include "vars.h" - -#include +#include "pgmspace.h" #include #include diff --git a/avr/src/status.c b/avr/src/status.c index 1062f25..8d1a0a8 100644 --- a/avr/src/status.c +++ b/avr/src/status.c @@ -35,10 +35,12 @@ stat_t status_code; // allocate a variable for the RITORNO macro + #define STAT_MSG(NAME, TEXT) static const char stat_##NAME[] PROGMEM = TEXT; #include "messages.def" #undef STAT_MSG + static const char *const stat_msg[] PROGMEM = { #define STAT_MSG(NAME, TEXT) stat_##NAME, #include "messages.def" @@ -71,7 +73,8 @@ stat_t status_message_P(const char *location, status_level_t level, va_list args; // Type - printf_P(PSTR("\n{\"level\":\"%S\",\"msg\":\""), status_level_pgmstr(level)); + printf_P(PSTR("\n{\"level\":\"%"PRPSTR"\", \"msg\":\""), + status_level_pgmstr(level)); // Message if (msg) { @@ -87,7 +90,7 @@ stat_t status_message_P(const char *location, status_level_t level, if (code) printf_P(PSTR(", \"code\": %d"), code); // Location - if (location) printf_P(PSTR(", \"where\": \"%S\""), location); + if (location) printf_P(PSTR(", \"where\": \"%"PRPSTR"\""), location); putchar('}'); putchar('\n'); @@ -102,7 +105,7 @@ void status_help() { for (int i = 0; i < STAT_MAX; i++) { if (i) putchar(','); putchar('\n'); - printf_P(PSTR(" \"%d\": \"%S\""), i, status_to_pgmstr(i)); + printf_P(PSTR(" \"%d\": \"%"PRPSTR"\""), i, status_to_pgmstr(i)); } putchar('\n'); @@ -112,8 +115,8 @@ void status_help() { /// 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); +stat_t status_alarm(const char *location, stat_t code, const char *msg) { + status_message_P(location, STAT_LEVEL_ERROR, code, msg); estop_trigger(code); while (!usart_tx_empty()) continue; return code; diff --git a/avr/src/status.h b/avr/src/status.h index e04d833..ef27054 100644 --- a/avr/src/status.h +++ b/avr/src/status.h @@ -27,7 +27,7 @@ #pragma once -#include +#include "pgmspace.h" // RITORNO is a handy way to provide exception returns @@ -61,7 +61,7 @@ stat_t status_message_P(const char *location, status_level_t level, void status_help(); /// Enter alarm state. returns same status code -stat_t status_alarm(const char *location, stat_t status); +stat_t status_alarm(const char *location, stat_t status, const char *msg); #define TO_STRING(x) _TO_STRING(x) #define _TO_STRING(x) #x @@ -83,5 +83,17 @@ stat_t status_alarm(const char *location, stat_t status); #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) +#define ALARM(CODE) status_alarm(STATUS_LOCATION, CODE, 0) +#define ASSERT(COND) \ + do { \ + if (!(COND)) \ + status_alarm(STATUS_LOCATION, STAT_INTERNAL_ERROR, PSTR(#COND)); \ + } while (0) + + +#ifdef DEBUG +#define DEBUG_CALL(FMT, ...) \ + printf("%s(" FMT ")\n", __FUNCTION__, ##__VA_ARGS__) +#else // DEBUG +#define DEBUG_CALL(...) +#endif // DEBUG diff --git a/avr/src/util.c b/avr/src/util.c index 518c8d2..021c1a1 100644 --- a/avr/src/util.c +++ b/avr/src/util.c @@ -28,7 +28,7 @@ #include "util.h" - +#include /// Fast inverse square root originally from Quake III Arena code. Original @@ -37,13 +37,13 @@ float invsqrt(float number) { const float threehalfs = 1.5F; - float x2 = number * 0.5; + float x2 = number * 0.5F; float y = number; - long i = *(long *)&y; // evil floating point bit level hacking + int32_t i = *(int32_t *)&y; // evil floating point bit level hacking i = 0x5f3759df - (i >> 1); // what the fuck? y = *(float *)&i; y = y * (threehalfs - x2 * y * y); // 1st iteration y = y * (threehalfs - x2 * y * y); // 2nd iteration, this can be removed - return y; //isnan(y) ? 1.0 / sqrt(number) : y; + return y; } diff --git a/avr/src/util.h b/avr/src/util.h index 8511876..c6ae8fb 100644 --- a/avr/src/util.h +++ b/avr/src/util.h @@ -42,25 +42,28 @@ #define copy_vector(d, s) memcpy(d, s, sizeof(d)) // Math utilities -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) +inline static float min(float a, float b) {return a < b ? a : b;} +inline static float max(float a, float b) {return a < b ? b : a;} +inline static float min3(float a, float b, float c) {return min(min(a, b), c);} +inline static float max3(float a, float b, float c) {return max(max(a, b), c);} +inline static 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) +inline static float max4(float a, float b, float c, float d) {return max(max(a, b), max(c, d));} float invsqrt(float number); +#ifndef __AVR__ +inline static double square(double x) {return x * x;} +#endif + // Floating-point utilities #define EPSILON 0.00001 // allowable rounding error for floats -inline bool fp_EQ(float a, float b) {return fabs(a - b) < EPSILON;} -inline bool fp_NE(float a, float b) {return fabs(a - b) > EPSILON;} -inline bool fp_ZERO(float a) {return fabs(a) < EPSILON;} -inline bool fp_NOT_ZERO(float a) {return !fp_ZERO(a);} -inline bool fp_FALSE(float a) {return fp_ZERO(a);} -inline bool fp_TRUE(float a) {return !fp_ZERO(a);} +inline static bool fp_EQ(float a, float b) {return fabs(a - b) < EPSILON;} +inline static bool fp_NE(float a, float b) {return fabs(a - b) > EPSILON;} +inline static bool fp_ZERO(float a) {return fabs(a) < EPSILON;} +inline static bool fp_FALSE(float a) {return fp_ZERO(a);} +inline static bool fp_TRUE(float a) {return !fp_ZERO(a);} // Constants #define MM_PER_INCH 25.4 diff --git a/avr/src/vars.c b/avr/src/vars.c index e22b00f..6b097a3 100644 --- a/avr/src/vars.c +++ b/avr/src/vars.c @@ -31,6 +31,7 @@ #include "status.h" #include "hardware.h" #include "config.h" +#include "pgmspace.h" #include #include @@ -39,8 +40,7 @@ #include #include #include - -#include +#include typedef uint8_t flags_t; @@ -67,7 +67,7 @@ static void var_print_string(string s) { // Program string static void var_print_pstring(pstring s) { - printf_P(PSTR("\"%S\""), s); + printf_P(PSTR("\"%"PRPSTR"\""), s); } @@ -321,7 +321,8 @@ bool vars_set(const char *name, const char *value) { int vars_parser(char *vars) { - if (!*vars || !*vars++ == '{') return STAT_OK; + if (!*vars || *vars != '{') return STAT_OK; + vars++; while (*vars) { while (isspace(*vars)) vars++; @@ -355,7 +356,8 @@ int vars_parser(char *vars) { void vars_print_help() { - static const char fmt[] PROGMEM = " $%-5s %-20S %-16S %S\n"; + static const char fmt[] PROGMEM = + " $%-5s %-20"PRPSTR" %-16"PRPSTR" %"PRPSTR"\n"; // Save and disable watchdog uint8_t wd_state = hw_disable_watchdog(); diff --git a/avr/test/Makefile b/avr/test/Makefile new file mode 100644 index 0000000..cc89e18 --- /dev/null +++ b/avr/test/Makefile @@ -0,0 +1,28 @@ +TESTS=planner-test + +PLANNER_TEST_SRC = gcode_parser.c machine.c status.c util.c axis.c report.c \ + homing.c probing.c command.c vars.c +PLANNER_TEST_SRC := $(patsubst %,../src/%,$(PLANNER_TEST_SRC)) +PLANNER_TEST_SRC += $(wildcard ../src/plan/*.c) planner-test.c hal.c + +CFLAGS = -I../src -Wall -Werror -DDEBUG -g -std=gnu99 +CFLAGS += -MD -MP -MT $@ -MF .dep/$(@F).d +CFLAGS += -DF_CPU=320000000 +LDFLAGS = -lm + +all: $(TESTS) + +planner-test: $(PLANNER_TEST_SRC) + gcc -o $@ $(PLANNER_TEST_SRC) $(CFLAGS) $(LDFLAGS) + +# Clean +tidy: + rm -f $(shell find -name \*~ -o -name \#\*) + +clean: tidy + rm -rf $(TESTS) + +.PHONY: tidy clean all + +# Dependencies +-include $(shell mkdir -p .dep) $(wildcard .dep/*) diff --git a/avr/test/hal.c b/avr/test/hal.c new file mode 100644 index 0000000..ce0b0c3 --- /dev/null +++ b/avr/test/hal.c @@ -0,0 +1,228 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 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" + +\******************************************************************************/ + +#include "status.h" +#include "spindle.h" +#include "i2c.h" +#include "cpp_magic.h" +#include "plan/buffer.h" + +#include +#include +#include +#include + + +typedef uint8_t flags_t; +typedef const char *string; +typedef const char *pstring; + + +#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ + TYPE get_##NAME(IF(INDEX)(int index)) __attribute__((weak)); \ + TYPE get_##NAME(IF(INDEX)(int index)) { \ + DEBUG_CALL(); \ + return 0; \ + } \ + IF(SET) \ + (void set_##NAME(IF(INDEX)(int index,) TYPE value) __attribute__((weak)); \ + void set_##NAME(IF(INDEX)(int index,) TYPE value) { \ + DEBUG_CALL(); \ + }) + +#include "vars.def" +#undef VAR + + +void command_mreset(int argc, char *argv[]) {} +void command_home(int argc, char *argv[]) {} +void i2c_set_read_callback(i2c_read_cb_t cb) {} +void print_status_flags(uint8_t flags) {DEBUG_CALL();} +uint8_t hw_disable_watchdog() {return 0;} +void hw_restore_watchdog(uint8_t state) {} + + +bool estop = false; + + +void estop_trigger(stat_t reason) { + DEBUG_CALL("%s", status_to_pgmstr(reason)); + mp_queue_dump(); + estop = true; + abort(); +} + + +void estop_clear() { + DEBUG_CALL(); + estop = false; +} + + +bool estop_triggered() {return estop;} + + +void hw_request_hard_reset() { + DEBUG_CALL(); + exit(0); +} + + +bool usart_tx_empty() {return true;} +bool usart_tx_full() {return false;} + + +char *usart_readline() { + static char *cmd = 0; + + if (cmd) { + free(cmd); + cmd = 0; + } + + size_t n = 0; + if (getline(&cmd, &n, stdin) == -1) { + free(cmd); + cmd = 0; + } + + return cmd; +} + + +void coolant_init() {} + + +void coolant_set_mist(bool x) { + DEBUG_CALL("%s", x ? "true" : "false"); +} + + +void coolant_set_flood(bool x) { + DEBUG_CALL("%s", x ? "true" : "false"); +} + + +void spindle_init() {} + + +void spindle_set_speed(float speed) { + DEBUG_CALL("%f", speed); +} + + +void spindle_set_mode(spindle_mode_t mode) { + DEBUG_CALL("%d", mode); +} + + +void motor_set_encoder(int motor, float encoder) { + DEBUG_CALL("%d, %f", motor, encoder); +} + + +bool switch_is_active(int index) { + DEBUG_CALL("%d", index); + return false; +} + + +bool switch_is_enabled(int index) { + DEBUG_CALL("%d", index); + return false; +} + + +static uint32_t ticks = 0; + + +uint32_t rtc_get_time() {return ticks;} + + +bool rtc_expired(uint32_t t) { + return true; + return 0 <= (int32_t)(ticks - t); +} + + +bool motor_is_enabled(int motor) {return true;} +int motor_get_axis(int motor) {return motor;} + + +#define MICROSTEPS 32 +#define TRAVEL_REV 5 +#define STEP_ANGLE 1.8 + + +float motor_get_steps_per_unit(int motor) { + return 360 * MICROSTEPS / TRAVEL_REV / STEP_ANGLE; +} + + +int32_t motor_get_encoder(int motor) { + DEBUG_CALL("%d", motor); + return 0; +} + + +void motor_end_move(int motor) { + DEBUG_CALL("%d", motor); +} + + +bool st_is_busy() {return false;} + + +float square(float x) {return x * x;} + + +stat_t st_prep_line(float time, const float target[], const int32_t error[]) { + DEBUG_CALL("%f, (%f, %f, %f, %f), (%d, %d, %d, %d)", + time, target[0], target[1], target[2], target[3], + error[0], error[1], error[2], error[3]); + + static float position[4] = {0}; + float dist = 0; + + for (int i = 0; i < 4; i++) { + dist += square((target[i] - position[i]) / motor_get_steps_per_unit(i)); + position[i] = target[i]; + } + + dist = sqrt(dist); + + float velocity = dist / time; + + printf("%0.10f, %0.10f, %0.10f\n", velocity, dist, time); + + return STAT_OK; +} + + +void st_prep_dwell(float seconds) { + DEBUG_CALL("%f", seconds); +} diff --git a/avr/test/planner-test.c b/avr/test/planner-test.c new file mode 100644 index 0000000..7af39ac --- /dev/null +++ b/avr/test/planner-test.c @@ -0,0 +1,75 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 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" + +\******************************************************************************/ + +#include "axis.h" +#include "machine.h" +#include "command.h" +#include "plan/planner.h" +#include "plan/exec.h" +#include "plan/state.h" + +#include +#include + + +int main(int argc, char *argv[]) { + mp_init(); // motion planning + machine_init(); // gcode machine + for (int i = 0; i < 4; i++) axis_set_motor(i, i); + + stat_t status = STAT_OK; + + while (!feof(stdin)) { + mp_state_callback(); + command_callback(); + } + + while (true) { + status = mp_exec_move(); + printf("EXEC: %s\n", status_to_pgmstr(status)); + + switch (status) { + case STAT_NOOP: break; // No command executed + case STAT_EAGAIN: continue; // No command executed, try again + + case STAT_OK: // Move executed + //if (!st.move_queued) ALARM(STAT_EXPECTED_MOVE); // No move was queued + //st.move_queued = false; + //st.move_ready = true; + continue; + + default: + printf("ERROR: %s\n", status_to_pgmstr(status)); + } + + break; + } + + printf("STATE: %s\n", mp_get_state_pgmstr(mp_get_state())); + + return status; +}