From 3a2bd88412923b32c8e5ab7160495e9b25a210ff Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sat, 6 Feb 2016 15:49:53 -0800 Subject: [PATCH] New config options --- Makefile | 3 +- cpp_magic.h | 442 ++++ src/canonical_machine.c | 840 ++------ src/canonical_machine.h | 185 +- src/clock.c | 161 ++ src/{xmega/xmega_init.h => clock.h} | 15 +- src/command.c | 194 ++ src/command.def | 33 + src/command.h | 55 + src/config.c | 656 ------ src/config.h | 637 +++--- src/config_app.c | 880 -------- src/config_app.h | 83 - src/controller.c | 168 +- src/controller.h | 58 +- src/cpp_magic.h | 471 +++++ src/cycle_homing.c | 587 +++--- src/cycle_jogging.c | 204 +- src/cycle_probing.c | 280 ++- src/encoder.c | 21 +- src/encoder.h | 17 +- src/gcode_parser.c | 52 +- src/gcode_parser.h | 12 +- src/gpio.c | 11 +- src/gpio.h | 8 +- src/hardware.c | 63 +- src/hardware.h | 48 +- src/help.c | 153 -- src/help.h | 42 - src/json_parser.c | 597 ------ src/json_parser.h | 109 - src/main.c | 30 +- src/messages.def | 276 +++ src/persistence.c | 70 - src/persistence.h | 52 - src/plan/arc.c | 460 ++++ src/plan/arc.h | 71 + src/plan/exec.c | 744 +++++++ src/{ => plan}/kinematics.c | 6 +- src/{ => plan}/kinematics.h | 6 +- src/plan/line.c | 767 +++++++ src/plan/planner.c | 412 ++++ src/plan/planner.h | 305 +++ src/{plan_zoid.c => plan/zoid.c} | 7 +- src/plan_arc.c | 464 ----- src/plan_arc.h | 77 - src/plan_exec.c | 731 ------- src/plan_line.c | 754 ------- src/plan_line.h | 34 - src/planner.c | 441 ---- src/planner.h | 322 --- src/pwm.c | 57 +- src/pwm.h | 45 +- src/report.c | 619 +----- src/report.h | 167 +- src/rtc.c | 81 + src/{xmega/xmega_rtc.h => rtc.h} | 16 +- src/settings.h | 137 -- src/settings/settings_Ultimaker.h | 247 --- src/settings/settings_cnc3040.h | 240 --- src/settings/settings_default.h | 238 --- src/settings/settings_openpnp.h | 224 -- src/settings/settings_othermill.h | 294 --- src/settings/settings_probotixV90.h | 237 --- src/settings/settings_shapeoko2.h | 283 --- src/settings/settings_test.h | 309 --- src/settings/settings_zen7x12.h | 245 --- src/spindle.c | 115 +- src/spindle.h | 12 +- src/status.c | 323 +-- src/{tinyg.h => status.h} | 296 +-- src/stepper.c | 290 +-- src/stepper.h | 212 +- src/switch.c | 80 +- src/switch.h | 22 +- src/test.c | 89 - src/test.h | 126 -- src/tests/test_001_smoke.h | 63 - src/tests/test_002_homing.h | 13 - src/tests/test_003_squares.h | 62 - src/tests/test_004_arcs.h | 38 - src/tests/test_005_dwell.h | 43 - src/tests/test_006_feedhold.h | 20 - src/tests/test_007_Mcodes.h | 62 - src/tests/test_008_json.h | 35 - src/tests/test_009_inverse_time.h | 71 - src/tests/test_010_rotary.h | 37 - src/tests/test_011_small_moves.h | 71 - src/tests/test_012_slow_moves.h | 22 - src/tests/test_013_coordinate_offsets.h | 55 - src/tests/test_014_microsteps.h | 47 - src/tests/test_050_mudflap.h | 334 --- src/tests/test_051_braid.h | 2549 ----------------------- src/tests/test_099.h | 112 - src/text_parser.c | 271 --- src/text_parser.h | 102 - src/tmc2660.c | 112 +- src/tmc2660.h | 12 +- src/util.c | 61 +- src/util.h | 48 +- src/vars.c | 249 +++ src/vars.def | 48 + src/vars.h | 40 + src/xmega/xmega_eeprom.c | 497 ----- src/xmega/xmega_eeprom.h | 216 -- src/xmega/xmega_init.c | 188 -- src/xmega/xmega_interrupts.c | 88 - src/xmega/xmega_interrupts.h | 160 -- src/xmega/xmega_rtc.c | 79 - 109 files changed, 6534 insertions(+), 17389 deletions(-) create mode 100644 cpp_magic.h create mode 100644 src/clock.c rename src/{xmega/xmega_init.h => clock.h} (80%) create mode 100644 src/command.c create mode 100644 src/command.def create mode 100644 src/command.h delete mode 100644 src/config.c delete mode 100644 src/config_app.c delete mode 100644 src/config_app.h create mode 100644 src/cpp_magic.h delete mode 100644 src/help.c delete mode 100644 src/help.h delete mode 100644 src/json_parser.c delete mode 100644 src/json_parser.h create mode 100644 src/messages.def delete mode 100644 src/persistence.c delete mode 100644 src/persistence.h create mode 100644 src/plan/arc.c create mode 100644 src/plan/arc.h create mode 100644 src/plan/exec.c rename src/{ => plan}/kinematics.c (98%) rename src/{ => plan}/kinematics.h (95%) create mode 100644 src/plan/line.c create mode 100644 src/plan/planner.c create mode 100644 src/plan/planner.h rename src/{plan_zoid.c => plan/zoid.c} (99%) delete mode 100644 src/plan_arc.c delete mode 100644 src/plan_arc.h delete mode 100644 src/plan_exec.c delete mode 100644 src/plan_line.c delete mode 100644 src/plan_line.h delete mode 100644 src/planner.c delete mode 100644 src/planner.h create mode 100644 src/rtc.c rename src/{xmega/xmega_rtc.h => rtc.h} (79%) delete mode 100644 src/settings.h delete mode 100644 src/settings/settings_Ultimaker.h delete mode 100644 src/settings/settings_cnc3040.h delete mode 100644 src/settings/settings_default.h delete mode 100644 src/settings/settings_openpnp.h delete mode 100644 src/settings/settings_othermill.h delete mode 100644 src/settings/settings_probotixV90.h delete mode 100644 src/settings/settings_shapeoko2.h delete mode 100644 src/settings/settings_test.h delete mode 100644 src/settings/settings_zen7x12.h rename src/{tinyg.h => status.h} (50%) delete mode 100644 src/test.c delete mode 100644 src/test.h delete mode 100644 src/tests/test_001_smoke.h delete mode 100644 src/tests/test_002_homing.h delete mode 100644 src/tests/test_003_squares.h delete mode 100644 src/tests/test_004_arcs.h delete mode 100644 src/tests/test_005_dwell.h delete mode 100644 src/tests/test_006_feedhold.h delete mode 100644 src/tests/test_007_Mcodes.h delete mode 100644 src/tests/test_008_json.h delete mode 100644 src/tests/test_009_inverse_time.h delete mode 100644 src/tests/test_010_rotary.h delete mode 100644 src/tests/test_011_small_moves.h delete mode 100644 src/tests/test_012_slow_moves.h delete mode 100644 src/tests/test_013_coordinate_offsets.h delete mode 100644 src/tests/test_014_microsteps.h delete mode 100644 src/tests/test_050_mudflap.h delete mode 100644 src/tests/test_051_braid.h delete mode 100644 src/tests/test_099.h delete mode 100644 src/text_parser.c delete mode 100644 src/text_parser.h create mode 100644 src/vars.c create mode 100644 src/vars.def create mode 100644 src/vars.h delete mode 100644 src/xmega/xmega_eeprom.c delete mode 100644 src/xmega/xmega_eeprom.h delete mode 100644 src/xmega/xmega_init.c delete mode 100644 src/xmega/xmega_interrupts.c delete mode 100644 src/xmega/xmega_interrupts.h delete mode 100644 src/xmega/xmega_rtc.c diff --git a/Makefile b/Makefile index 60c8120..ce3c7f4 100755 --- a/Makefile +++ b/Makefile @@ -15,6 +15,7 @@ CFLAGS += $(COMMON) CFLAGS += -gdwarf-2 -std=gnu99 -Wall -Werror -DF_CPU=$(CLOCK)UL -Os CFLAGS += -funsigned-bitfields -fpack-struct -fshort-enums -funsigned-char CFLAGS += -MD -MP -MT $@ -MF build/dep/$(@F).d +CFLAGS += -Isrc # Linker flags LDFLAGS += $(COMMON) -Wl,-u,vfprintf -lprintf_flt -lm -Wl,-Map=$(PROJECT).map @@ -38,7 +39,7 @@ FUSE5=0xeb # SRC SRC = $(wildcard src/*.c) -SRC += $(wildcard src/xmega/*.c) +SRC += $(wildcard src/plan/*.c) OBJ = $(patsubst src/%.c,build/%.o,$(SRC)) # Build diff --git a/cpp_magic.h b/cpp_magic.h new file mode 100644 index 0000000..cd2c8eb --- /dev/null +++ b/cpp_magic.h @@ -0,0 +1,442 @@ +/** + * This header file contains a library of advanced C Pre-Processor (CPP) macros + * which implement various useful functions, such as iteration, in the + * pre-processor. + * + * Though the file name (quite validly) labels this as magic, there should be + * enough documentation in the comments for a reader only casually familiar + * with the CPP to be able to understand how everything works. + * + * The majority of the magic tricks used in this file are based on those + * described by pfultz2 in his "Cloak" library: + * + * https://github.com/pfultz2/Cloak/wiki/C-Preprocessor-tricks,-tips,-and-idioms + * + * Major differences are a greater level of detailed explanation in this + * implementation and also a refusal to include any macros which require a O(N) + * macro definitions to handle O(N) arguments (with the exception of DEFERn). + */ + +#ifndef CPP_MAGIC_H +#define CPP_MAGIC_H + +/** + * Force the pre-processor to expand the macro a large number of times. Usage: + * + * EVAL(expression) + * + * This is useful when you have a macro which evaluates to a valid macro + * expression which is not subsequently expanded in the same pass. A contrived, + * but easy to understand, example of such a macro follows. Note that though + * this example is contrived, this behaviour is abused to implement bounded + * recursion in macros such as FOR. + * + * #define A(x) x+1 + * #define EMPTY + * #define NOT_QUITE_RIGHT(x) A EMPTY (x) + * NOT_QUITE_RIGHT(999) + * + * Here's what happens inside the C preprocessor: + * + * 1. It sees a macro "NOT_QUITE_RIGHT" and performs a single macro expansion + * pass on its arguments. Since the argument is "999" and this isn't a macro, + * this is a boring step resulting in no change. + * 2. The NOT_QUITE_RIGHT macro is substituted for its definition giving "A + * EMPTY() (x)". + * 3. The expander moves from left-to-right trying to expand the macro: + * The first token, A, cannot be expanded since there are no brackets + * immediately following it. The second token EMPTY(), however, can be + * expanded (recursively in this manner) and is replaced with "". + * 4. Expansion continues from the start of the substituted test (which in this + * case is just empty), and sees "(999)" but since no macro name is present, + * nothing is done. This results in a final expansion of "A (999)". + * + * Unfortunately, this doesn't quite meet expectations since you may expect that + * "A (999)" would have been expanded into "999+1". Unfortunately this requires + * a second expansion pass but luckily we can force the macro processor to make + * more passes by abusing the first step of macro expansion: the preprocessor + * expands arguments in their own pass. If we define a macro which does nothing + * except produce its arguments e.g.: + * + * #define PASS_THROUGH(...) __VA_ARGS__ + * + * We can now do "PASS_THROUGH(NOT_QUITE_RIGHT(999))" causing "NOT_QUITE_RIGHT" to be + * expanded to "A (999)", as described above, when the arguments are expanded. + * Now when the body of PASS_THROUGH is expanded, "A (999)" gets expanded to + * "999+1". + * + * The EVAL defined below is essentially equivalent to a large nesting of + * "PASS_THROUGH(PASS_THROUGH(PASS_THROUGH(..." which results in the + * preprocessor making a large number of expansion passes over the given + * expression. + */ +#define EVAL(...) EVAL1024(__VA_ARGS__) +#define EVAL1024(...) EVAL512(EVAL512(__VA_ARGS__)) +#define EVAL512(...) EVAL256(EVAL256(__VA_ARGS__)) +#define EVAL256(...) EVAL128(EVAL128(__VA_ARGS__)) +#define EVAL128(...) EVAL64(EVAL64(__VA_ARGS__)) +#define EVAL64(...) EVAL32(EVAL32(__VA_ARGS__)) +#define EVAL32(...) EVAL16(EVAL16(__VA_ARGS__)) +#define EVAL16(...) EVAL8(EVAL8(__VA_ARGS__)) +#define EVAL8(...) EVAL4(EVAL4(__VA_ARGS__)) +#define EVAL4(...) EVAL2(EVAL2(__VA_ARGS__)) +#define EVAL2(...) EVAL1(EVAL1(__VA_ARGS__)) +#define EVAL1(...) __VA_ARGS__ + + +/** + * Macros which expand to common values + */ +#define PASS(...) __VA_ARGS__ +#define EMPTY() +#define COMMA() , +#define PLUS() + +#define ZERO() 0 +#define ONE() 1 + +/** + * Causes a function-style macro to require an additional pass to be expanded. + * + * This is useful, for example, when trying to implement recursion since the + * recursive step must not be expanded in a single pass as the pre-processor + * will catch it and prevent it. + * + * Usage: + * + * DEFER1(IN_NEXT_PASS)(args, to, the, macro) + * + * How it works: + * + * 1. When DEFER1 is expanded, first its arguments are expanded which are + * simply IN_NEXT_PASS. Since this is a function-style macro and it has no + * arguments, nothing will happen. + * 2. The body of DEFER1 will now be expanded resulting in EMPTY() being + * deleted. This results in "IN_NEXT_PASS (args, to, the macro)". Note that + * since the macro expander has already passed IN_NEXT_PASS by the time it + * expands EMPTY() and so it won't spot that the brackets which remain can be + * applied to IN_NEXT_PASS. + * 3. At this point the macro expansion completes. If one more pass is made, + * IN_NEXT_PASS(args, to, the, macro) will be expanded as desired. + */ +#define DEFER1(id) id EMPTY() + +/** + * As with DEFER1 except here n additional passes are required for DEFERn. + * + * The mechanism is analogous. + * + * Note that there doesn't appear to be a way of combining DEFERn macros in + * order to achieve exponentially increasing defers e.g. as is done by EVAL. + */ +#define DEFER2(id) id EMPTY EMPTY()() +#define DEFER3(id) id EMPTY EMPTY EMPTY()()() +#define DEFER4(id) id EMPTY EMPTY EMPTY EMPTY()()()() +#define DEFER5(id) id EMPTY EMPTY EMPTY EMPTY EMPTY()()()()() +#define DEFER6(id) id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()() +#define DEFER7(id) id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()() +#define DEFER8(id) id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()()() + + +/** + * Indirection around the standard ## concatenation operator. This simply + * ensures that the arguments are expanded (once) before concatenation. + */ +#define CAT(a, ...) a ## __VA_ARGS__ +#define CAT3(a, b, ...) a ## b ## __VA_ARGS__ + + +/** + * Get the first argument and ignore the rest. + */ +#define FIRST(a, ...) a + +/** + * Get the second argument and ignore the rest. + */ +#define SECOND(a, b, ...) b + +/** + * Expects a single input (not containing commas). Returns 1 if the input is + * PROBE() and otherwise returns 0. + * + * This can be useful as the basis of a NOT function. + * + * This macro abuses the fact that PROBE() contains a comma while other valid + * inputs must not. + */ +#define IS_PROBE(...) SECOND(__VA_ARGS__, 0) +#define PROBE() ~, 1 + + +/** + * Logical negation. 0 is defined as false and everything else as true. + * + * When 0, _NOT_0 will be found which evaluates to the PROBE. When 1 (or any other + * value) is given, an appropriately named macro won't be found and the + * concatenated string will be produced. IS_PROBE then simply checks to see if + * the PROBE was returned, cleanly converting the argument into a 1 or 0. + */ +#define NOT(x) IS_PROBE(CAT(_NOT_, x)) +#define _NOT_0 PROBE() + +/** + * Macro version of C's famous "cast to bool" operator (i.e. !!) which takes + * anything and casts it to 0 if it is 0 and 1 otherwise. + */ +#define BOOL(x) NOT(NOT(x)) + +/** + * Logical OR. Simply performs a lookup. + */ +#define OR(a,b) CAT3(_OR_, a, b) +#define _OR_00 0 +#define _OR_01 1 +#define _OR_10 1 +#define _OR_11 1 + +/** + * Logical AND. Simply performs a lookup. + */ +#define AND(a,b) CAT3(_AND_, a, b) +#define _AND_00 0 +#define _AND_01 0 +#define _AND_10 0 +#define _AND_11 1 + + +/** + * Macro if statement. Usage: + * + * IF(c)(expansion when true) + * + * Here's how: + * + * 1. The preprocessor expands the arguments to _IF casting the condition to '0' + * or '1'. + * 2. The casted condition is concatencated with _IF_ giving _IF_0 or _IF_1. + * 3. The _IF_0 and _IF_1 macros either returns the argument or doesn't (e.g. + * they implement the "choice selection" part of the macro). + * 4. Note that the "true" clause is in the extra set of brackets; thus these + * become the arguments to _IF_0 or _IF_1 and thus a selection is made! + */ +#define IF(c) _IF(BOOL(c)) +#define _IF(c) CAT(_IF_,c) +#define _IF_0(...) +#define _IF_1(...) __VA_ARGS__ + +/** + * Macro if/else statement. Usage: + * + * IF_ELSE(c)( \ + * expansion when true, \ + * expansion when false \ + * ) + * + * The mechanism is analogous to IF. + */ +#define IF_ELSE(c) _IF_ELSE(BOOL(c)) +#define _IF_ELSE(c) CAT(_IF_ELSE_,c) +#define _IF_ELSE_0(t,f) f +#define _IF_ELSE_1(t,f) t + + +/** + * Macro which checks if it has any arguments. Returns '0' if there are no + * arguments, '1' otherwise. + * + * Limitation: HAS_ARGS(,1,2,3) returns 0 -- this check essentially only checks + * that the first argument exists. + * + * This macro works as follows: + * + * 1. _END_OF_ARGUMENTS_ is concatenated with the first argument. + * 2. If the first argument is not present then only "_END_OF_ARGUMENTS_" will + * remain, otherwise "_END_OF_ARGUMENTS something_here" will remain. + * 3. In the former case, the _END_OF_ARGUMENTS_() macro expands to a + * 0 when it is expanded. In the latter, a non-zero result remains. + * 4. BOOL is used to force non-zero results into 1 giving the clean 0 or 1 + * output required. + */ +#define HAS_ARGS(...) BOOL(FIRST(_END_OF_ARGUMENTS_ __VA_ARGS__)()) +#define _END_OF_ARGUMENTS_() 0 + + +/** + * Macro map/list comprehension. Usage: + * + * MAP(op, sep, ...) + * + * Produces a 'sep()'-separated list of the result of op(arg) for each arg. + * + * Example Usage: + * + * #define MAKE_HAPPY(x) happy_##x + * #define COMMA() , + * MAP(MAKE_HAPPY, COMMA, 1,2,3) + * + * Which expands to: + * + * happy_1 , happy_2 , happy_3 + * + * How it works: + * + * 1. The MAP macro simply maps the inner MAP_INNER function in an EVAL which + * forces it to be expanded a large number of times, thus enabling many steps + * of iteration (see step 6). + * 2. The MAP_INNER macro is substituted for its body. + * 3. In the body, op(cur_val) is substituted giving the value for this + * iteration. + * 4. The IF macro expands according to whether further iterations are required. + * This expansion either produces _IF_0 or _IF_1. + * 5. Since the IF is followed by a set of brackets containing the "if true" + * clause, these become the argument to _IF_0 or _IF_1. At this point, the + * macro in the brackets will be expanded giving the separator followed by + * _MAP_INNER EMPTY()()(op, sep, __VA_ARGS__). + * 5. If the IF was not taken, the above will simply be discarded and everything + * stops. If the IF is taken, The expression is then processed a second time + * yielding "_MAP_INNER()(op, sep, __VA_ARGS__)". Note that this call looks + * very similar to the essentially the same as the original call except the + * first argument has been dropped. + * 6. At this point expansion of MAP_INNER will terminate. However, since we can + * force more rounds of expansion using EVAL1. In the argument-expansion pass + * of the EVAL1, _MAP_INNER() is expanded to MAP_INNER which is then expanded + * using the arguments which follow it as in step 2-5. This is followed by a + * second expansion pass as the substitution of EVAL1() is expanded executing + * 2-5 a second time. This results in up to two iterations occurring. Using + * many nested EVAL1 macros, i.e. the very-deeply-nested EVAL macro, will in + * this manner produce further iterations, hence the outer MAP macro doing + * this for us. + * + * Important tricks used: + * + * * If we directly produce "MAP_INNER" in an expansion of MAP_INNER, a special + * case in the preprocessor will prevent it being expanded in the future, even + * if we EVAL. As a result, the MAP_INNER macro carefully only expands to + * something containing "_MAP_INNER()" which requires a further expansion step + * to invoke MAP_INNER and thus implementing the recursion. + * * To prevent _MAP_INNER being expanded within the macro we must first defer its + * expansion during its initial pass as an argument to _IF_0 or _IF_1. We must + * then defer its expansion a second time as part of the body of the _IF_0. As + * a result hence the DEFER2. + * * _MAP_INNER seemingly gets away with producing itself because it actually only + * produces MAP_INNER. It just happens that when _MAP_INNER() is expanded in + * this case it is followed by some arguments which get consumed by MAP_INNER + * and produce a _MAP_INNER. As such, the macro expander never marks + * _MAP_INNER as expanding to itself and thus it will still be expanded in + * future productions of itself. + */ +#define MAP(...) \ + IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_INNER(__VA_ARGS__))) +#define MAP_INNER(op,sep,cur_val, ...) \ + op(cur_val) \ + IF(HAS_ARGS(__VA_ARGS__))( \ + sep() DEFER2(_MAP_INNER)()(op, sep, ##__VA_ARGS__) \ + ) +#define _MAP_INNER() MAP_INNER + + +/** + * This is a variant of the MAP macro which also includes as an argument to the + * operation a valid C variable name which is different for each iteration. + * + * Usage: + * MAP_WITH_ID(op, sep, ...) + * + * Where op is a macro op(val, id) which takes a list value and an ID. This ID + * will simply be a unary number using the digit "I", that is, I, II, III, IIII, + * and so on. + * + * Example: + * + * #define MAKE_STATIC_VAR(type, name) static type name; + * MAP_WITH_ID(MAKE_STATIC_VAR, EMPTY, int, int, int, bool, char) + * + * Which expands to: + * + * static int I; static int II; static int III; static bool IIII; static char IIIII; + * + * The mechanism is analogous to the MAP macro. + */ +#define MAP_WITH_ID(op,sep,...) \ + IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_WITH_ID_INNER(op,sep,I, ##__VA_ARGS__))) +#define MAP_WITH_ID_INNER(op,sep,id,cur_val, ...) \ + op(cur_val,id) \ + IF(HAS_ARGS(__VA_ARGS__))( \ + sep() DEFER2(_MAP_WITH_ID_INNER)()(op, sep, CAT(id,I), ##__VA_ARGS__) \ + ) +#define _MAP_WITH_ID_INNER() MAP_WITH_ID_INNER + + +/** + * This is a variant of the MAP macro which iterates over pairs rather than + * singletons. + * + * Usage: + * MAP_PAIRS(op, sep, ...) + * + * Where op is a macro op(val_1, val_2) which takes two list values. + * + * Example: + * + * #define MAKE_STATIC_VAR(type, name) static type name; + * MAP_PAIRS(MAKE_STATIC_VAR, EMPTY, char, my_char, int, my_int) + * + * Which expands to: + * + * static char my_char; static int my_int; + * + * The mechanism is analogous to the MAP macro. + */ +#define MAP_PAIRS(op,sep,...) \ + IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_PAIRS_INNER(op,sep,__VA_ARGS__))) +#define MAP_PAIRS_INNER(op,sep,cur_val_1, cur_val_2, ...) \ + op(cur_val_1,cur_val_2) \ + IF(HAS_ARGS(__VA_ARGS__))( \ + sep() DEFER2(_MAP_PAIRS_INNER)()(op, sep, __VA_ARGS__) \ + ) +#define _MAP_PAIRS_INNER() MAP_PAIRS_INNER + +/** + * This is a variant of the MAP macro which iterates over a two-element sliding + * window. + * + * Usage: + * MAP_SLIDE(op, last_op, sep, ...) + * + * Where op is a macro op(val_1, val_2) which takes the two list values + * currently in the window. last_op is a macro taking a single value which is + * called for the last argument. + * + * Example: + * + * #define SIMON_SAYS_OP(simon, next) IF(NOT(simon()))(next) + * #define SIMON_SAYS_LAST_OP(val) last_but_not_least_##val + * #define SIMON_SAYS() 0 + * + * MAP_SLIDE(SIMON_SAYS_OP, SIMON_SAYS_LAST_OP, EMPTY, wiggle, SIMON_SAYS, dance, move, SIMON_SAYS, boogie, stop) + * + * Which expands to: + * + * dance boogie last_but_not_least_stop + * + * The mechanism is analogous to the MAP macro. + */ +#define MAP_SLIDE(op,last_op,sep,...) \ + IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_SLIDE_INNER(op,last_op,sep,__VA_ARGS__))) +#define MAP_SLIDE_INNER(op,last_op,sep,cur_val, ...) \ + IF(HAS_ARGS(__VA_ARGS__))(op(cur_val,FIRST(__VA_ARGS__))) \ + IF(NOT(HAS_ARGS(__VA_ARGS__)))(last_op(cur_val)) \ + IF(HAS_ARGS(__VA_ARGS__))( \ + sep() DEFER2(_MAP_SLIDE_INNER)()(op, last_op, sep, __VA_ARGS__) \ + ) +#define _MAP_SLIDE_INNER() MAP_SLIDE_INNER + + +/** + * Strip any excess commas from a set of arguments. + */ +#define REMOVE_TRAILING_COMMAS(...) \ + MAP(PASS, COMMA, __VA_ARGS__) + + +#endif diff --git a/src/canonical_machine.c b/src/canonical_machine.c index 29a650d..2091543 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -89,22 +89,26 @@ * and have no buffer. */ -#include "tinyg.h" // #1 -#include "config.h" // #2 -#include "text_parser.h" #include "canonical_machine.h" -#include "plan_arc.h" -#include "planner.h" + +#include "config.h" #include "stepper.h" #include "encoder.h" #include "spindle.h" -#include "report.h" #include "gpio.h" #include "switch.h" #include "hardware.h" #include "util.h" #include "usart.h" // for serial queue flush +#include "plan/arc.h" +#include "plan/planner.h" + +#include +#include +#include +#include + cmSingleton_t cm; // canonical machine controller singleton // command execution callbacks from planner queue @@ -116,9 +120,6 @@ static void _exec_flood_coolant_control(float *value, float *flag); static void _exec_absolute_origin(float *value, float *flag); static void _exec_program_finalize(float *value, float *flag); -static int8_t _get_axis(const index_t index); -static int8_t _get_axis_type(const index_t index); - /* * Canonical Machine State functions * @@ -184,21 +185,33 @@ uint8_t cm_get_runtime_busy() {return mp_get_runtime_busy();} float cm_get_feed_rate(GCodeState_t *gcode_state) {return gcode_state->feed_rate;} -void cm_set_motion_mode(GCodeState_t *gcode_state, uint8_t motion_mode) {gcode_state->motion_mode = motion_mode;} -void cm_set_spindle_mode(GCodeState_t *gcode_state, uint8_t spindle_mode) {gcode_state->spindle_mode = spindle_mode;} -void cm_set_spindle_speed_parameter(GCodeState_t *gcode_state, float speed) {gcode_state->spindle_speed = speed;} + +void cm_set_motion_mode(GCodeState_t *gcode_state, uint8_t motion_mode) { + gcode_state->motion_mode = motion_mode; +} + + +void cm_set_spindle_mode(GCodeState_t *gcode_state, uint8_t spindle_mode) { + gcode_state->spindle_mode = spindle_mode; +} + + +void cm_set_spindle_speed_parameter(GCodeState_t *gcode_state, float speed) { + gcode_state->spindle_speed = speed; +} + + void cm_set_tool_number(GCodeState_t *gcode_state, uint8_t tool) {gcode_state->tool = tool;} void cm_set_absolute_override(GCodeState_t *gcode_state, uint8_t absolute_override) { gcode_state->absolute_override = absolute_override; - cm_set_work_offsets(MODEL); // must reset offsets if you change absolute override + cm_set_work_offsets(MODEL); // must reset offsets if you change absolute override } void cm_set_model_linenum(uint32_t linenum) { - cm.gm.linenum = linenum; // you must first set the model line number, - nv_add_object((const char_t *)"n"); // then add the line number to the nv list + cm.gm.linenum = linenum; // you must first set the model line number, } @@ -224,7 +237,7 @@ void cm_set_model_linenum(uint32_t linenum) { * * To reduce complexity and data load the following is done: * - Full data for coordinates/offsets is only accessible by the canonical machine, not the downstream - * - A fully resolved set of coord and G92 offsets, with per-move exceptions can be captured as "work_offsets" + * - Resolved set of coord and G92 offsets, with per-move exceptions can be captured as "work_offsets" * - The core gcode context (gm) only knows about the active coord system and the work offsets */ @@ -266,7 +279,7 @@ float cm_get_work_offset(GCodeState_t *gcode_state, uint8_t axis) { * MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model * PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to * RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct - * ACTIVE_MODEL cm.am // active model pointer is maintained by state management + * ACTIVE_MODEL cm.am // active model pointer maintained by state management */ void cm_set_work_offsets(GCodeState_t *gcode_state) { for (uint8_t axis = AXIS_X; axis < AXES; axis++) @@ -275,7 +288,7 @@ void cm_set_work_offsets(GCodeState_t *gcode_state) { /* - * cm_get_absolute_position() - get position of axis in absolute coordinates + * Get position of axis in absolute coordinates * * This function accepts as input: * MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model @@ -289,8 +302,9 @@ float cm_get_absolute_position(GCodeState_t *gcode_state, uint8_t axis) { return mp_get_runtime_absolute_position(axis); } + /* - * cm_get_work_position() - return work position in external form + * Return work position in external form * * ... that means in prevailing units (mm/inch) and with all offsets applied * @@ -323,12 +337,11 @@ float cm_get_work_position(GCodeState_t *gcode_state, uint8_t axis) { ***********************************************************************************/ /* - * cm_finalize_move() - perform final operations for a traverse or feed - * cm_update_model_position_from_runtime() - set endpoint position from final runtime position + * Perform final operations for a traverse or feed * - * These routines set the point position in the gcode model. + * These routines set the point position in the gcode model. * - * Note: As far as the canonical machine is concerned the final position of a Gcode block (move) + * Note: As far as the canonical machine is concerned the final position of a Gcode block (move) * is achieved as soon as the move is planned and the move target becomes the new model position. * In reality the planner will (in all likelihood) have only just queued the move for later * execution, and the real tool position is still close to the starting point. @@ -342,27 +355,23 @@ void cm_finalize_move() { } +/// Set endpoint position from final runtime position void cm_update_model_position_from_runtime() {copy_vector(cm.gmx.position, mr.gm.target);} /* - * cm_deferred_write_callback() - write any changed G10 values back to persistence + * Write any changed G10 values back to persistence * * Only runs if there is G10 data to write, there is no movement, and the serial queues are quiescent * This could be made tighter by issuing an XOFF or ~CTS beforehand and releasing it afterwards. */ stat_t cm_deferred_write_callback() { - if ((cm.cycle_state == CYCLE_OFF) && (cm.deferred_write_flag == true)) { - if (!usart_rx_empty()) return STAT_OK; // don't write back if serial RX is not empty + if (cm.cycle_state == CYCLE_OFF && cm.deferred_write_flag && usart_rx_empty()) { cm.deferred_write_flag = false; - nvObj_t nv; - for (uint8_t i=1; i<=COORDS; i++) - for (uint8_t j=0; j DISABLE_SOFT_LIMIT) && (target[axis] > cm.a[axis].travel_max)) return STAT_SOFT_LIMIT_EXCEEDED; } - } return STAT_OK; } @@ -475,10 +483,141 @@ void canonical_machine_init() { memset(&cm.gn, 0, sizeof(GCodeInput_t)); memset(&cm.gf, 0, sizeof(GCodeInput_t)); - canonical_machine_init_assertions(); // establish assertions ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer - // set gcode defaults + // axes defaults + cm.a[AXIS_X].axis_mode = X_AXIS_MODE; + cm.a[AXIS_X].velocity_max = X_VELOCITY_MAX; + cm.a[AXIS_X].feedrate_max = X_FEEDRATE_MAX; + cm.a[AXIS_X].travel_min = X_TRAVEL_MIN; + cm.a[AXIS_X].travel_max = X_TRAVEL_MAX; + cm.a[AXIS_X].jerk_max = X_JERK_MAX; + cm.a[AXIS_X].jerk_homing = X_JERK_HOMING; + cm.a[AXIS_X].junction_dev = X_JUNCTION_DEVIATION; + cm.a[AXIS_X].search_velocity = X_SEARCH_VELOCITY; + cm.a[AXIS_X].latch_velocity = X_LATCH_VELOCITY; + cm.a[AXIS_X].latch_backoff = X_LATCH_BACKOFF; + cm.a[AXIS_X].zero_backoff = X_ZERO_BACKOFF; + + cm.a[AXIS_Y].axis_mode = Y_AXIS_MODE; + cm.a[AXIS_Y].velocity_max = Y_VELOCITY_MAX; + cm.a[AXIS_Y].feedrate_max = Y_FEEDRATE_MAX; + cm.a[AXIS_Y].travel_min = Y_TRAVEL_MIN; + cm.a[AXIS_Y].travel_max = Y_TRAVEL_MAX; + cm.a[AXIS_Y].jerk_max = Y_JERK_MAX; + cm.a[AXIS_Y].jerk_homing = Y_JERK_HOMING; + cm.a[AXIS_Y].junction_dev = Y_JUNCTION_DEVIATION; + cm.a[AXIS_Y].search_velocity = Y_SEARCH_VELOCITY; + cm.a[AXIS_Y].latch_velocity = Y_LATCH_VELOCITY; + cm.a[AXIS_Y].latch_backoff = Y_LATCH_BACKOFF; + cm.a[AXIS_Y].zero_backoff = Y_ZERO_BACKOFF; + + cm.a[AXIS_Z].axis_mode = Z_AXIS_MODE; + cm.a[AXIS_Z].velocity_max = Z_VELOCITY_MAX; + cm.a[AXIS_Z].feedrate_max = Z_FEEDRATE_MAX; + cm.a[AXIS_Z].travel_min = Z_TRAVEL_MIN; + cm.a[AXIS_Z].travel_max = Z_TRAVEL_MAX; + cm.a[AXIS_Z].jerk_max = Z_JERK_MAX; + cm.a[AXIS_Z].jerk_homing = Z_JERK_HOMING; + cm.a[AXIS_Z].junction_dev = Z_JUNCTION_DEVIATION; + cm.a[AXIS_Z].search_velocity = Z_SEARCH_VELOCITY; + cm.a[AXIS_Z].latch_velocity = Z_LATCH_VELOCITY; + cm.a[AXIS_Z].latch_backoff = Z_LATCH_BACKOFF; + cm.a[AXIS_Z].zero_backoff = Z_ZERO_BACKOFF; + + cm.a[AXIS_A].axis_mode = A_AXIS_MODE; + cm.a[AXIS_A].velocity_max = A_VELOCITY_MAX; + cm.a[AXIS_A].feedrate_max = A_FEEDRATE_MAX; + cm.a[AXIS_A].travel_min = A_TRAVEL_MIN; + cm.a[AXIS_A].travel_max = A_TRAVEL_MAX; + cm.a[AXIS_A].jerk_max = A_JERK_MAX; + cm.a[AXIS_A].jerk_homing = A_JERK_HOMING; + cm.a[AXIS_A].junction_dev = A_JUNCTION_DEVIATION; + cm.a[AXIS_A].radius = A_RADIUS; + cm.a[AXIS_A].search_velocity = A_SEARCH_VELOCITY; + cm.a[AXIS_A].latch_velocity = A_LATCH_VELOCITY; + cm.a[AXIS_A].latch_backoff = A_LATCH_BACKOFF; + cm.a[AXIS_A].zero_backoff = A_ZERO_BACKOFF; + + cm.a[AXIS_B].axis_mode = B_AXIS_MODE; + cm.a[AXIS_B].velocity_max = B_VELOCITY_MAX; + cm.a[AXIS_B].feedrate_max = B_FEEDRATE_MAX; + cm.a[AXIS_B].travel_min = B_TRAVEL_MIN; + cm.a[AXIS_B].travel_max = B_TRAVEL_MAX; + cm.a[AXIS_B].jerk_max = B_JERK_MAX; + cm.a[AXIS_B].junction_dev = B_JUNCTION_DEVIATION; + cm.a[AXIS_B].radius = B_RADIUS; + + cm.a[AXIS_C].axis_mode = C_AXIS_MODE; + cm.a[AXIS_C].velocity_max = C_VELOCITY_MAX; + cm.a[AXIS_C].feedrate_max = C_FEEDRATE_MAX; + cm.a[AXIS_C].travel_min = C_TRAVEL_MIN; + cm.a[AXIS_C].travel_max = C_TRAVEL_MAX; + cm.a[AXIS_C].jerk_max = C_JERK_MAX; + cm.a[AXIS_C].junction_dev = C_JUNCTION_DEVIATION; + cm.a[AXIS_C].radius = C_RADIUS; + + // Init 1/jerk + for (uint8_t axis = 0; axis < AXES; axis++) + cm.a[axis].recip_jerk = 1 / (cm.a[axis].jerk_max * JERK_MULTIPLIER); + + // Coordinate system offset defaults (G54-G59) + cm.offset[G54][AXIS_X] = G54_X_OFFSET; + cm.offset[G54][AXIS_Y] = G54_Y_OFFSET; + cm.offset[G54][AXIS_Z] = G54_Z_OFFSET; + cm.offset[G54][AXIS_A] = G54_A_OFFSET; + cm.offset[G54][AXIS_B] = G54_B_OFFSET; + cm.offset[G54][AXIS_C] = G54_C_OFFSET; + + cm.offset[G55][AXIS_X] = G55_X_OFFSET; + cm.offset[G55][AXIS_Y] = G55_Y_OFFSET; + cm.offset[G55][AXIS_Z] = G55_Z_OFFSET; + cm.offset[G55][AXIS_A] = G55_A_OFFSET; + cm.offset[G55][AXIS_B] = G55_B_OFFSET; + cm.offset[G55][AXIS_C] = G55_C_OFFSET; + + cm.offset[G56][AXIS_X] = G56_X_OFFSET; + cm.offset[G56][AXIS_Y] = G56_Y_OFFSET; + cm.offset[G56][AXIS_Z] = G56_Z_OFFSET; + cm.offset[G56][AXIS_A] = G56_A_OFFSET; + cm.offset[G56][AXIS_B] = G56_B_OFFSET; + cm.offset[G56][AXIS_C] = G56_C_OFFSET; + + cm.offset[G57][AXIS_X] = G57_X_OFFSET; + cm.offset[G57][AXIS_Y] = G57_Y_OFFSET; + cm.offset[G57][AXIS_Z] = G57_Z_OFFSET; + cm.offset[G57][AXIS_A] = G57_A_OFFSET; + cm.offset[G57][AXIS_B] = G57_B_OFFSET; + cm.offset[G57][AXIS_C] = G57_C_OFFSET; + + cm.offset[G58][AXIS_X] = G58_X_OFFSET; + cm.offset[G58][AXIS_Y] = G58_Y_OFFSET; + cm.offset[G58][AXIS_Z] = G58_Z_OFFSET; + cm.offset[G58][AXIS_A] = G58_A_OFFSET; + cm.offset[G58][AXIS_B] = G58_B_OFFSET; + cm.offset[G58][AXIS_C] = G58_C_OFFSET; + + cm.offset[G59][AXIS_X] = G59_X_OFFSET; + cm.offset[G59][AXIS_Y] = G59_Y_OFFSET; + cm.offset[G59][AXIS_Z] = G59_Z_OFFSET; + cm.offset[G59][AXIS_A] = G59_A_OFFSET; + cm.offset[G59][AXIS_B] = G59_B_OFFSET; + cm.offset[G59][AXIS_C] = G59_C_OFFSET; + + // Machine defaults + cm.junction_acceleration = JUNCTION_ACCELERATION; + cm.chordal_tolerance = CHORDAL_TOLERANCE; + cm.soft_limit_enable = SOFT_LIMIT_ENABLE; + cm.arc_segment_len = ARC_SEGMENT_LENGTH; + + // GCode defaults + cm.select_plane = GCODE_DEFAULT_PLANE; + cm.units_mode = GCODE_DEFAULT_UNITS; + cm.coord_system = GCODE_DEFAULT_COORD_SYSTEM; + cm.path_control = GCODE_DEFAULT_PATH_CONTROL; + cm.distance_mode = GCODE_DEFAULT_DISTANCE_MODE; + + // Set gcode defaults cm_set_units_mode(cm.units_mode); cm_set_coord_system(cm.coord_system); cm_select_plane(cm.select_plane); @@ -488,60 +627,34 @@ void canonical_machine_init() { cm.gmx.block_delete_switch = true; - // never start a machine in a motion mode + // Never start a machine in a motion mode cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; - // reset request flags + // Reset request flags cm.feedhold_requested = false; cm.queue_flush_requested = false; cm.cycle_start_requested = false; - // signal that the machine is ready for action + // Signal that the machine is ready for action cm.machine_state = MACHINE_READY; cm.combined_state = COMBINED_READY; - // sub-system inits + // Sub-system inits cm_spindle_init(); cm_arc_init(); } -/* - * canonical_machine_init_assertions() - * canonical_machine_test_assertions() - test assertions, return error code if violation exists - */ -void canonical_machine_init_assertions() { - cm.magic_start = MAGICNUM; - cm.magic_end = MAGICNUM; - cm.gmx.magic_start = MAGICNUM; - cm.gmx.magic_end = MAGICNUM; - arc.magic_start = MAGICNUM; - arc.magic_end = MAGICNUM; -} - - -stat_t canonical_machine_test_assertions() { - if ((cm.magic_start != MAGICNUM) || (cm.magic_end != MAGICNUM)) return STAT_CANONICAL_MACHINE_ASSERTION_FAILURE; - if ((cm.gmx.magic_start != MAGICNUM) || (cm.gmx.magic_end != MAGICNUM)) return STAT_CANONICAL_MACHINE_ASSERTION_FAILURE; - if ((arc.magic_start != MAGICNUM) || (arc.magic_end != MAGICNUM)) return STAT_CANONICAL_MACHINE_ASSERTION_FAILURE; - - return STAT_OK; -} - - -/* - * cm_soft_alarm() - alarm state; send an exception report and stop processing input - * cm_clear() - clear soft alarm - * cm_hard_alarm() - alarm state; send an exception report and shut down machine - */ +/// Alarm state; send an exception report and stop processing input stat_t cm_soft_alarm(stat_t status) { - rpt_exception(status); // send alarm message + print_status_message("Soft alarm", status); cm.machine_state = MACHINE_ALARM; - return status; // NB: More efficient than inlining rpt_exception() call. + return status; } -stat_t cm_clear(nvObj_t *nv) { // clear soft alarm +/// Clear soft alarm +stat_t cm_clear() { if (cm.cycle_state == CYCLE_OFF) cm.machine_state = MACHINE_PROGRAM_STOP; else cm.machine_state = MACHINE_CYCLE; @@ -550,11 +663,12 @@ stat_t cm_clear(nvObj_t *nv) { // clear soft alarm } +/// Alarm state; send an exception report and shut down machine stat_t cm_hard_alarm(stat_t status) { - // stop the motors and the spindle - stepper_init(); // hard stop + // Hard stop the motors and the spindle + stepper_init(); cm_spindle_control(SPINDLE_OFF); - rpt_exception(status); // send shutdown message + print_status_message("HARD ALARM", status); cm.machine_state = MACHINE_SHUTDOWN; return status; } @@ -865,7 +979,7 @@ stat_t cm_set_path_control(uint8_t mode) { * Machining Functions (4.3.6) * *******************************/ /* - * cm_arc_feed() - SEE plan_arc.c(pp) + * cm_arc_feed() - SEE arc.c(pp) */ @@ -895,6 +1009,7 @@ stat_t cm_straight_feed(float target[], float flags[]) { cm_cycle_start(); // required for homing & other cycles status = mp_aline(&cm.gm); // send the move to the planner cm_finalize_move(); + return status; } @@ -1051,13 +1166,8 @@ stat_t cm_spindle_override_factor(uint8_t flag) { // M50.1 } -/* - * cm_message() - queue a RAM string as a message in the response (unconditionally) - * - * Note: If you need to post a FLASH string use pstr2str to convert it to a RAM string - */ -void cm_message(char_t *message) { - nv_add_string((const char_t *)"msg", message); // add message to the response object +void cm_message(char *message) { + printf(message); } @@ -1160,8 +1270,6 @@ stat_t cm_queue_flush() { usart_rx_flush(); // flush serial queues mp_flush_planner(); // flush planner queue - qr_request_queue_report(0); // request a queue report, since we've changed the number of buffers available - rx_request_rx_report(); // Note: The following uses low-level mp calls for absolute position. // It could also use cm_get_absolute_position(RUNTIME, axis); @@ -1231,18 +1339,14 @@ static void _exec_program_finalize(float *value, float *flag) { cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // G94 cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); } - - sr_request_status_report(SR_IMMEDIATE_REQUEST); // request a final status report (not unfiltered) } void cm_cycle_start() { cm.machine_state = MACHINE_CYCLE; - if (cm.cycle_state == CYCLE_OFF) { // don't (re)start homing, probe or other canned cycles + if (cm.cycle_state == CYCLE_OFF) // don't (re)start homing, probe or other canned cycles cm.cycle_state = CYCLE_MACHINING; - qr_init_queue_report(); // clear queue reporting buffer counts - } } @@ -1272,311 +1376,14 @@ void cm_program_end() { } -// Strings for writing settings as nvObj string values -// Ref: http://www.avrfreaks.net/index.php?name=PNphpBB2&file=printview&t=120881&start=0 -#ifdef __TEXT_MODE - -static const char msg_units0[] PROGMEM = " in"; // used by generic print functions -static const char msg_units1[] PROGMEM = " mm"; -static const char msg_units2[] PROGMEM = " deg"; -static const char *const msg_units[] PROGMEM = {msg_units0, msg_units1, msg_units2}; -#define DEGREE_INDEX 2 - -static const char msg_am00[] PROGMEM = "[disabled]"; -static const char msg_am01[] PROGMEM = "[standard]"; -static const char msg_am02[] PROGMEM = "[inhibited]"; -static const char msg_am03[] PROGMEM = "[radius]"; -static const char *const msg_am[] PROGMEM = {msg_am00, msg_am01, msg_am02, msg_am03}; - -static const char msg_g20[] PROGMEM = "G20 - inches mode"; -static const char msg_g21[] PROGMEM = "G21 - millimeter mode"; -static const char *const msg_unit[] PROGMEM = {msg_g20, msg_g21}; - -static const char msg_stat0[] PROGMEM = "Initializing"; // combined state (stat) uses this array -static const char msg_stat1[] PROGMEM = "Ready"; -static const char msg_stat2[] PROGMEM = "Alarm"; -static const char msg_stat3[] PROGMEM = "Stop"; -static const char msg_stat4[] PROGMEM = "End"; -static const char msg_stat5[] PROGMEM = "Run"; -static const char msg_stat6[] PROGMEM = "Hold"; -static const char msg_stat7[] PROGMEM = "Probe"; -static const char msg_stat8[] PROGMEM = "Cycle"; -static const char msg_stat9[] PROGMEM = "Homing"; -static const char msg_stat10[] PROGMEM = "Jog"; -static const char msg_stat11[] PROGMEM = "Shutdown"; -static const char *const msg_stat[] PROGMEM = { - msg_stat0, msg_stat1, msg_stat2, msg_stat3, - msg_stat4, msg_stat5, msg_stat6, msg_stat7, - msg_stat8, msg_stat9, msg_stat10, msg_stat11 -}; - -static const char msg_macs0[] PROGMEM = "Initializing"; -static const char msg_macs1[] PROGMEM = "Ready"; -static const char msg_macs2[] PROGMEM = "Alarm"; -static const char msg_macs3[] PROGMEM = "Stop"; -static const char msg_macs4[] PROGMEM = "End"; -static const char msg_macs5[] PROGMEM = "Cycle"; -static const char msg_macs6[] PROGMEM = "Shutdown"; -static const char *const msg_macs[] PROGMEM = { - msg_macs0, msg_macs1, msg_macs2, msg_macs3, - msg_macs4, msg_macs5, msg_macs6 -}; - -static const char msg_cycs0[] PROGMEM = "Off"; -static const char msg_cycs1[] PROGMEM = "Machining"; -static const char msg_cycs2[] PROGMEM = "Probe"; -static const char msg_cycs3[] PROGMEM = "Homing"; -static const char msg_cycs4[] PROGMEM = "Jog"; -static const char *const msg_cycs[] PROGMEM = {msg_cycs0, msg_cycs1, msg_cycs2, msg_cycs3, msg_cycs4}; - -static const char msg_mots0[] PROGMEM = "Stop"; -static const char msg_mots1[] PROGMEM = "Run"; -static const char msg_mots2[] PROGMEM = "Hold"; -static const char *const msg_mots[] PROGMEM = {msg_mots0, msg_mots1, msg_mots2}; - -static const char msg_hold0[] PROGMEM = "Off"; -static const char msg_hold1[] PROGMEM = "Sync"; -static const char msg_hold2[] PROGMEM = "Plan"; -static const char msg_hold3[] PROGMEM = "Decel"; -static const char msg_hold4[] PROGMEM = "Hold"; -static const char msg_hold5[] PROGMEM = "End Hold"; -static const char *const msg_hold[] PROGMEM = {msg_hold0, msg_hold1, msg_hold2, msg_hold3, msg_hold4, msg_hold5}; - -static const char msg_home0[] PROGMEM = "Not Homed"; -static const char msg_home1[] PROGMEM = "Homed"; -static const char msg_home2[] PROGMEM = "Homing"; -static const char *const msg_home[] PROGMEM = {msg_home0, msg_home1, msg_home2}; - -static const char msg_g53[] PROGMEM = "G53 - machine coordinate system"; -static const char msg_g54[] PROGMEM = "G54 - coordinate system 1"; -static const char msg_g55[] PROGMEM = "G55 - coordinate system 2"; -static const char msg_g56[] PROGMEM = "G56 - coordinate system 3"; -static const char msg_g57[] PROGMEM = "G57 - coordinate system 4"; -static const char msg_g58[] PROGMEM = "G58 - coordinate system 5"; -static const char msg_g59[] PROGMEM = "G59 - coordinate system 6"; -static const char *const msg_coor[] PROGMEM = {msg_g53, msg_g54, msg_g55, msg_g56, msg_g57, msg_g58, msg_g59}; - -static const char msg_g00[] PROGMEM = "G0 - linear traverse (seek)"; -static const char msg_g01[] PROGMEM = "G1 - linear feed"; -static const char msg_g02[] PROGMEM = "G2 - clockwise arc feed"; -static const char msg_g03[] PROGMEM = "G3 - counter clockwise arc feed"; -static const char msg_g80[] PROGMEM = "G80 - cancel motion mode (none active)"; -static const char *const msg_momo[] PROGMEM = {msg_g00, msg_g01, msg_g02, msg_g03, msg_g80}; - -static const char msg_g17[] PROGMEM = "G17 - XY plane"; -static const char msg_g18[] PROGMEM = "G18 - XZ plane"; -static const char msg_g19[] PROGMEM = "G19 - YZ plane"; -static const char *const msg_plan[] PROGMEM = {msg_g17, msg_g18, msg_g19}; - -static const char msg_g61[] PROGMEM = "G61 - exact path mode"; -static const char msg_g6a[] PROGMEM = "G61.1 - exact stop mode"; -static const char msg_g64[] PROGMEM = "G64 - continuous mode"; -static const char *const msg_path[] PROGMEM = {msg_g61, msg_g6a, msg_g64}; - -static const char msg_g90[] PROGMEM = "G90 - absolute distance mode"; -static const char msg_g91[] PROGMEM = "G91 - incremental distance mode"; -static const char *const msg_dist[] PROGMEM = {msg_g90, msg_g91}; - -static const char msg_g93[] PROGMEM = "G93 - inverse time mode"; -static const char msg_g94[] PROGMEM = "G94 - units-per-minute mode (i.e. feedrate mode)"; -static const char msg_g95[] PROGMEM = "G95 - units-per-revolution mode"; -static const char *const msg_frmo[] PROGMEM = {msg_g93, msg_g94, msg_g95}; - -#else - -#define msg_units 0 -#define msg_unit 0 -#define msg_stat 0 -#define msg_macs 0 -#define msg_cycs 0 -#define msg_mots 0 -#define msg_hold 0 -#define msg_home 0 -#define msg_coor 0 -#define msg_momo 0 -#define msg_plan 0 -#define msg_path 0 -#define msg_dist 0 -#define msg_frmo 0 -#define msg_am 0 - -#endif // __TEXT_MODE - /// return ASCII char for axis given the axis number -char_t cm_get_axis_char(const int8_t axis) { - char_t axis_char[] = "XYZABC"; +char cm_get_axis_char(const int8_t axis) { + char axis_char[] = "XYZABC"; if ((axis < 0) || (axis > AXES)) return ' '; return axis_char[axis]; } -/// return axis number or -1 if NA -static int8_t _get_axis(const index_t index) { - char_t *ptr; - char_t tmp[TOKEN_LEN+1]; - char_t axes[] = {"xyzabc"}; - - strncpy_P(tmp, cfgArray[index].token, TOKEN_LEN); // kind of a hack. Looks for an axis - if ((ptr = strchr(axes, tmp[0])) == 0) { // character in the 0 and 3 positions - if ((ptr = strchr(axes, tmp[3])) == 0) // to accommodate 'xam' and 'g54x' styles - return -1; - } - - return ptr - axes; -} - - -/// return 0 -f axis is linear, 1 if rotary, -1 if NA -static int8_t _get_axis_type(const index_t index) { - int8_t axis = _get_axis(index); - if (axis >= AXIS_A) return 1; - if (axis == -1) return -1; - return 0; -} - - -/**** Functions called directly from cfgArray table - mostly wrappers **** - * _get_msg_helper() - helper to get string values - * - * cm_get_stat() - get combined machine state as value and string - * cm_get_macs() - get raw machine state as value and string - * cm_get_cycs() - get raw cycle state as value and string - * cm_get_mots() - get raw motion state as value and string - * cm_get_hold() - get raw hold state as value and string - * cm_get_home() - get raw homing state as value and string - * - * cm_get_unit() - get units mode as integer and display string - * cm_get_coor() - get goodinate system - * cm_get_momo() - get runtime motion mode - * cm_get_plan() - get model gcode plane select - * cm_get_path() - get model gcode path control mode - * cm_get_dist() - get model gcode distance mode - * cm_get_frmo() - get model gcode feed rate mode - * cm_get_tool() - get tool - * cm_get_feed() - get feed rate - * cm_get_mline()- get model line number for status reports - * cm_get_line() - get active (model or runtime) line number for status reports - * cm_get_vel() - get runtime velocity - * cm_get_ofs() - get current work offset (runtime) - * cm_get_pos() - get current work position (runtime) - * cm_get_mpos() - get current machine position (runtime) - * - * cm_print_pos()- print work position (with proper units) - * cm_print_mpos()- print machine position (always mm units) - * cm_print_coor()- print coordinate offsets with linear units - * cm_print_corr()- print coordinate offsets with rotary units - */ -stat_t _get_msg_helper(nvObj_t *nv, const char *const msg_array[], uint8_t value) { - nv->value = (float)value; - nv->valuetype = TYPE_INTEGER; - return nv_copy_string(nv, (const char_t *)GET_TEXT_ITEM(msg_array, value)); -} - - -stat_t cm_get_stat(nvObj_t *nv) {return _get_msg_helper(nv, msg_stat, cm_get_combined_state());} -stat_t cm_get_macs(nvObj_t *nv) {return _get_msg_helper(nv, msg_macs, cm_get_machine_state());} -stat_t cm_get_cycs(nvObj_t *nv) {return _get_msg_helper(nv, msg_cycs, cm_get_cycle_state());} -stat_t cm_get_mots(nvObj_t *nv) {return _get_msg_helper(nv, msg_mots, cm_get_motion_state());} -stat_t cm_get_hold(nvObj_t *nv) {return _get_msg_helper(nv, msg_hold, cm_get_hold_state());} -stat_t cm_get_home(nvObj_t *nv) {return _get_msg_helper(nv, msg_home, cm_get_homing_state());} - -stat_t cm_get_unit(nvObj_t *nv) {return _get_msg_helper(nv, msg_unit, cm_get_units_mode(ACTIVE_MODEL));} -stat_t cm_get_coor(nvObj_t *nv) {return _get_msg_helper(nv, msg_coor, cm_get_coord_system(ACTIVE_MODEL));} -stat_t cm_get_momo(nvObj_t *nv) {return _get_msg_helper(nv, msg_momo, cm_get_motion_mode(ACTIVE_MODEL));} -stat_t cm_get_plan(nvObj_t *nv) {return _get_msg_helper(nv, msg_plan, cm_get_select_plane(ACTIVE_MODEL));} -stat_t cm_get_path(nvObj_t *nv) {return _get_msg_helper(nv, msg_path, cm_get_path_control(ACTIVE_MODEL));} -stat_t cm_get_dist(nvObj_t *nv) {return _get_msg_helper(nv, msg_dist, cm_get_distance_mode(ACTIVE_MODEL));} -stat_t cm_get_frmo(nvObj_t *nv) {return _get_msg_helper(nv, msg_frmo, cm_get_feed_rate_mode(ACTIVE_MODEL));} - - -stat_t cm_get_toolv(nvObj_t *nv) { - nv->value = (float)cm_get_tool(ACTIVE_MODEL); - nv->valuetype = TYPE_INTEGER; - return STAT_OK; -} - - -stat_t cm_get_mline(nvObj_t *nv) { - nv->value = (float)cm_get_linenum(MODEL); - nv->valuetype = TYPE_INTEGER; - return STAT_OK; -} - - -stat_t cm_get_line(nvObj_t *nv) { - nv->value = (float)cm_get_linenum(ACTIVE_MODEL); - nv->valuetype = TYPE_INTEGER; - return STAT_OK; -} - - -stat_t cm_get_vel(nvObj_t *nv) { - if (cm_get_motion_state() == MOTION_STOP) nv->value = 0; - else { - nv->value = mp_get_runtime_velocity(); - if (cm_get_units_mode(RUNTIME) == INCHES) nv->value *= INCHES_PER_MM; - } - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return STAT_OK; -} - - -stat_t cm_get_feed(nvObj_t *nv) { - nv->value = cm_get_feed_rate(ACTIVE_MODEL); - if (cm_get_units_mode(ACTIVE_MODEL) == INCHES) nv->value *= INCHES_PER_MM; - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return STAT_OK; -} - - -stat_t cm_get_pos(nvObj_t *nv) { - nv->value = cm_get_work_position(ACTIVE_MODEL, _get_axis(nv->index)); - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return STAT_OK; -} - - -stat_t cm_get_mpo(nvObj_t *nv) { - nv->value = cm_get_absolute_position(ACTIVE_MODEL, _get_axis(nv->index)); - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return STAT_OK; -} - - -stat_t cm_get_ofs(nvObj_t *nv) { - nv->value = cm_get_work_offset(ACTIVE_MODEL, _get_axis(nv->index)); - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return STAT_OK; -} - - -/* - * AXIS GET AND SET FUNCTIONS - * - * cm_get_am() - get axis mode w/enumeration string - * cm_set_am() - set axis mode w/exception handling for axis type - * cm_set_sw() - run this any time you change a switch setting - */ -stat_t cm_get_am(nvObj_t *nv) { - get_ui8(nv); - return _get_msg_helper(nv, msg_am, nv->value); -} - - -stat_t cm_set_am(nvObj_t *nv) { // axis mode - if (_get_axis_type(nv->index) == 0) { // linear - if (nv->value > AXIS_MODE_MAX_LINEAR) return STAT_INPUT_EXCEEDS_MAX_VALUE; - } else if (nv->value > AXIS_MODE_MAX_ROTARY) return STAT_INPUT_EXCEEDS_MAX_VALUE; - - set_ui8(nv); - return STAT_OK; -} - /**** Jerk functions * cm_get_axis_jerk() - returns jerk for an axis * cm_set_axis_jerk() - sets the jerk for an axis, including recirpcal and cached values @@ -1605,254 +1412,7 @@ void cm_set_axis_jerk(uint8_t axis, float jerk) { } -stat_t cm_set_xjm(nvObj_t *nv) { - if (nv->value > JERK_MULTIPLIER) nv->value /= JERK_MULTIPLIER; - set_flu(nv); - cm_set_axis_jerk(_get_axis(nv->index), nv->value); - return STAT_OK; -} - - -stat_t cm_set_xjh(nvObj_t *nv) { - if (nv->value > JERK_MULTIPLIER) nv->value /= JERK_MULTIPLIER; - set_flu(nv); - return STAT_OK; -} - -/* - * Commands - * - * cm_run_qf() - flush planner queue - * cm_run_home() - run homing sequence - */ -stat_t cm_run_qf(nvObj_t *nv) { - cm_request_queue_flush(); - return STAT_OK; -} - - -stat_t cm_run_home(nvObj_t *nv) { - if (fp_TRUE(nv->value)) {cm_homing_cycle_start();} - return STAT_OK; -} - - -/* - * Debugging Commands - * - * cm_dam() - dump active model - */ -stat_t cm_dam(nvObj_t *nv) { - printf("Active model:\n"); - cm_print_vel(nv); - cm_print_feed(nv); - cm_print_line(nv); - cm_print_stat(nv); - cm_print_macs(nv); - cm_print_cycs(nv); - cm_print_mots(nv); - cm_print_hold(nv); - cm_print_home(nv); - cm_print_unit(nv); - cm_print_coor(nv); - cm_print_momo(nv); - cm_print_plan(nv); - cm_print_path(nv); - cm_print_dist(nv); - cm_print_frmo(nv); - cm_print_tool(nv); - - return STAT_OK; -} - - -// AXIS JOGGING +// Axis jogging float cm_get_jogging_dest() { return cm.jogging_dest; } - - -stat_t cm_run_jogx(nvObj_t *nv) { - set_flt(nv); - cm_jogging_cycle_start(AXIS_X); - return STAT_OK; -} - - -stat_t cm_run_jogy(nvObj_t *nv) { - set_flt(nv); - cm_jogging_cycle_start(AXIS_Y); - return STAT_OK; -} - - -stat_t cm_run_jogz(nvObj_t *nv) { - set_flt(nv); - cm_jogging_cycle_start(AXIS_Z); - return STAT_OK; -} - - -stat_t cm_run_joga(nvObj_t *nv) { - set_flt(nv); - cm_jogging_cycle_start(AXIS_A); - return STAT_OK; -} - -#ifdef __TEXT_MODE - -// model state print functions -const char fmt_vel[] PROGMEM = "Velocity:%17.3f%s/min\n"; -const char fmt_feed[] PROGMEM = "Feed rate:%16.3f%s/min\n"; -const char fmt_line[] PROGMEM = "Line number:%10.0f\n"; -const char fmt_stat[] PROGMEM = "Machine state: %s\n"; // combined machine state -const char fmt_macs[] PROGMEM = "Raw machine state: %s\n"; // raw machine state -const char fmt_cycs[] PROGMEM = "Cycle state: %s\n"; -const char fmt_mots[] PROGMEM = "Motion state: %s\n"; -const char fmt_hold[] PROGMEM = "Feedhold state: %s\n"; -const char fmt_home[] PROGMEM = "Homing state: %s\n"; -const char fmt_unit[] PROGMEM = "Units: %s\n"; // units mode as ASCII string -const char fmt_coor[] PROGMEM = "Coordinate system: %s\n"; -const char fmt_momo[] PROGMEM = "Motion mode: %s\n"; -const char fmt_plan[] PROGMEM = "Plane: %s\n"; -const char fmt_path[] PROGMEM = "Path Mode: %s\n"; -const char fmt_dist[] PROGMEM = "Distance mode: %s\n"; -const char fmt_frmo[] PROGMEM = "Feed rate mode: %s\n"; -const char fmt_tool[] PROGMEM = "Tool number %d\n"; - -const char fmt_pos[] PROGMEM = "%c position:%15.3f%s\n"; -const char fmt_mpo[] PROGMEM = "%c machine posn:%11.3f%s\n"; -const char fmt_ofs[] PROGMEM = "%c work offset:%12.3f%s\n"; -const char fmt_hom[] PROGMEM = "%c axis homing state:%2.0f\n"; - -const char fmt_gpl[] PROGMEM = "[gpl] default gcode plane%10d [0=G17,1=G18,2=G19]\n"; -const char fmt_gun[] PROGMEM = "[gun] default gcode units mode%5d [0=G20,1=G21]\n"; -const char fmt_gco[] PROGMEM = "[gco] default gcode coord system%3d [1-6 (G54-G59)]\n"; -const char fmt_gpa[] PROGMEM = "[gpa] default gcode path control%3d [0=G61,1=G61.1,2=G64]\n"; -const char fmt_gdi[] PROGMEM = "[gdi] default gcode distance mode%2d [0=G90,1=G91]\n"; - -void cm_print_vel(nvObj_t *nv) {text_print_flt_units(nv, fmt_vel, GET_UNITS(ACTIVE_MODEL));} -void cm_print_feed(nvObj_t *nv) {text_print_flt_units(nv, fmt_feed, GET_UNITS(ACTIVE_MODEL));} -void cm_print_line(nvObj_t *nv) {text_print_int(nv, fmt_line);} -void cm_print_stat(nvObj_t *nv) {text_print_str(nv, fmt_stat);} -void cm_print_macs(nvObj_t *nv) {text_print_str(nv, fmt_macs);} -void cm_print_cycs(nvObj_t *nv) {text_print_str(nv, fmt_cycs);} -void cm_print_mots(nvObj_t *nv) {text_print_str(nv, fmt_mots);} -void cm_print_hold(nvObj_t *nv) {text_print_str(nv, fmt_hold);} -void cm_print_home(nvObj_t *nv) {text_print_str(nv, fmt_home);} -void cm_print_unit(nvObj_t *nv) {text_print_str(nv, fmt_unit);} -void cm_print_coor(nvObj_t *nv) {text_print_str(nv, fmt_coor);} -void cm_print_momo(nvObj_t *nv) {text_print_str(nv, fmt_momo);} -void cm_print_plan(nvObj_t *nv) {text_print_str(nv, fmt_plan);} -void cm_print_path(nvObj_t *nv) {text_print_str(nv, fmt_path);} -void cm_print_dist(nvObj_t *nv) {text_print_str(nv, fmt_dist);} -void cm_print_frmo(nvObj_t *nv) {text_print_str(nv, fmt_frmo);} -void cm_print_tool(nvObj_t *nv) {text_print_int(nv, fmt_tool);} - -void cm_print_gpl(nvObj_t *nv) {text_print_int(nv, fmt_gpl);} -void cm_print_gun(nvObj_t *nv) {text_print_int(nv, fmt_gun);} -void cm_print_gco(nvObj_t *nv) {text_print_int(nv, fmt_gco);} -void cm_print_gpa(nvObj_t *nv) {text_print_int(nv, fmt_gpa);} -void cm_print_gdi(nvObj_t *nv) {text_print_int(nv, fmt_gdi);} - -// system state print functions -const char fmt_ja[] PROGMEM = "[ja] junction acceleration%8.0f%s\n"; -const char fmt_ct[] PROGMEM = "[ct] chordal tolerance%17.4f%s\n"; -const char fmt_sl[] PROGMEM = "[sl] soft limit enable%12d\n"; -const char fmt_ml[] PROGMEM = "[ml] min line segment%17.3f%s\n"; -const char fmt_ma[] PROGMEM = "[ma] min arc segment%18.3f%s\n"; -const char fmt_ms[] PROGMEM = "[ms] min segment time%13.0f uSec\n"; - -void cm_print_ja(nvObj_t *nv) {text_print_flt_units(nv, fmt_ja, GET_UNITS(ACTIVE_MODEL));} -void cm_print_ct(nvObj_t *nv) {text_print_flt_units(nv, fmt_ct, GET_UNITS(ACTIVE_MODEL));} -void cm_print_sl(nvObj_t *nv) {text_print_ui8(nv, fmt_sl);} -void cm_print_ml(nvObj_t *nv) {text_print_flt_units(nv, fmt_ml, GET_UNITS(ACTIVE_MODEL));} -void cm_print_ma(nvObj_t *nv) {text_print_flt_units(nv, fmt_ma, GET_UNITS(ACTIVE_MODEL));} -void cm_print_ms(nvObj_t *nv) {text_print_flt_units(nv, fmt_ms, GET_UNITS(ACTIVE_MODEL));} - -// axis print functions -static const char fmt_Xam[] PROGMEM = "[%s%s] %s axis mode%18d %s\n"; -static const char fmt_Xfr[] PROGMEM = "[%s%s] %s feedrate maximum%11.0f%s/min\n"; -static const char fmt_Xvm[] PROGMEM = "[%s%s] %s velocity maximum%11.0f%s/min\n"; -static const char fmt_Xtm[] PROGMEM = "[%s%s] %s travel maximum%17.3f%s\n"; -static const char fmt_Xtn[] PROGMEM = "[%s%s] %s travel minimum%17.3f%s\n"; -static const char fmt_Xjm[] PROGMEM = "[%s%s] %s jerk maximum%15.0f%s/min^3 * 1 million\n"; -static const char fmt_Xjh[] PROGMEM = "[%s%s] %s jerk homing%16.0f%s/min^3 * 1 million\n"; -static const char fmt_Xjd[] PROGMEM = "[%s%s] %s junction deviation%14.4f%s (larger is faster)\n"; -static const char fmt_Xra[] PROGMEM = "[%s%s] %s radius value%20.4f%s\n"; -static const char fmt_Xsn[] PROGMEM = "[%s%s] %s switch min%17d [0=off,1=homing,2=limit,3=limit+homing]\n"; -static const char fmt_Xsx[] PROGMEM = "[%s%s] %s switch max%17d [0=off,1=homing,2=limit,3=limit+homing]\n"; -static const char fmt_Xsv[] PROGMEM = "[%s%s] %s search velocity%12.0f%s/min\n"; -static const char fmt_Xlv[] PROGMEM = "[%s%s] %s latch velocity%13.0f%s/min\n"; -static const char fmt_Xlb[] PROGMEM = "[%s%s] %s latch backoff%18.3f%s\n"; -static const char fmt_Xzb[] PROGMEM = "[%s%s] %s zero backoff%19.3f%s\n"; -static const char fmt_cofs[] PROGMEM = "[%s%s] %s %s offset%20.3f%s\n"; -static const char fmt_cpos[] PROGMEM = "[%s%s] %s %s position%18.3f%s\n"; - - -/// helper to print an integer value with no units -static void _print_axis_ui8(nvObj_t *nv, const char *format) { - fprintf_P(stderr, format, nv->group, nv->token, nv->group, (uint8_t)nv->value); -} - - -/// helper to print a floating point linear value in prevailing units -static void _print_axis_flt(nvObj_t *nv, const char *format) { - char *units; - if (_get_axis_type(nv->index) == 0) // linear - units = (char *)GET_UNITS(MODEL); - else units = (char *)GET_TEXT_ITEM(msg_units, DEGREE_INDEX); - - fprintf_P(stderr, format, nv->group, nv->token, nv->group, nv->value, units); -} - - -static void _print_axis_coord_flt(nvObj_t *nv, const char *format) { - char *units; - if (_get_axis_type(nv->index) == 0) // linear - units = (char *)GET_UNITS(MODEL); - else units = (char *)GET_TEXT_ITEM(msg_units, DEGREE_INDEX); - - fprintf_P(stderr, format, nv->group, nv->token, nv->group, nv->token, nv->value, units); -} - - -/// print position with unit displays for MM or Inches -static void _print_pos(nvObj_t *nv, const char *format, uint8_t units) { - char axes[] = {"XYZABC"}; - uint8_t axis = _get_axis(nv->index); - if (axis >= AXIS_A) {units = DEGREES;} - fprintf_P(stderr, format, axes[axis], nv->value, GET_TEXT_ITEM(msg_units, units)); -} - - -void cm_print_am(nvObj_t *nv) { // print axis mode with enumeration string - fprintf_P(stderr, fmt_Xam, nv->group, nv->token, nv->group, (uint8_t)nv->value, - GET_TEXT_ITEM(msg_am, (uint8_t)nv->value)); -} - - -void cm_print_fr(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xfr);} -void cm_print_vm(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xvm);} -void cm_print_tm(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xtm);} -void cm_print_tn(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xtn);} -void cm_print_jm(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xjm);} -void cm_print_jh(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xjh);} -void cm_print_jd(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xjd);} -void cm_print_ra(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xra);} -void cm_print_sn(nvObj_t *nv) {_print_axis_ui8(nv, fmt_Xsn);} -void cm_print_sx(nvObj_t *nv) {_print_axis_ui8(nv, fmt_Xsx);} -void cm_print_sv(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xsv);} -void cm_print_lv(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xlv);} -void cm_print_lb(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xlb);} -void cm_print_zb(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xzb);} - -void cm_print_cofs(nvObj_t *nv) {_print_axis_coord_flt(nv, fmt_cofs);} -void cm_print_cpos(nvObj_t *nv) {_print_axis_coord_flt(nv, fmt_cpos);} - -void cm_print_pos(nvObj_t *nv) {_print_pos(nv, fmt_pos, cm_get_units_mode(MODEL));} -/// print position with fixed unit display - always in Degrees or MM -void cm_print_mpo(nvObj_t *nv) {_print_pos(nv, fmt_mpo, MILLIMETERS);} -void cm_print_ofs(nvObj_t *nv) {_print_pos(nv, fmt_ofs, MILLIMETERS);} - -#endif // __TEXT_MODE diff --git a/src/canonical_machine.h b/src/canonical_machine.h index ff076c0..48de721 100644 --- a/src/canonical_machine.h +++ b/src/canonical_machine.h @@ -28,10 +28,13 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#ifndef CANONICAL_MACHINE_H_ONCE -#define CANONICAL_MACHINE_H_ONCE +#ifndef CANONICAL_MACHINE_H +#define CANONICAL_MACHINE_H #include "config.h" +#include "status.h" + +#include #define MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model #define PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to @@ -105,7 +108,6 @@ typedef struct GCodeState { // Gcode model state - used by model, plan typedef struct GCodeStateExtended { // Gcode dynamic state extensions - used by model and arcs - uint16_t magic_start; // magic number to test memory integrity uint8_t next_action; // handles G modal group 1 moves & non-modals uint8_t program_flow; // used only by the gcode_parser @@ -129,8 +131,6 @@ typedef struct GCodeStateExtended { // Gcode dynamic state extensions - used b // unimplemented gcode parameters // float cutter_radius; // D - cutter radius compensation (0 is off) // float cutter_length; // H - cutter length compensation (0 is off) - - uint16_t magic_end; } GCodeStateX_t; @@ -201,8 +201,6 @@ typedef struct cmAxis { typedef struct cmSingleton { // struct to manage cm globals and cycles - magic_t magic_start; // magic number to test memory integrity - // Public // system group settings float junction_acceleration; // centripetal acceleration max for cornering @@ -253,8 +251,6 @@ typedef struct cmSingleton { // struct to manage cm globals and cycles GCodeStateX_t gmx; // extended gcode model state GCodeInput_t gn; // gcode input values - transient GCodeInput_t gf; // gcode input flags - transient - - magic_t magic_end; } cmSingleton_t; @@ -511,7 +507,7 @@ enum cmAxisMode { // axis modes (ordered: see _cm_get_feed_time( AXIS_STANDARD, // axis in coordinated motion w/standard behaviors AXIS_INHIBITED, // axis is computed but not activated AXIS_RADIUS // rotary axis calibrated to circumference -}; // ordering must be preserved. See cm_set_move_times() +}; // ordering must be preserved. #define AXIS_MODE_MAX_LINEAR AXIS_INHIBITED #define AXIS_MODE_MAX_ROTARY AXIS_RADIUS @@ -568,12 +564,9 @@ stat_t cm_test_soft_limits(float target[]); // Initialization and termination (4.3.2) void canonical_machine_init(); -void canonical_machine_init_assertions(); -stat_t canonical_machine_test_assertions(); - stat_t cm_hard_alarm(stat_t status); // enter hard alarm state. returns same status code stat_t cm_soft_alarm(stat_t status); // enter soft alarm state. returns same status code -stat_t cm_clear(nvObj_t *nv); +stat_t cm_clear(); // Representation (4.3.3) stat_t cm_select_plane(uint8_t plane); // G17, G18, G19 @@ -629,7 +622,7 @@ stat_t cm_traverse_override_factor(uint8_t flag); // M50.3 stat_t cm_spindle_override_enable(uint8_t flag); // M51 stat_t cm_spindle_override_factor(uint8_t flag); // M51.1 -void cm_message(char_t *message); // msg to console (e.g. Gcode comments) +void cm_message(char *message); // msg to console (e.g. Gcode comments) // Program Functions (4.3.10) void cm_request_feedhold(); @@ -646,6 +639,8 @@ void cm_program_stop(); // M0 void cm_optional_program_stop(); // M1 void cm_program_end(); // M2 +char cm_get_axis_char(const int8_t axis); + /*--- Cycles ---*/ // Homing cycles @@ -662,162 +657,4 @@ stat_t cm_jogging_callback(); // jogging cycl stat_t cm_jogging_cycle_start(uint8_t axis); // {"jogx":-100.3} float cm_get_jogging_dest(); -/*--- cfgArray interface functions ---*/ - -char_t cm_get_axis_char(const int8_t axis); - -stat_t cm_get_mline(nvObj_t *nv); // get model line number -stat_t cm_get_line(nvObj_t *nv); // get active (model or runtime) line number -stat_t cm_get_stat(nvObj_t *nv); // get combined machine state as value and string -stat_t cm_get_macs(nvObj_t *nv); // get raw machine state as value and string -stat_t cm_get_cycs(nvObj_t *nv); // get raw cycle state (etc etc)... -stat_t cm_get_mots(nvObj_t *nv); // get raw motion state... -stat_t cm_get_hold(nvObj_t *nv); // get raw hold state... -stat_t cm_get_home(nvObj_t *nv); // get raw homing state... -stat_t cm_get_unit(nvObj_t *nv); // get unit mode... -stat_t cm_get_coor(nvObj_t *nv); // get coordinate system in effect... -stat_t cm_get_momo(nvObj_t *nv); // get motion mode... -stat_t cm_get_plan(nvObj_t *nv); // get active plane... -stat_t cm_get_path(nvObj_t *nv); // get patch control mode... -stat_t cm_get_dist(nvObj_t *nv); // get distance mode... -stat_t cm_get_frmo(nvObj_t *nv); // get feedrate mode... -stat_t cm_get_toolv(nvObj_t *nv); // get tool (value) -stat_t cm_get_pwr(nvObj_t *nv); // get motor power enable state - -stat_t cm_get_vel(nvObj_t *nv); // get runtime velocity... -stat_t cm_get_feed(nvObj_t *nv); -stat_t cm_get_pos(nvObj_t *nv); // get runtime work position... -stat_t cm_get_mpo(nvObj_t *nv); // get runtime machine position... -stat_t cm_get_ofs(nvObj_t *nv); // get runtime work offset... - -stat_t cm_run_qf(nvObj_t *nv); // run queue flush -stat_t cm_run_home(nvObj_t *nv); // start homing cycle - -stat_t cm_dam(nvObj_t *nv); // dump active model (debugging command) - -stat_t cm_run_jogx(nvObj_t *nv); // start jogging cycle for x -stat_t cm_run_jogy(nvObj_t *nv); // start jogging cycle for y -stat_t cm_run_jogz(nvObj_t *nv); // start jogging cycle for z -stat_t cm_run_joga(nvObj_t *nv); // start jogging cycle for a - -stat_t cm_get_am(nvObj_t *nv); // get axis mode -stat_t cm_set_am(nvObj_t *nv); // set axis mode -stat_t cm_set_xjm(nvObj_t *nv); // set jerk max with 1,000,000 correction -stat_t cm_set_xjh(nvObj_t *nv); // set jerk homing with 1,000,000 correction - -#ifdef __TEXT_MODE - -void cm_print_vel(nvObj_t *nv); // model state reporting -void cm_print_feed(nvObj_t *nv); -void cm_print_line(nvObj_t *nv); -void cm_print_stat(nvObj_t *nv); -void cm_print_macs(nvObj_t *nv); -void cm_print_cycs(nvObj_t *nv); -void cm_print_mots(nvObj_t *nv); -void cm_print_hold(nvObj_t *nv); -void cm_print_home(nvObj_t *nv); -void cm_print_unit(nvObj_t *nv); -void cm_print_coor(nvObj_t *nv); -void cm_print_momo(nvObj_t *nv); -void cm_print_plan(nvObj_t *nv); -void cm_print_path(nvObj_t *nv); -void cm_print_dist(nvObj_t *nv); -void cm_print_frmo(nvObj_t *nv); -void cm_print_tool(nvObj_t *nv); - -void cm_print_gpl(nvObj_t *nv); // Gcode defaults -void cm_print_gun(nvObj_t *nv); -void cm_print_gco(nvObj_t *nv); -void cm_print_gpa(nvObj_t *nv); -void cm_print_gdi(nvObj_t *nv); - -void cm_print_lin(nvObj_t *nv); // generic print for linear values -void cm_print_pos(nvObj_t *nv); // print runtime work position in prevailing units -void cm_print_mpo(nvObj_t *nv); // print runtime work position always in MM units -void cm_print_ofs(nvObj_t *nv); // print runtime work offset always in MM units - -void cm_print_ja(nvObj_t *nv); // global CM settings -void cm_print_ct(nvObj_t *nv); -void cm_print_sl(nvObj_t *nv); -void cm_print_ml(nvObj_t *nv); -void cm_print_ma(nvObj_t *nv); -void cm_print_ms(nvObj_t *nv); -void cm_print_st(nvObj_t *nv); - -void cm_print_am(nvObj_t *nv); // axis print functions -void cm_print_fr(nvObj_t *nv); -void cm_print_vm(nvObj_t *nv); -void cm_print_tm(nvObj_t *nv); -void cm_print_tn(nvObj_t *nv); -void cm_print_jm(nvObj_t *nv); -void cm_print_jh(nvObj_t *nv); -void cm_print_jd(nvObj_t *nv); -void cm_print_ra(nvObj_t *nv); -void cm_print_sn(nvObj_t *nv); -void cm_print_sx(nvObj_t *nv); -void cm_print_sv(nvObj_t *nv); -void cm_print_lv(nvObj_t *nv); -void cm_print_lb(nvObj_t *nv); -void cm_print_zb(nvObj_t *nv); -void cm_print_cofs(nvObj_t *nv); -void cm_print_cpos(nvObj_t *nv); - -#else // __TEXT_MODE - -#define cm_print_vel tx_print_stub // model state reporting -#define cm_print_feed tx_print_stub -#define cm_print_line tx_print_stub -#define cm_print_stat tx_print_stub -#define cm_print_macs tx_print_stub -#define cm_print_cycs tx_print_stub -#define cm_print_mots tx_print_stub -#define cm_print_hold tx_print_stub -#define cm_print_home tx_print_stub -#define cm_print_unit tx_print_stub -#define cm_print_coor tx_print_stub -#define cm_print_momo tx_print_stub -#define cm_print_plan tx_print_stub -#define cm_print_path tx_print_stub -#define cm_print_dist tx_print_stub -#define cm_print_frmo tx_print_stub -#define cm_print_tool tx_print_stub - -#define cm_print_gpl tx_print_stub // Gcode defaults -#define cm_print_gun tx_print_stub -#define cm_print_gco tx_print_stub -#define cm_print_gpa tx_print_stub -#define cm_print_gdi tx_print_stub - -#define cm_print_lin tx_print_stub // generic print for linear values -#define cm_print_pos tx_print_stub // print runtime work position in prevailing units -#define cm_print_mpo tx_print_stub // print runtime work position always in MM uints -#define cm_print_ofs tx_print_stub // print runtime work offset always in MM uints - -#define cm_print_ja tx_print_stub // global CM settings -#define cm_print_ct tx_print_stub -#define cm_print_sl tx_print_stub -#define cm_print_ml tx_print_stub -#define cm_print_ma tx_print_stub -#define cm_print_ms tx_print_stub -#define cm_print_st tx_print_stub - -#define cm_print_am tx_print_stub // axis print functions -#define cm_print_fr tx_print_stub -#define cm_print_vm tx_print_stub -#define cm_print_tm tx_print_stub -#define cm_print_tn tx_print_stub -#define cm_print_jm tx_print_stub -#define cm_print_jh tx_print_stub -#define cm_print_jd tx_print_stub -#define cm_print_ra tx_print_stub -#define cm_print_sn tx_print_stub -#define cm_print_sx tx_print_stub -#define cm_print_sv tx_print_stub -#define cm_print_lv tx_print_stub -#define cm_print_lb tx_print_stub -#define cm_print_zb tx_print_stub -#define cm_print_cofs tx_print_stub -#define cm_print_cpos tx_print_stub - -#endif // __TEXT_MODE -#endif // CANONICAL_MACHINE_H_ONCE +#endif // CANONICAL_MACHINE_H diff --git a/src/clock.c b/src/clock.c new file mode 100644 index 0000000..5d8886d --- /dev/null +++ b/src/clock.c @@ -0,0 +1,161 @@ +/* + * Part of TinyG project + * + * Copyright (c) 2010 - 2013 Alden S. Hart Jr. + * + * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "clock.h" + +#include "hardware.h" + +#include +#include + +void CCPWrite(volatile uint8_t * address, uint8_t value); + +/* + * This routine is lifted and modified from Boston Android and from + * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=711659 +*/ + +void clock_init() { +#ifdef __CLOCK_EXTERNAL_8MHZ // external 8 Mhx Xtal with 4x PLL = 32 Mhz + OSC.XOSCCTRL = 0x4B; // 2-9 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup + OSC.CTRL = 0x08; // enable external crystal oscillator + while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready + OSC.PLLCTRL = 0xC4; // XOSC is PLL Source; 4x Factor (32 MHz sys clock) + OSC.CTRL = 0x18; // Enable PLL & External Oscillator + while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready + CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc); // switch to PLL clock + OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock +#endif + +#ifdef __CLOCK_EXTERNAL_16MHZ // external 16 Mhx Xtal with 2x PLL = 32 Mhz + OSC.XOSCCTRL = 0xCB; // 12-16 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup + OSC.CTRL = 0x08; // enable external crystal oscillator + while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready + OSC.PLLCTRL = 0xC2; // XOSC is PLL Source; 2x Factor (32 MHz sys clock) + OSC.CTRL = 0x18; // Enable PLL & External Oscillator + while(!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready + CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc); // switch to PLL clock + OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock +#endif + +#ifdef __CLOCK_INTERNAL_32MHZ // 32 MHz internal clock (Boston Android code) + CCP = CCP_IOREG_gc; // Security Signature to modify clk + OSC.CTRL = OSC_RC32MEN_bm; // enable internal 32MHz oscillator + while (!(OSC.STATUS & OSC_RC32MRDY_bm)); // wait for oscillator ready + CCP = CCP_IOREG_gc; // Security Signature to modify clk + CLK.CTRL = 0x01; // select sysclock 32MHz osc +#endif +} + +/****************************************************************************** + * The following code was excerpted from the Atmel AVR1003 clock driver example + * and carries its copyright: + * + * $Revision: 2771 $ + * $Date: 2009-09-11 11:54:26 +0200 (fr, 11 sep 2009) $ \n + * + * Copyright (c) 2008, Atmel Corporation All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of ATMEL may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND + * SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR FART + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SHOE DAMAGE. + *****************************************************************************/ + +/* Macros to protect the code from interrupts */ + +#define AVR_ENTER_CRITICAL_REGION() uint8_t volatile saved_sreg = SREG; cli(); +#define AVR_LEAVE_CRITICAL_REGION() SREG = saved_sreg; + + +/* CCP write helper function written in assembly. + * + * This function is written in assembly because of the time critial + * operation of writing to the registers. + * + * - address A pointer to the address to write to. + * - value The value to put in to the register. + */ +void CCPWrite( volatile uint8_t * address, uint8_t value ) { +#ifdef __ICCAVR__ + + // Store global interrupt setting in scratch register and disable interrupts. + asm("in R1, 0x3F \n" + "cli" + ); + + // Move destination address pointer to Z pointer registers. + asm("movw r30, r16"); +#ifdef RAMPZ + asm("ldi R16, 0 \n" + "out 0x3B, R16" + ); + +#endif + asm("ldi r16, 0xD8 \n" + "out 0x34, r16 \n" +#if (__MEMORY_MODEL__ == 1) + "st Z, r17 \n"); +#elif (__MEMORY_MODEL__ == 2) + "st Z, r18 \n"); +#else /* (__MEMORY_MODEL__ == 3) || (__MEMORY_MODEL__ == 5) */ +"st Z, r19 \n"); +#endif /* __MEMORY_MODEL__ */ + +// Restore global interrupt setting from scratch register. +asm("out 0x3F, R1"); + +#elif defined __GNUC__ +AVR_ENTER_CRITICAL_REGION(); +volatile uint8_t * tmpAddr = address; +#ifdef RAMPZ +RAMPZ = 0; +#endif +asm volatile( + "movw r30, %0" "\n\t" + "ldi r16, %2" "\n\t" + "out %3, r16" "\n\t" + "st Z, %1" "\n\t" + : + : "r" (tmpAddr), "r" (value), "M" (CCP_IOREG_gc), "i" (&CCP) + : "r16", "r30", "r31" + ); + +AVR_LEAVE_CRITICAL_REGION(); +#endif +} + diff --git a/src/xmega/xmega_init.h b/src/clock.h similarity index 80% rename from src/xmega/xmega_init.h rename to src/clock.h index 6588405..4c377b7 100644 --- a/src/xmega/xmega_init.h +++ b/src/clock.h @@ -1,5 +1,4 @@ /* - * xmega_init.h - general init and support functions for xmega family * Part of TinyG project * * Copyright (c) 2010 - 2013 Alden S. Hart Jr. @@ -17,14 +16,12 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#ifndef xmega_support_h -#define xmega_support_h +#ifndef CLOCK_H +#define CLOCK_H -/* - * Global Scope Functions - */ +#include -void xmega_init(); -void CCPWrite( volatile uint8_t * address, uint8_t value ); +void clock_init(); +void CCPWrite(volatile uint8_t *address, uint8_t value); -#endif +#endif // CLOCK_H diff --git a/src/command.c b/src/command.c new file mode 100644 index 0000000..f0903fb --- /dev/null +++ b/src/command.c @@ -0,0 +1,194 @@ +/******************************************************************************\ + + This file is part of the TinyG firmware. + + Copyright (c) 2016, Buildbotics LLC + All rights reserved. + + The C! library is free software: you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation, either version 2.1 of + the License, or (at your option) any later version. + + The C! library 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 C! library. If not, see + . + + In addition, BSD licensing may be granted on a case by case basis + by written permission from at least one of the copyright holders. + You may request written permission by emailing the authors. + + For information regarding this software email: + Joseph Coffland + joseph@buildbotics.com + +\******************************************************************************/ + +#include "command.h" + +#include "gcode_parser.h" +#include "usart.h" +#include "hardware.h" +#include "report.h" +#include "vars.h" +#include "config.h" + +#include + +#include +#include + + +static char input[INPUT_BUFFER_LEN]; + + +// Command forward declarations +// (Don't be afraid, X-Macros rock!) +#define CMD(name, func, minArgs, maxArgs, help) \ + uint8_t func(int, char *[]); + +#include "command.def" +#undef CMD + +// Command names & help +#define CMD(name, func, minArgs, maxArgs, help) \ + static const char pstr_##name[] PROGMEM = #name; \ + static const char name##_help[] PROGMEM = help; + +#include "command.def" +#undef CMD + +// Command table +#define CMD(name, func, minArgs, maxArgs, help) \ + {pstr_##name, func, minArgs, maxArgs, name##_help}, + +static const command_t commands[] PROGMEM = { +#include "command.def" +#undef CMD + {}, // Sentinel +}; + + +stat_t command_dispatch() { + // Read input line or return if incomplete, usart_gets() is non-blocking + ritorno(usart_gets(input, sizeof(input))); + + return command_eval(input); +} + + +int command_find(const char *match) { + for (int i = 0; ; i++) { + const char *name = pgm_read_word(&commands[i].name); + if (!name) break; + + if (strcmp_P(match, name) == 0) return i; + } + + return -1; +} + + +int command_exec(int argc, char *argv[]) { + stat_t status = STAT_INVALID_OR_MALFORMED_COMMAND; + + int i = command_find(argv[0]); + if (i != -1) { + putchar('\n'); + + uint8_t minArgs = pgm_read_byte(&commands[i].minArgs); + uint8_t maxArgs = pgm_read_byte(&commands[i].maxArgs); + + if (argc <= minArgs) printf_P(PSTR("Too few arguments\n")); + else if (maxArgs < argc - 1) printf_P(PSTR("Too many arguments\n")); + else { + command_cb_t cb = pgm_read_word(&commands[i].cb); + status = cb(argc, argv); + } + + } else printf_P(PSTR("Unknown command '%s'\n"), argv[0]); + + return status; +} + + +int command_eval(char *cmd) { + while (*cmd && isspace(*cmd)) cmd++; + + if (!*cmd) { + report_request_full(); + return STAT_OK; + } + + if (*cmd == '{') return vars_parser(cmd); + if (*cmd != '$') return gc_gcode_parser(cmd); + + // Parse line + char *p = cmd + 1; + int argc = 0; + char *argv[MAX_ARGS] = {}; + + while (argc < MAX_ARGS && *p) { + // Skip space + while (*p && isspace(*p)) *p++ = 0; + + // Start of token + if (*p) argv[argc++] = p; + + // Find end + while (*p && !isspace(*p)) p++; + } + + // Exec command + if (argc) return command_exec(argc, argv); + + return STAT_OK; +} + + +// Command functions +void static print_command_help(int i) { + static const char fmt[] PROGMEM = " %-8S %S\n"; + const char *name = pgm_read_word(&commands[i].name); + const char *help = pgm_read_word(&commands[i].help); + + printf_P(fmt, name, help); +} + + +uint8_t command_help(int argc, char *argv[]) { + if (argc == 2) { + int i = command_find(argv[0]); + + if (i == -1) { + printf_P(PSTR("Command not found\n")); + return STAT_INVALID_OR_MALFORMED_COMMAND; + + } else print_command_help(i); + + return 0; + } + + puts_P(PSTR("\nCommands:")); + for (int i = 0; ; i++) { + const char *name = pgm_read_word(&commands[i].name); + if (!name) break; + print_command_help(i); + } + + puts_P(PSTR("\nVariables:")); + vars_print_help(); + + return 0; +} + + +uint8_t command_reboot(int argc, char *argv[]) { + hw_request_hard_reset(); + return 0; +} diff --git a/src/command.def b/src/command.def new file mode 100644 index 0000000..f0131f2 --- /dev/null +++ b/src/command.def @@ -0,0 +1,33 @@ +/******************************************************************************\ + + This file is part of the TinyG firmware. + + Copyright (c) 2016, Buildbotics LLC + All rights reserved. + + The C! library is free software: you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation, either version 2.1 of + the License, or (at your option) any later version. + + The C! library 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 C! library. If not, see + . + + In addition, BSD licensing may be granted on a case by case basis + by written permission from at least one of the copyright holders. + You may request written permission by emailing the authors. + + For information regarding this software email: + Joseph Coffland + joseph@buildbotics.com + +\******************************************************************************/ + +CMD(help, command_help, 0, 1, "Print this help screen") +CMD(reboot, command_reboot, 0, 0, "Reboot the controller") diff --git a/src/command.h b/src/command.h new file mode 100644 index 0000000..3b6f42d --- /dev/null +++ b/src/command.h @@ -0,0 +1,55 @@ +/******************************************************************************\ + + This file is part of the TinyG firmware. + + Copyright (c) 2016, Buildbotics LLC + All rights reserved. + + The C! library is free software: you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation, either version 2.1 of + the License, or (at your option) any later version. + + The C! library 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 C! library. If not, see + . + + In addition, BSD licensing may be granted on a case by case basis + by written permission from at least one of the copyright holders. + You may request written permission by emailing the authors. + + For information regarding this software email: + Joseph Coffland + joseph@buildbotics.com + +\******************************************************************************/ + +#ifndef COMMAND_H +#define COMMAND_H + +#include "status.h" + +#define MAX_ARGS 16 + +typedef uint8_t (*command_cb_t)(int argc, char *argv[]); + +typedef struct { + const char *name; + command_cb_t cb; + uint8_t minArgs; + uint8_t maxArgs; + const char *help; +} command_t; + + +stat_t command_dispatch(); +int command_find(const char *name); +int command_exec(int argc, char *argv[]); +int command_eval(char *cmd); + +#endif // COMMAND_H diff --git a/src/config.c b/src/config.c deleted file mode 100644 index 5c65a55..0000000 --- a/src/config.c +++ /dev/null @@ -1,656 +0,0 @@ -/* - * config.c - application independent configuration handling - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -#include "tinyg.h" // #1 -#include "config.h" // #2 -#include "report.h" -#include "controller.h" -#include "canonical_machine.h" -#include "json_parser.h" -#include "text_parser.h" -#include "persistence.h" -#include "hardware.h" -#include "help.h" -#include "util.h" - -static void _set_defa(nvObj_t *nv); - -nvStr_t nvStr; -nvList_t nvl; - -/*********************************************************************************** - **** CODE ************************************************************************* - ***********************************************************************************/ -/* Primary access points to functions bound to text mode / JSON functions - * These gatekeeper functions check index ranges so others don't have to - * - * nv_set() - Write a value or invoke a function - operates on single valued elements or groups - * nv_get() - Build a nvObj with the values from the target & return the value - * Populate nv body with single valued elements or groups (iterates) - * nv_print() - Output a formatted string for the value. - * nv_persist() - persist value to non-volatile storage. Takes special cases into account - * - * !!!! NOTE: nv_persist() cannot be called from an interrupt on the AVR due to the AVR1008 EEPROM workaround - */ -stat_t nv_set(nvObj_t *nv) { - if (nv->index >= nv_index_max()) - return STAT_INTERNAL_RANGE_ERROR; - return ((fptrCmd)GET_TABLE_WORD(set))(nv); -} - - -stat_t nv_get(nvObj_t *nv) { - if (nv->index >= nv_index_max()) return STAT_INTERNAL_RANGE_ERROR; - return ((fptrCmd)GET_TABLE_WORD(get))(nv); -} - - -void nv_print(nvObj_t *nv) { - if (nv->index >= nv_index_max()) return; - ((fptrCmd)GET_TABLE_WORD(print))(nv); -} - - -stat_t nv_persist(nvObj_t *nv) { // nv_persist() cannot be called from an interrupt on the AVR due to the AVR1008 EEPROM workaround - if (nv_index_lt_groups(nv->index) == false) return STAT_INTERNAL_RANGE_ERROR; - if (GET_TABLE_BYTE(flags) & F_PERSIST) return write_persistent_value(nv); - - return STAT_OK; -} - - -/************************************************************************************ - * Called once on hard reset - * - * Performs one of 2 actions: - * (1) if persistence is set up or out-of-rev load RAM and NVM with settings.h defaults - * (2) if persistence is set up and at current config version use NVM data for config - * - * You can assume the cfg struct has been zeroed by a hard reset. - * Do not clear it as the version and build numbers have already been set by tg_init() - * - * NOTE: Config assertions are handled from the controller - */ -void config_init() { - nvObj_t *nv = nv_reset_nv_list(); - config_init_assertions(); - - cm_set_units_mode(MILLIMETERS); // must do inits in millimeter mode - nv->index = 0; // this will read the first record in NVM - - read_persistent_value(nv); - if (nv->value != cs.fw_build) _set_defa(nv); // case (1) NVM is not setup or not in revision - - else { // case (2) NVM is setup and in revision - rpt_print_loading_configs_message(); - - for (nv->index = 0; nv_index_is_single(nv->index); nv->index++) - if (GET_TABLE_BYTE(flags) & F_INITIALIZE) { - strncpy_P(nv->token, cfgArray[nv->index].token, TOKEN_LEN); // read the token from the array - read_persistent_value(nv); - nv_set(nv); - } - - sr_init_status_report(); - } -} - - -/* - * set_defaults() - reset persistence with default values for machine profile - * _set_defa() - helper function and called directly from config_init() - */ -static void _set_defa(nvObj_t *nv) { - cm_set_units_mode(MILLIMETERS); // must do inits in MM mode - for (nv->index = 0; nv_index_is_single(nv->index); nv->index++) - if (GET_TABLE_BYTE(flags) & F_INITIALIZE) { - nv->value = GET_TABLE_FLOAT(def_value); - strncpy_P(nv->token, cfgArray[nv->index].token, TOKEN_LEN); - nv_set(nv); - nv_persist(nv); - } - - sr_init_status_report(); // reset status reports - rpt_print_initializing_message(); // don't start TX until all the NVM persistence is done -} - - -stat_t set_defaults(nvObj_t *nv) { - // failsafe. nv->value must be true or no action occurs - if (fp_FALSE(nv->value)) return help_defa(nv); - _set_defa(nv); - - // The values in nv are now garbage. Mark the nv as $defa so it displays nicely. - nv->valuetype = TYPE_INTEGER; - nv->value = 1; - return STAT_OK; -} - - -/* - * config_init_assertions() - * config_test_assertions() - check memory integrity of config sub-system - */ -void config_init_assertions() { - cfg.magic_start = MAGICNUM; - cfg.magic_end = MAGICNUM; - nvl.magic_start = MAGICNUM; - nvl.magic_end = MAGICNUM; - nvStr.magic_start = MAGICNUM; - nvStr.magic_end = MAGICNUM; -} - - -stat_t config_test_assertions() { - if ((cfg.magic_start != MAGICNUM) || (cfg.magic_end != MAGICNUM)) return STAT_CONFIG_ASSERTION_FAILURE; - if ((nvl.magic_start != MAGICNUM) || (nvl.magic_end != MAGICNUM)) return STAT_CONFIG_ASSERTION_FAILURE; - if ((nvStr.magic_start != MAGICNUM) || (nvStr.magic_end != MAGICNUM)) return STAT_CONFIG_ASSERTION_FAILURE; - if (global_string_buf[MESSAGE_LEN-1] != 0) return STAT_CONFIG_ASSERTION_FAILURE; - return STAT_OK; -} - - -/* Generic gets() - * get_nul() - get nothing (returns STAT_PARAMETER_CANNOT_BE_READ) - * get_ui8() - get value as 8 bit uint8_t - * get_int() - get value as 32 bit integer - * get_data() - get value as 32 bit integer blind cast - * get_flt() - get value as float - * get_format() - internal accessor for printf() format string - */ -stat_t get_nul(nvObj_t *nv) { - nv->valuetype = TYPE_0; - return STAT_PARAMETER_CANNOT_BE_READ; -} - - -stat_t get_ui8(nvObj_t *nv) { - nv->value = (float)*((uint8_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_INTEGER; - return STAT_OK; -} - - -stat_t get_int(nvObj_t *nv) { - nv->value = *((uint32_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_INTEGER; - return STAT_OK; -} - - -stat_t get_data(nvObj_t *nv) { - uint32_t *v = (uint32_t*)&nv->value; - *v = *((uint32_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_DATA; - return STAT_OK; -} - - -stat_t get_flt(nvObj_t *nv) { - nv->value = *((float *)GET_TABLE_WORD(target)); - nv->precision = (int8_t)GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return STAT_OK; -} - - -/* Generic sets() - * set_nul() - set nothing (returns STAT_PARAMETER_IS_READ_ONLY) - * set_ui8() - set value as 8 bit uint8_t value - * set_01() - set a 0 or 1 uint8_t value with validation - * set_012() - set a 0, 1 or 2 uint8_t value with validation - * set_0123() - set a 0, 1, 2 or 3 uint8_t value with validation - * set_int() - set value as 32 bit integer - * set_data() - set value as 32 bit integer blind cast - * set_flt() - set value as float - */ -stat_t set_nul(nvObj_t *nv) { return STAT_PARAMETER_IS_READ_ONLY; } - -stat_t set_ui8(nvObj_t *nv) { - *((uint8_t *)GET_TABLE_WORD(target)) = nv->value; - nv->valuetype = TYPE_INTEGER; - return STAT_OK; -} - - -stat_t set_01(nvObj_t *nv) { - if ((uint8_t)nv->value > 1) return STAT_INPUT_VALUE_RANGE_ERROR; - return set_ui8(nv); -} - - -stat_t set_012(nvObj_t *nv) { - if ((uint8_t)nv->value > 2) return STAT_INPUT_VALUE_RANGE_ERROR; - return set_ui8(nv); -} - - -stat_t set_0123(nvObj_t *nv) { - if ((uint8_t)nv->value > 3) return STAT_INPUT_VALUE_RANGE_ERROR; - return set_ui8(nv); -} - - -stat_t set_int(nvObj_t *nv) { - *((uint32_t *)GET_TABLE_WORD(target)) = (uint32_t)nv->value; - nv->valuetype = TYPE_INTEGER; - return STAT_OK; -} - - -stat_t set_data(nvObj_t *nv) { - uint32_t *v = (uint32_t*)&nv->value; - *((uint32_t *)GET_TABLE_WORD(target)) = *v; - nv->valuetype = TYPE_DATA; - return STAT_OK; -} - - -stat_t set_flt(nvObj_t *nv) { - *((float *)GET_TABLE_WORD(target)) = nv->value; - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return STAT_OK; -} - - -/************************************************************************************ - * Group operations - * - * Group operations work on parent/child groups where the parent is one of: - * axis group x,y,z,a,b,c - * motor group 1,2,3,4 - * PWM group p1 - * coordinate group g54,g55,g56,g57,g58,g59,g92 - * system group "sys" - a collection of otherwise unrelated variables - * - * Text mode can only GET groups. For example: - * $x get all members of an axis group - * $1 get all members of a motor group - * $ get any named group from the above lists - * - * In JSON groups are carried as parent / child objects & can get and set elements: - * {"x":""} get all X axis parameters - * {"x":{"vm":""}} get X axis velocity max - * {"x":{"vm":1000}} set X axis velocity max - * {"x":{"vm":"","fr":""}} get X axis velocity max and feed rate - * {"x":{"vm":1000,"fr";900}} set X axis velocity max and feed rate - * {"x":{"am":1,"fr":800,....}} set multiple or all X axis parameters - */ - -/* - * get_grp() - read data from axis, motor, system or other group - * - * get_grp() is a group expansion function that expands the parent group and returns - * the values of all the children in that group. It expects the first nvObj in the - * nvBody to have a valid group name in the token field. This first object will be set - * to a TYPE_PARENT. The group field is left nul - as the group field refers to a parent - * group, which this group has none. - * - * All subsequent nvObjs in the body will be populated with their values. - * The token field will be populated as will the parent name in the group field. - * - * The sys group is an exception where the children carry a blank group field, even though - * the sys parent is labeled as a TYPE_PARENT. - */ -stat_t get_grp(nvObj_t *nv) { - char_t *parent_group = nv->token; // token in the parent nv object is the group - char_t group[GROUP_LEN+1]; // group string retrieved from cfgArray child - nv->valuetype = TYPE_PARENT; // make first object the parent - - for (index_t i=0; nv_index_is_single(i); i++) { - strcpy_P(group, cfgArray[i].group); // don't need strncpy as it's always terminated - if (strcmp(parent_group, group) != 0) continue; - (++nv)->index = i; - nv_get_nvObj(nv); - } - - return STAT_OK; -} - - -/* - * set_grp() - get or set one or more values in a group - * - * This functions is called "_set_group()" but technically it's a getter and - * a setter. It iterates the group children and either gets the value or sets - * the value for each depending on the nv->valuetype. - * - * This function serves JSON mode only as text mode shouldn't call it. - */ -stat_t set_grp(nvObj_t *nv) { - if (cfg.comm_mode == TEXT_MODE) - return STAT_INVALID_OR_MALFORMED_COMMAND; - - for (uint8_t i=0; inx) == 0) break; - if (nv->valuetype == TYPE_EMPTY) break; - else if (nv->valuetype == TYPE_0) nv_get(nv); // 0 means GET the value - else { - nv_set(nv); - nv_persist(nv); - } - } - - return STAT_OK; -} - - -/* - * nv_group_is_prefixed() - hack - * - * This little function deals with the exception cases that some groups don't use - * the parent token as a prefix to the child elements; SR being a good example. - */ -uint8_t nv_group_is_prefixed(char_t *group) { - if (strcmp("sr",group) == 0) return false; - if (strcmp("sys",group) == 0) return false; - return true; -} - - -// nvObj helper functions and other low-level nv helpers - -/* nv_get_index() - get index from mnenonic token + group - * - * nv_get_index() is the most expensive routine in the whole config. It does a - * linear table scan of the PROGMEM strings, which of course could be further - * optimized with indexes or hashing. - */ -index_t nv_get_index(const char_t *group, const char_t *token) { - char_t c; - char_t str[TOKEN_LEN + GROUP_LEN+1]; // should actually never be more than TOKEN_LEN+1 - strncpy(str, group, GROUP_LEN+1); - strncat(str, token, TOKEN_LEN+1); - - index_t i; - index_t index_max = nv_index_max(); - - for (i = 0; i < index_max; i++) { - if ((c = GET_TOKEN_BYTE(token[0])) != str[0]) continue; // 1st character mismatch - if ((c = GET_TOKEN_BYTE(token[1])) == 0) {if (str[1] == 0) return i;} // one character match - if (c != str[1]) continue; // 2nd character mismatch - if ((c = GET_TOKEN_BYTE(token[2])) == 0) {if (str[2] == 0) return i;} // two character match - if (c != str[2]) continue; // 3rd character mismatch - if ((c = GET_TOKEN_BYTE(token[3])) == 0) {if (str[3] == 0) return i;} // three character match - if (c != str[3]) continue; // 4th character mismatch - if ((c = GET_TOKEN_BYTE(token[4])) == 0) {if (str[4] == 0) return i;} // four character match - if (c != str[4]) continue; // 5th character mismatch - return i; // five character match - } - - return NO_MATCH; -} - -// nv_get_type() - returns command type as a NV_TYPE enum -uint8_t nv_get_type(nvObj_t *nv) { - if (nv->token[0] == 0) return NV_TYPE_0; - if (strcmp("gc", nv->token) == 0) return NV_TYPE_GCODE; - if (strcmp("sr", nv->token) == 0) return NV_TYPE_REPORT; - if (strcmp("qr", nv->token) == 0) return NV_TYPE_REPORT; - if (strcmp("msg",nv->token) == 0) return NV_TYPE_MESSAGE; - if (strcmp("err",nv->token) == 0) return NV_TYPE_MESSAGE; // errors are reported as messages - if (strcmp("n", nv->token) == 0) return NV_TYPE_LINENUM; - - return NV_TYPE_CONFIG; -} - - -/****************************************************************************** - * nvObj low-level object and list operations - * nv_get_nvObj() - setup a nv object by providing the index - * nv_reset_nv() - quick clear for a new nv object - * nv_reset_nv_list() - clear entire header, body and footer for a new use - * nv_copy_string() - used to write a string to shared string storage and link it - * nv_add_object() - write contents of parameter to first free object in the body - * nv_add_integer() - add an integer value to end of nv body (Note 1) - * nv_add_float() - add a floating point value to end of nv body - * nv_add_string() - add a string object to end of nv body - * nv_add_conditional_message() - add a message to nv body if messages are enabled - * - * Note: Functions that return a nv pointer point to the object that was modified or - * a 0 pointer if there was an error. - * - * Note: Adding a really large integer (like a checksum value) may lose precision due - * to the cast to a float. Sometimes it's better to load an integer as a string if - * all you want to do is display it. - * - * Note: A trick is to cast all string constants for nv_copy_string(), nv_add_object(), - * nv_add_string() and nv_add_conditional_message() to (const char_t *). Examples: - * - * nv_add_string((const char_t *)"msg", string); - * - * On the AVR this will save a little static RAM. The "msg" string will occupy flash - * as an initializer and be instantiated in stack RAM when the function executes. - */ -void nv_get_nvObj(nvObj_t *nv) { - if (nv->index >= nv_index_max()) { return; } // sanity - - index_t tmp = nv->index; - nv_reset_nv(nv); - nv->index = tmp; - - strcpy_P(nv->token, cfgArray[nv->index].token); // token field is always terminated - strcpy_P(nv->group, cfgArray[nv->index].group); // group field is always terminated - - // special processing for system groups and stripping tokens for groups - if (nv->group[0] != 0) { - if (GET_TABLE_BYTE(flags) & F_NOSTRIP) { - nv->group[0] = 0; - } else { - strcpy(nv->token, &nv->token[strlen(nv->group)]); // strip group from the token - } - } - - ((fptrCmd)GET_TABLE_WORD(get))(nv); // populate the value -} - - -nvObj_t *nv_reset_nv(nvObj_t *nv) { // clear a single nvObj structure - nv->valuetype = TYPE_EMPTY; // selective clear is much faster than calling memset - nv->index = 0; - nv->value = 0; - nv->precision = 0; - nv->token[0] = 0; - nv->group[0] = 0; - nv->stringp = 0; - - if (nv->pv == 0) nv->depth = 0; // set depth correctly - else { - if (nv->pv->valuetype == TYPE_PARENT) nv->depth = nv->pv->depth + 1; - else nv->depth = nv->pv->depth; - } - - return nv; // return pointer to nv as a convenience to callers -} - - -nvObj_t *nv_reset_nv_list() { // clear the header and response body - nvStr.wp = 0; // reset the shared string - nvObj_t *nv = nvl.list; // set up linked list and initialize elements - - for (uint8_t i=0; ipv = (nv-1); // the ends are bogus & corrected later - nv->nx = (nv+1); - nv->index = 0; - nv->depth = 1; // header and footer are corrected later - nv->precision = 0; - nv->valuetype = TYPE_EMPTY; - nv->token[0] = 0; - } - - (--nv)->nx = 0; - nv = nvl.list; // setup response header element ('r') - nv->pv = 0; - nv->depth = 0; - nv->valuetype = TYPE_PARENT; - strcpy(nv->token, "r"); - return nv_body; // this is a convenience for calling routines -} - - -stat_t nv_copy_string(nvObj_t *nv, const char_t *src) { - if ((nvStr.wp + strlen(src)) > NV_SHARED_STRING_LEN) - return STAT_BUFFER_FULL; - - char_t *dst = &nvStr.string[nvStr.wp]; - strcpy(dst, src); // copy string to current head position - // string has already been tested for overflow, above - nvStr.wp += strlen(src)+1; // advance head for next string - nv->stringp = (char_t (*)[])dst; - return STAT_OK; -} - - -nvObj_t *nv_add_object(const char_t *token) { // add an object to the body using a token - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == 0) return 0; // not supposed to find a 0; here for safety - continue; - } - - // load the index from the token or die trying - if ((nv->index = nv_get_index((const char_t *)"",token)) == NO_MATCH) { return 0;} - nv_get_nvObj(nv); // populate the object from the index - return nv; - } - - return 0; -} - - -nvObj_t *nv_add_integer(const char_t *token, const uint32_t value) { // add an integer object to the body - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == 0) return 0; // not supposed to find a 0; here for safety - continue; - } - - strncpy(nv->token, token, TOKEN_LEN); - nv->value = (float) value; - nv->valuetype = TYPE_INTEGER; - return nv; - } - - return 0; -} - - -nvObj_t *nv_add_data(const char_t *token, const uint32_t value) { // add an integer object to the body - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == 0) return 0; // not supposed to find a 0; here for safety - continue; - } - - strcpy(nv->token, token); - float *v = (float*)&value; - nv->value = *v; - nv->valuetype = TYPE_DATA; - return nv; - } - - return 0; -} - -nvObj_t *nv_add_float(const char_t *token, const float value) { // add a float object to the body - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == 0) return 0; // not supposed to find a 0; here for safety - continue; - } - - strncpy(nv->token, token, TOKEN_LEN); - nv->value = value; - nv->valuetype = TYPE_FLOAT; - return nv; - } - - return 0; -} - - -// ASSUMES A RAM STRING. If you need to post a FLASH string use pstr2str to convert it to a RAM string -nvObj_t *nv_add_string(const char_t *token, const char_t *string) { // add a string object to the body - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == 0) return 0; // not supposed to find a 0; here for safety - continue; - } - - strncpy(nv->token, token, TOKEN_LEN); - if (nv_copy_string(nv, string) != STAT_OK) return 0; - - nv->index = nv_get_index((const char_t *)"", nv->token); - nv->valuetype = TYPE_STRING; - return nv; - } - - return 0; -} - - -/* - * cm_conditional_message() - queue a RAM string as a message in the response (conditionally) - * - * Note: If you need to post a FLASH string use pstr2str to convert it to a RAM string - */ -nvObj_t *nv_add_conditional_message(const char_t *string) { // conditionally add a message object to the body - if ((cfg.comm_mode == JSON_MODE) && (js.echo_json_messages != true)) return 0; - return nv_add_string((const char_t *)"msg", string); -} - - -/**** nv_print_list() - print nv_array as JSON or text ********************** - * - * Generate and print the JSON and text mode output strings. Use this function - * for all text and JSON output that wants to be in a response header. - * Don't just printf stuff. - * - * Inputs: - * json_flags = JSON_OBJECT_FORMAT - print just the body w/o header or footer - * json_flags = JSON_RESPONSE_FORMAT - print a full "r" object with footer - * - * text_flags = TEXT_INLINE_PAIRS - print text as name/value pairs on a single line - * text_flags = TEXT_INLINE_VALUES - print text as comma separated values on a single line - * text_flags = TEXT_MULTILINE_FORMATTED - print text one value per line with formatting string - */ -void nv_print_list(stat_t status, uint8_t text_flags, uint8_t json_flags) { - if (cfg.comm_mode == JSON_MODE) json_print_list(status, json_flags); - else text_print_list(status, text_flags); -} - - -void nv_dump_nv(nvObj_t *nv) { - printf("i:%d, d:%d, t:%d, p:%d, v:%f, g:%s, t:%s, s:%s\n", nv->index, nv->depth, - nv->valuetype, nv->precision, (double)nv->value, nv->group, nv->token, - (char *)nv->stringp); -} diff --git a/src/config.h b/src/config.h index a51936c..4fdce29 100644 --- a/src/config.h +++ b/src/config.h @@ -1,367 +1,276 @@ -/* - * config.h - configuration sub-system generic part (see config_app for application part) - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2014 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#ifndef CONFIG_H_ONCE -#define CONFIG_H_ONCE - -// Please note config_app.h in included at the end of this file - -/**** Config System Overview and Usage *** - * - * --- Config objects and the config list --- - * - * The config system provides a structured way to get, set and print configuration variables. - * The config system operates as a list of "objects" (OK, so they are not really objects) - * that encapsulate each variable. The list may also may have header and footer objects. - * - * The list is populated by the text_parser or the JSON_parser depending on the mode. - * This way the internals don't care about how the variable is represented or communicated - * externally as all internal operations occur on the nvObjs, not the wire form (text or JSON). - */ - -/* --- Config variables, tables and strings --- - * - * Each configuration value is identified by a short mnemonic string (token). The token - * is resolved to an index into the cfgArray which is an array of structures with the - * static assignments for each variable. The cfgArray contains typed data in program - * memory (PROGMEM, in the AVR). - * - * Each cfgItem has: - * - group string identifying what group the variable is part of; or "" if no group - * - token string - the token for that variable - pre-pended with the group (if present) - * - operations flags - e.g. if the value should be initialized and/or persisted to NVM - * - function pointer for formatted print() method for text-mode readouts - * - function pointer for get() method - gets value from memory - * - function pointer for set() method - sets value and runs functions - * - target - memory location that the value is written to / read from - * - default value - for cold initialization - * - * Additionally an NVM array contains values persisted to EEPROM as floats; indexed by cfgArray index - * - * The following rules apply to mnemonic tokens - * - are up to 5 alphnuneric characters and cannot contain whitespace or separators - * - must be unique (non colliding). - * - axis tokens start with the axis letter and are typically 3 characters including the axis letter - * - motor tokens start with the motor digit and are typically 3 characters including the motor digit - * - non-axis or non-motor tokens are 2-5 characters and by convention generally should not start - * with: xyzabcuvw0123456789 (but there can be exceptions) - * - * "Groups" are collections of values that mimic REST resources. Groups include: - * - axis groups prefixed by "xyzabc" ("uvw" are reserved) - * - motor groups prefixed by "1234" ("56789" are reserved) - * - PWM groups prefixed by p1, p2 (p3 - p9 are reserved) - * - coordinate system groups prefixed by g54, g55, g56, g57, g59, g92 - * - a system group is identified by "sys" and contains a collection of otherwise unrelated values - * - * "Uber-groups" are groups of groups that are only used for text-mode printing - e.g. - * - group of all axes groups - * - group of all motor groups - * - group of all offset groups - * - group of all groups - */ - -/* --- Making changes and adding new values - * - * Adding a new value to config (or changing an existing one) involves touching the following places: - * - * - Create a new record in cfgArray[]. Use existing ones for examples. - * - * - Create functions for print, get, and set. You can often use existing generic fucntions for - * get and set, and sometimes print. If print requires any custom text it requires it's own function - * Look in the modules for examples - e.g. at the end of canoonical_machine.c - * - * - The ordering of group displays is set by the order of items in cfgArray. None of the other - * orders matter but are generally kept sequenced for easier reading and code maintenance. Also, - * Items earlier in the array will resolve token searches faster than ones later in the array. - * - * Note that matching will occur from the most specific to the least specific, meaning that - * if tokens overlap the longer one should be earlier in the array: "gco" should precede "gc". - */ - -/**** nvObj lists **** - * - * Commands and groups of commands are processed internally a doubly linked list of nvObj_t - * structures. This isolates the command and config internals from the details of communications, - * parsing and display in text mode and JSON mode. - * - * The first element of the list is designated the response header element ("r") but the list - * can also be serialized as a simple object by skipping over the header - * - * To use the nvObj list first reset it by calling nv_reset_nv_list(). This initializes the - * header, marks the the objects as TYPE_EMPTY (-1), resets the shared string, relinks all objects - * with NX and PV pointers, and makes the last element the terminating element by setting its NX - * pointer to 0. The terminating element may carry data, and will be processed. - * - * When you use the list you can terminate your own last element, or just leave the EMPTY elements - * to be skipped over during output serialization. - * - * We don't use recursion so parent/child nesting relationships are captured in a 'depth' variable, - * This must remain consistent if the curlies are to work out. You should not have to track depth - * explicitly if you use nv_reset_nv_list() or the accessor functions like nv_add_integer() or - * nv_add_message(). If you see problems with curlies check the depth values in the lists. - * - * Use the nv_print_list() dispatcher for all JSON and text output. Do not simply run through printf. - */ - -/* Token and Group Fields - * - * The nvObject struct (nvObj_t) has strict rules on the use of the token and group fields. - * The following forms are legal which support the use cases listed: - * - * Forms - * - group is 0; token is full token including any group profix - * - group is populated; token is carried without the group prefix - * - group is populated; token is 0 - indicates a group operation - * - * Use Cases - * - Lookup full token in cfgArray to get the index. Concatenates grp+token as key - * - Text-mode displays. Concatenates grp+token for display, may also use grp alone - * - JSON-mode display for single - element value e.g. xvm. Concatenate as above - * - JSON-mode display of a parent/child group. Parent is named grp, children nems are tokens - */ - -/* --- nv object string handling --- - * - * It's very expensive to allocate sufficient string space to each nvObj, so nv uses a cheater's - * malloc. A single string of length NV_SHARED_STRING_LEN is shared by all nvObjs for all strings. - * The observation is that the total rendered output in JSON or text mode cannot exceed the size of - * the output buffer (typ 256 bytes), So some number less than that is sufficient for shared strings. - * This is all mediated through nv_copy_string(), nv_copy_string_P(), and nv_reset_nv_list(). - */ - -/* --- Setting nvObj indexes --- - * - * It's the responsibility of the object creator to set the index. Downstream functions - * all expect a valid index. Set the index by calling nv_get_index(). This also validates - * the token and group if no lookup exists. Setting the index is an expensive operation - * (linear table scan), so there are some exceptions where the index does not need to be set. - * These cases are put in the code, commented out, and explained. - */ - -/* --- Other Notes:--- - * - * NV_BODY_LEN needs to allow for one parent JSON object and enough children to complete the - * largest possible operation - usually the status report. - */ - -#include - -// Sizing and footprints // chose one based on # of elements in cfgArray -typedef uint16_t index_t; // use this if there are > 255 indexed objects - - // defines allocated from stack (not-pre-allocated) -#define NV_FORMAT_LEN 128 // print formatting string max length -#define NV_MESSAGE_LEN 128 // sufficient space to contain end-user messages - - // pre-allocated defines (take RAM permanently) -#define NV_SHARED_STRING_LEN 512 // shared string for string values -#define NV_BODY_LEN 30 // body elements - allow for 1 parent + N children - // (each body element takes about 30 bytes of RAM) - - -// Stuff you probably don't want to change -#define GROUP_LEN 3 // max length of group prefix -#define TOKEN_LEN 5 // mnemonic token string: group prefix + short token -#define NV_FOOTER_LEN 18 // sufficient space to contain a JSON footer array -#define NV_LIST_LEN (NV_BODY_LEN + 2) // +2 allows for a header and a footer -#define NV_MAX_OBJECTS (NV_BODY_LEN - 1) // maximum number of objects in a body string -#define NO_MATCH (index_t)0xFFFF -#define NV_STATUS_REPORT_LEN NV_MAX_OBJECTS // max number of status report elements - see cfgArray - // **** must also line up in cfgArray, se00 - seXX **** - -enum tgCommunicationsMode { - TEXT_MODE = 0, // text command line mode - JSON_MODE, // strict JSON construction - JSON_MODE_RELAXED // relaxed JSON construction (future) -}; - - -enum flowControl { - FLOW_CONTROL_OFF = 0, // flow control disabled - FLOW_CONTROL_XON, // flow control uses XON/XOFF - FLOW_CONTROL_RTS // flow control uses RTS/CTS -}; - - -enum valueType { // value typing for config and JSON - TYPE_EMPTY = -1, // value struct is empty (which is not the same as "0") - TYPE_0 = 0, // value is 'null' (meaning the JSON null value) - TYPE_BOOL, // value is "true" (1) or "false"(0) - TYPE_INTEGER, // value is a uint32_t - TYPE_DATA, // value is blind cast to uint32_t - TYPE_FLOAT, // value is a floating point number - TYPE_STRING, // value is in string field - TYPE_ARRAY, // value is array element count, values are CSV ASCII in string field - TYPE_PARENT // object is a parent to a sub-object -}; - - -// Operations flags and shorthand -#define F_INITIALIZE 0x01 // initialize this item (run set during initialization) -#define F_PERSIST 0x02 // persist this item when set is run -#define F_NOSTRIP 0x04 // do not strip the group prefix from the token -#define F_CONVERT 0x08 // set if unit conversion is required - -#define _f0 0x00 -#define _fi (F_INITIALIZE) -#define _fp (F_PERSIST) -#define _fn (F_NOSTRIP) -#define _fc (F_CONVERT) -#define _fip (F_INITIALIZE | F_PERSIST) -#define _fipc (F_INITIALIZE | F_PERSIST | F_CONVERT) -#define _fipn (F_INITIALIZE | F_PERSIST | F_NOSTRIP) -#define _fipnc (F_INITIALIZE | F_PERSIST | F_NOSTRIP | F_CONVERT) - - -typedef struct nvString { // shared string object - uint16_t magic_start; -#if (NV_SHARED_STRING_LEN < 256) - uint8_t wp; // use this string array index value if string len < 256 bytes +#ifndef CONFIG_H +#define CONFIG_H + +// Compile-time settings +#define __STEP_CORRECTION +//#define __JERK_EXEC // Use computed jerk (versus forward difference based exec) +//#define __KAHAN // Use Kahan summation in aline exec functions + +#define INPUT_BUFFER_LEN 255 // text buffer size (255 max) + +#define AXES 6 // number of axes +#define MOTORS 4 // number of motors on the board +#define COORDS 6 // number of supported coordinate systems (1-6) +#define PWMS 2 // number of supported PWM channels + +#define AXIS_X 0 +#define AXIS_Y 1 +#define AXIS_Z 2 +#define AXIS_A 3 +#define AXIS_B 4 +#define AXIS_C 5 +#define AXIS_U 6 // reserved +#define AXIS_V 7 // reserved +#define AXIS_W 8 // reserved + +// Motor settings +#define MOTOR_MICROSTEPS 8 +#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // one of: MOTOR_DISABLED (0) + // MOTOR_ALWAYS_POWERED (1) + // MOTOR_POWERED_IN_CYCLE (2) + // MOTOR_POWERED_ONLY_WHEN_MOVING (3) +#define MOTOR_IDLE_TIMEOUT 2.00 // seconds to maintain motor at full power before idling + + +#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV 1.25 // 1tr +#define M1_MICROSTEPS MOTOR_MICROSTEPS // 1mi +#define M1_POLARITY 0 // 1po 0=normal, 1=reversed +#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp + +#define M2_MOTOR_MAP AXIS_Y +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 1.25 +#define M2_MICROSTEPS MOTOR_MICROSTEPS +#define M2_POLARITY 0 +#define M2_POWER_MODE MOTOR_POWER_MODE +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M3_MOTOR_MAP AXIS_Z +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 1.25 +#define M3_MICROSTEPS MOTOR_MICROSTEPS +#define M3_POLARITY 0 +#define M3_POWER_MODE MOTOR_POWER_MODE +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M4_MOTOR_MAP AXIS_A +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M4_MICROSTEPS MOTOR_MICROSTEPS +#define M4_POLARITY 0 +#define M4_POWER_MODE MOTOR_POWER_MODE +#define M4_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M5_MOTOR_MAP AXIS_B +#define M5_STEP_ANGLE 1.8 +#define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M5_MICROSTEPS MOTOR_MICROSTEPS +#define M5_POLARITY 0 +#define M5_POWER_MODE MOTOR_POWER_MODE +#define M5_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M6_MOTOR_MAP AXIS_C +#define M6_STEP_ANGLE 1.8 +#define M6_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M6_MICROSTEPS MOTOR_MICROSTEPS +#define M6_POLARITY 0 +#define M6_POWER_MODE MOTOR_POWER_MODE +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL + + +// Switch settings +#define SWITCH_TYPE SW_TYPE_NORMALLY_OPEN // one of: SW_TYPE_NORMALLY_OPEN, SW_TYPE_NORMALLY_CLOSED +#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define Y_SWITCH_MODE_MIN SW_MODE_HOMING +#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED +#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED +#define Z_SWITCH_MODE_MAX SW_MODE_HOMING +#define A_SWITCH_MODE_MIN SW_MODE_HOMING +#define A_SWITCH_MODE_MAX SW_MODE_DISABLED +#define B_SWITCH_MODE_MIN SW_MODE_HOMING +#define B_SWITCH_MODE_MAX SW_MODE_DISABLED +#define C_SWITCH_MODE_MIN SW_MODE_HOMING +#define C_SWITCH_MODE_MAX SW_MODE_DISABLED + + +// Machine settings +#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing +#define SOFT_LIMIT_ENABLE 0 // 0 = off, 1 = on +#define JERK_MAX 10 // yes, that's "20,000,000" mm/(min^3) +#define JUNCTION_DEVIATION 0.05 // default value, in mm +#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners + + +// Axis settings +#define VELOCITY_MAX 2000 +#define FEEDRATE_MAX 4000 + +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX VELOCITY_MAX // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX FEEDRATE_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits +#define X_TRAVEL_MAX 150 // xtm travel between switches or crashes +#define X_JERK_MAX JERK_MAX // xjm +#define X_JERK_HOMING (X_JERK_MAX * 2) // xjh +#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd +#define X_SEARCH_VELOCITY 500 // xsv move in negative direction +#define X_LATCH_VELOCITY 100 // xlv mm/min +#define X_LATCH_BACKOFF 5 // xlb mm +#define X_ZERO_BACKOFF 1 // xzb mm + +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX VELOCITY_MAX +#define Y_FEEDRATE_MAX FEEDRATE_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX 150 +#define Y_JERK_MAX JERK_MAX +#define Y_JERK_HOMING (Y_JERK_MAX * 2) +#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Y_SEARCH_VELOCITY 500 +#define Y_LATCH_VELOCITY 100 +#define Y_LATCH_BACKOFF 5 +#define Y_ZERO_BACKOFF 1 + +#define Z_AXIS_MODE AXIS_STANDARD +#define Z_VELOCITY_MAX VELOCITY_MAX +#define Z_FEEDRATE_MAX FEEDRATE_MAX +#define Z_TRAVEL_MIN 0 +#define Z_TRAVEL_MAX 75 +#define Z_JERK_MAX JERK_MAX +#define Z_JERK_HOMING (Z_JERK_MAX * 2) +#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Z_SEARCH_VELOCITY 400 +#define Z_LATCH_VELOCITY 100 +#define Z_LATCH_BACKOFF 5 +#define Z_ZERO_BACKOFF 1 + +#if 0 +#define A_AXIS_MODE AXIS_STANDARD +#define A_VELOCITY_MAX VELOCITY_MAX +#define A_FEEDRATE_MAX FEEDRATE_MAX +#define A_TRAVEL_MIN 0 +#define A_TRAVEL_MAX 75 +#define A_JERK_MAX JERK_MAX +#define A_JERK_HOMING (A_JERK_MAX * 2) +#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define A_SEARCH_VELOCITY 400 +#define A_LATCH_VELOCITY 100 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 1 +#define A_RADIUS 0 + +// A values are chosen to make the A motor react the same as X for testing #else - uint16_t wp; // use this string array index value is string len > 255 bytes +#define A_AXIS_MODE AXIS_RADIUS +#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis +#define A_FEEDRATE_MAX A_VELOCITY_MAX +#define A_TRAVEL_MIN -1 +#define A_TRAVEL_MAX -1 // same number means infinite +#define A_JERK_MAX (X_JERK_MAX * (360 / M1_TRAVEL_PER_REV)) +#define A_JERK_HOMING (A_JERK_MAX * 2) +#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define A_SEARCH_VELOCITY 600 +#define A_LATCH_VELOCITY 100 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 #endif - char_t string[NV_SHARED_STRING_LEN]; - uint16_t magic_end; // guard to detect string buffer underruns -} nvStr_t; - - -typedef struct nvObject { // depending on use, not all elements may be populated - struct nvObject *pv; // pointer to previous object or 0 if first object - struct nvObject *nx; // pointer to next object or 0 if last object - index_t index; // index of tokenized name, or -1 if no token (optional) - int8_t depth; // depth of object in the tree. 0 is root (-1 is invalid) - int8_t valuetype; // see valueType enum - int8_t precision; // decimal precision for reporting (JSON) - float value; // numeric value - char_t group[GROUP_LEN + 1]; // group prefix or 0 if not in a group - char_t token[TOKEN_LEN + 1]; // full mnemonic token for lookup - char_t (*stringp)[]; // pointer to array of characters from shared character array -} nvObj_t; - -typedef uint8_t (*fptrCmd)(nvObj_t *nv); // required for cfg table access -typedef void (*fptrPrint)(nvObj_t *nv); // required for PROGMEM access - - -typedef struct nvList { - uint16_t magic_start; - nvObj_t list[NV_LIST_LEN]; // list of nv objects, including space for a JSON header element - uint16_t magic_end; -} nvList_t; - - -typedef struct cfgItem { - char_t group[GROUP_LEN + 1]; // group prefix (with 0 termination) - char_t token[TOKEN_LEN + 1]; // token - stripped of group prefix (w/0 termination) - uint8_t flags; // operations flags - see defines below - int8_t precision; // decimal precision for display (JSON) - fptrPrint print; // print binding: aka void (*print)(nvObj_t *nv); - fptrCmd get; // GET binding aka uint8_t (*get)(nvObj_t *nv) - fptrCmd set; // SET binding aka uint8_t (*set)(nvObj_t *nv) - float *target; // target for writing config value - float def_value; // default value for config item -} cfgItem_t; - - -extern nvStr_t nvStr; -extern nvList_t nvl; -extern const cfgItem_t cfgArray[]; - - -#define nv_header (&nvl.list[0]) -#define nv_body (&nvl.list[1]) - - -void config_init(); -stat_t set_defaults(nvObj_t *nv); // reset config to default values -void config_init_assertions(); -stat_t config_test_assertions(); - - -// main entry points for core access functions -stat_t nv_get(nvObj_t *nv); // main entry point for get value -stat_t nv_set(nvObj_t *nv); // main entry point for set value -void nv_print(nvObj_t *nv); // main entry point for print value -stat_t nv_persist(nvObj_t *nv); // main entry point for persistence - - -// Helpers -uint8_t nv_get_type(nvObj_t *nv); -index_t nv_get_index(const char_t *group, const char_t *token); -index_t nv_index_max(); // (see config_app.c) -uint8_t nv_index_is_single(index_t index); // (see config_app.c) -uint8_t nv_index_is_group(index_t index); // (see config_app.c) -uint8_t nv_index_lt_groups(index_t index); // (see config_app.c) -uint8_t nv_group_is_prefixed(char_t *group); - - -// Generic internal functions and accessors -stat_t set_nul(nvObj_t *nv); // set nothing (no operation) -stat_t set_ui8(nvObj_t *nv); // set uint8_t value -stat_t set_01(nvObj_t *nv); // set a 0 or 1 value with validation -stat_t set_012(nvObj_t *nv); // set a 0, 1 or 2 value with validation -stat_t set_0123(nvObj_t *nv); // set a 0, 1, 2 or 3 value with validation -stat_t set_int(nvObj_t *nv); // set uint32_t integer value -stat_t set_data(nvObj_t *nv); // set uint32_t integer value blind cast -stat_t set_flt(nvObj_t *nv); // set floating point value - -stat_t get_nul(nvObj_t *nv); // get null value type -stat_t get_ui8(nvObj_t *nv); // get uint8_t value -stat_t get_int(nvObj_t *nv); // get uint32_t integer value -stat_t get_data(nvObj_t *nv); // get uint32_t integer value blind cast -stat_t get_flt(nvObj_t *nv); // get floating point value - -stat_t set_grp(nvObj_t *nv); // set data for a group -stat_t get_grp(nvObj_t *nv); // get data for a group - - -// nvObj and list functions -void nv_get_nvObj(nvObj_t *nv); -nvObj_t *nv_reset_nv(nvObj_t *nv); -nvObj_t *nv_reset_nv_list(); -stat_t nv_copy_string(nvObj_t *nv, const char_t *src); -nvObj_t *nv_add_object(const char_t *token); -nvObj_t *nv_add_integer(const char_t *token, const uint32_t value); -nvObj_t *nv_add_float(const char_t *token, const float value); -nvObj_t *nv_add_string(const char_t *token, const char_t *string); -nvObj_t *nv_add_conditional_message(const char_t *string); -void nv_print_list(stat_t status, uint8_t text_flags, uint8_t json_flags); - - -// Application specific helpers and functions (config_app.c) -stat_t set_flu(nvObj_t *nv); // set floating point number with G20/G21 units conversion -void preprocess_float(nvObj_t *nv); // pre-process float values for units and illegal values - - -// Diagnostics -void nv_dump_nv(nvObj_t *nv); - - -// Please notice that config_app.h is here -#include "config_app.h" - -#endif // CONFIG_H_ONCE +#define B_AXIS_MODE AXIS_DISABLED +#define B_VELOCITY_MAX 3600 +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MIN -1 +#define B_TRAVEL_MAX -1 +#define B_JERK_MAX JERK_MAX +#define B_JERK_HOMING (B_JERK_MAX * 2) +#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define B_RADIUS 1 +#define B_SEARCH_VELOCITY 600 +#define B_LATCH_VELOCITY 100 +#define B_LATCH_BACKOFF 5 +#define B_ZERO_BACKOFF 2 + +#define C_AXIS_MODE AXIS_DISABLED +#define C_VELOCITY_MAX 3600 +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MIN -1 +#define C_TRAVEL_MAX -1 +#define C_JERK_MAX JERK_MAX +#define C_JERK_HOMING (C_JERK_MAX * 2) +#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define C_RADIUS 1 +#define C_SEARCH_VELOCITY 600 +#define C_LATCH_VELOCITY 100 +#define C_LATCH_BACKOFF 5 +#define C_ZERO_BACKOFF 2 + + +// PWM settings +#define P1_PWM_FREQUENCY 100 // in Hz +#define P1_CW_SPEED_LO 1000 // in RPM (arbitrary units) +#define P1_CW_SPEED_HI 2000 +#define P1_CW_PHASE_LO 0.125 // phase [0..1] +#define P1_CW_PHASE_HI 0.2 +#define P1_CCW_SPEED_LO 1000 +#define P1_CCW_SPEED_HI 2000 +#define P1_CCW_PHASE_LO 0.125 +#define P1_CCW_PHASE_HI 0.2 +#define P1_PWM_PHASE_OFF 0.1 + + +// Gcode defaults +#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES +#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ +#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58 or G59 +#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS +#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE + + +// Coordinate offsets +#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros +#define G54_Y_OFFSET 0 +#define G54_Z_OFFSET 0 +#define G54_A_OFFSET 0 +#define G54_B_OFFSET 0 +#define G54_C_OFFSET 0 + +#define G55_X_OFFSET (X_TRAVEL_MAX / 2) // set to middle of table +#define G55_Y_OFFSET (Y_TRAVEL_MAX / 2) +#define G55_Z_OFFSET 0 +#define G55_A_OFFSET 0 +#define G55_B_OFFSET 0 +#define G55_C_OFFSET 0 + +#define G56_X_OFFSET 0 +#define G56_Y_OFFSET 0 +#define G56_Z_OFFSET 0 +#define G56_A_OFFSET 0 +#define G56_B_OFFSET 0 +#define G56_C_OFFSET 0 + +#define G57_X_OFFSET 0 +#define G57_Y_OFFSET 0 +#define G57_Z_OFFSET 0 +#define G57_A_OFFSET 0 +#define G57_B_OFFSET 0 +#define G57_C_OFFSET 0 + +#define G58_X_OFFSET 0 +#define G58_Y_OFFSET 0 +#define G58_Z_OFFSET 0 +#define G58_A_OFFSET 0 +#define G58_B_OFFSET 0 +#define G58_C_OFFSET 0 + +#define G59_X_OFFSET 0 +#define G59_Y_OFFSET 0 +#define G59_Z_OFFSET 0 +#define G59_A_OFFSET 0 +#define G59_B_OFFSET 0 +#define G59_C_OFFSET 0 + +#endif // CONFIG_H diff --git a/src/config_app.c b/src/config_app.c deleted file mode 100644 index c33e060..0000000 --- a/src/config_app.c +++ /dev/null @@ -1,880 +0,0 @@ -/* - * config_app.c - application-specific part of configuration data - * This file is part of the TinyG2 project - * - * Copyright (c) 2013 - 2015 Alden S. Hart, Jr. - * - * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* This file contains application specific data for the config system: - * - application-specific functions and function prototypes - * - application-specific message and print format strings - * - application-specific config array - * - any other application-specific data or functions - * - * See config_app.h for a detailed description of config objects and the config table - */ - -#include "tinyg.h" // #1 -#include "config.h" // #2 -#include "controller.h" -#include "canonical_machine.h" -#include "gcode_parser.h" -#include "json_parser.h" -#include "text_parser.h" -#include "settings.h" -#include "planner.h" -#include "stepper.h" -#include "switch.h" -#include "pwm.h" -#include "report.h" -#include "hardware.h" -#include "test.h" -#include "util.h" -#include "help.h" -#include "usart.h" -#include "tmc2660.h" - -cfgParameters_t cfg; // Application specific configuration parameters - -// See config.cpp/.h for generic variables and functions that are not specific to -// TinyG or the motion control application domain - -// helpers (most helpers are defined immediately above their usage so they don't need prototypes here) -static stat_t _do_motors(nvObj_t *nv); // print parameters for all motor groups -static stat_t _do_axes(nvObj_t *nv); // print parameters for all axis groups -static stat_t _do_offsets(nvObj_t *nv); // print offset parameters for G54-G59,G92, G28, G30 -static stat_t _do_all(nvObj_t *nv); // print all parameters - -// communications settings and functions -static stat_t set_ec(nvObj_t *nv); // expand CRLF on TX output -static stat_t set_ee(nvObj_t *nv); // enable character echo -static stat_t set_ex(nvObj_t *nv); // enable XON/XOFF and RTS/CTS flow control -static stat_t set_baud(nvObj_t *nv); // set baud rate -static stat_t get_rx(nvObj_t *nv); // get bytes in RX buffer - - -/*********************************************************************************** - **** CONFIG TABLE **************************************************************** - *********************************************************************************** - * - * NOTES AND CAVEATS - * - * - Token matching occurs from the most specific to the least specific. This means - * that if shorter tokens overlap longer ones the longer one must precede the - * shorter one. E.g. "gco" needs to come before "gc" - * - * - Mark group strings for entries that have no group as nul --> "". - * This is important for group expansion. - * - * - Groups do not have groups. Neither do uber-groups, e.g. - * 'x' is --> { "", "x", and 'm' is --> { "", "m", - * - * - Be careful not to define groups longer than GROUP_LEN (3) and tokens longer - * than TOKEN_LEN (5). (See config.h for lengths). The combined group + token - * cannot exceed TOKEN_LEN. String functions working on the table assume these - * rules are followed and do not check lengths or perform other validation. - * - * NOTE: If the count of lines in cfgArray exceeds 255 you need to change index_t - * uint16_t in the config.h file. - */ -const cfgItem_t cfgArray[] PROGMEM = { - // group, token, flags, p, print_func, get_func, set_func, target for get/set, default value - {"sys", "fb", _fipn,2, hw_print_fb, get_flt, set_nul, (float *)&cs.fw_build, TINYG_FIRMWARE_BUILD}, // MUST BE FIRST! - {"sys", "fv", _fipn,3, hw_print_fv, get_flt, set_nul, (float *)&cs.fw_version, TINYG_FIRMWARE_VERSION}, - {"sys", "hp", _fipn,0, hw_print_hp, get_flt, set_flt, (float *)&cs.hw_platform,TINYG_HARDWARE_PLATFORM}, - {"sys", "hv", _fipn,0, hw_print_hv, get_flt, hw_set_hv,(float *)&cs.hw_version, TINYG_HARDWARE_VERSION}, - {"sys", "id", _fn, 0, hw_print_id, hw_get_id, set_nul, (float *)&cs.null, 0}, // device ID (ASCII signature) - - {"mst", "mst1", _f0, 0, tmc2660_print_motor_step, tmc2660_get_motor_step, set_nul, (float *)&cs.null, 0}, - {"mst", "mst2", _f0, 0, tmc2660_print_motor_step, tmc2660_get_motor_step, set_nul, (float *)&cs.null, 0}, - {"mst", "mst3", _f0, 0, tmc2660_print_motor_step, tmc2660_get_motor_step, set_nul, (float *)&cs.null, 0}, - {"mst", "mst4", _f0, 0, tmc2660_print_motor_step, tmc2660_get_motor_step, set_nul, (float *)&cs.null, 0}, - - {"mfl", "mfl1", _f0, 0, tmc2660_print_motor_flags, tmc2660_get_motor_flags, set_nul, (float *)&cs.null, 0}, - {"mfl", "mfl2", _f0, 0, tmc2660_print_motor_flags, tmc2660_get_motor_flags, set_nul, (float *)&cs.null, 0}, - {"mfl", "mfl3", _f0, 0, tmc2660_print_motor_flags, tmc2660_get_motor_flags, set_nul, (float *)&cs.null, 0}, - {"mfl", "mfl4", _f0, 0, tmc2660_print_motor_flags, tmc2660_get_motor_flags, set_nul, (float *)&cs.null, 0}, - - // Dynamic model attributes for reporting purposes (up front for speed) - {"", "n", _fi, 0, cm_print_line, cm_get_mline,set_int,(float *)&cm.gm.linenum,0}, // Model line number - {"", "line",_fi, 0, cm_print_line, cm_get_line, set_int,(float *)&cm.gm.linenum,0}, // Active model or runtime line number - {"", "vel", _f0, 2, cm_print_vel, cm_get_vel, set_nul,(float *)&cs.null, 0}, // current velocity - {"", "feed",_f0, 2, cm_print_feed, cm_get_feed, set_nul,(float *)&cs.null, 0}, // feed rate - {"", "stat",_f0, 0, cm_print_stat, cm_get_stat, set_nul,(float *)&cs.null, 0}, // combined machine state - {"", "macs",_f0, 0, cm_print_macs, cm_get_macs, set_nul,(float *)&cs.null, 0}, // raw machine state - {"", "cycs",_f0, 0, cm_print_cycs, cm_get_cycs, set_nul,(float *)&cs.null, 0}, // cycle state - {"", "mots",_f0, 0, cm_print_mots, cm_get_mots, set_nul,(float *)&cs.null, 0}, // motion state - {"", "hold",_f0, 0, cm_print_hold, cm_get_hold, set_nul,(float *)&cs.null, 0}, // feedhold state - {"", "unit",_f0, 0, cm_print_unit, cm_get_unit, set_nul,(float *)&cs.null, 0}, // units mode - {"", "coor",_f0, 0, cm_print_coor, cm_get_coor, set_nul,(float *)&cs.null, 0}, // coordinate system - {"", "momo",_f0, 0, cm_print_momo, cm_get_momo, set_nul,(float *)&cs.null, 0}, // motion mode - {"", "plan",_f0, 0, cm_print_plan, cm_get_plan, set_nul,(float *)&cs.null, 0}, // plane select - {"", "path",_f0, 0, cm_print_path, cm_get_path, set_nul,(float *)&cs.null, 0}, // path control mode - {"", "dist",_f0, 0, cm_print_dist, cm_get_dist, set_nul,(float *)&cs.null, 0}, // distance mode - {"", "frmo",_f0, 0, cm_print_frmo, cm_get_frmo, set_nul,(float *)&cs.null, 0}, // feed rate mode - {"", "tool",_f0, 0, cm_print_tool, cm_get_toolv,set_nul,(float *)&cs.null, 0}, // active tool - - {"mpo","mpox",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0}, // X machine position - {"mpo","mpoy",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0}, // Y machine position - {"mpo","mpoz",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0}, // Z machine position - {"mpo","mpoa",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0}, // A machine position - {"mpo","mpob",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0}, // B machine position - {"mpo","mpoc",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0}, // C machine position - - {"pos","posx",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0}, // X work position - {"pos","posy",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0}, // Y work position - {"pos","posz",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0}, // Z work position - {"pos","posa",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0}, // A work position - {"pos","posb",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0}, // B work position - {"pos","posc",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0}, // C work position - - {"ofs","ofsx",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0}, // X work offset - {"ofs","ofsy",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0}, // Y work offset - {"ofs","ofsz",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0}, // Z work offset - {"ofs","ofsa",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0}, // A work offset - {"ofs","ofsb",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0}, // B work offset - {"ofs","ofsc",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0}, // C work offset - - {"hom","home",_f0, 0, cm_print_home, cm_get_home, cm_run_home,(float *)&cs.null, 0}, // homing state, invoke homing cycle - {"hom","homx",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_X], false}, // X homed - Homing status group - {"hom","homy",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_Y], false}, // Y homed - {"hom","homz",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_Z], false}, // Z homed - {"hom","homa",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_A], false}, // A homed - {"hom","homb",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_B], false}, // B homed - {"hom","homc",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_C], false}, // C homed - - {"prb","prbe",_f0, 0, tx_print_nul, get_ui8, set_nul,(float *)&cm.probe_state, 0}, // probing state - {"prb","prbx",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_X], 0}, - {"prb","prby",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_Y], 0}, - {"prb","prbz",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_Z], 0}, - {"prb","prba",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_A], 0}, - {"prb","prbb",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_B], 0}, - {"prb","prbc",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_C], 0}, - - {"jog","jogx",_f0, 0, tx_print_nul, get_nul, cm_run_jogx, (float *)&cm.jogging_dest, 0}, - {"jog","jogy",_f0, 0, tx_print_nul, get_nul, cm_run_jogy, (float *)&cm.jogging_dest, 0}, - {"jog","jogz",_f0, 0, tx_print_nul, get_nul, cm_run_jogz, (float *)&cm.jogging_dest, 0}, - {"jog","joga",_f0, 0, tx_print_nul, get_nul, cm_run_joga, (float *)&cm.jogging_dest, 0}, - - {"pwr","pwr1",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, // motor power enable readouts - {"pwr","pwr2",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, - {"pwr","pwr3",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, - {"pwr","pwr4",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, -#if (MOTORS >= 5) - {"pwr","pwr5",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, -#endif -#if (MOTORS >= 6) - {"pwr","pwr6",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, -#endif - - // Reports, tests, help, and messages - {"", "sr", _f0, 0, sr_print_sr, sr_get, sr_set, (float *)&cs.null, 0}, // status report object - {"", "qr", _f0, 0, qr_print_qr, qr_get, set_nul, (float *)&cs.null, 0}, // queue report - planner buffers available - {"", "qi", _f0, 0, qr_print_qi, qi_get, set_nul, (float *)&cs.null, 0}, // queue report - buffers added to queue - {"", "qo", _f0, 0, qr_print_qo, qo_get, set_nul, (float *)&cs.null, 0}, // queue report - buffers removed from queue - {"", "er", _f0, 0, tx_print_nul, rpt_er, set_nul, (float *)&cs.null, 0}, // invoke bogus exception report for testing - {"", "qf", _f0, 0, tx_print_nul, get_nul, cm_run_qf,(float *)&cs.null, 0}, // queue flush - {"", "rx", _f0, 0, tx_print_int, get_rx, set_nul, (float *)&cs.null, 0}, // space in RX buffer - {"", "msg", _f0, 0, tx_print_str, get_nul, set_nul, (float *)&cs.null, 0}, // string for generic messages - {"", "clear",_f0,0, tx_print_nul, cm_clear,cm_clear, (float *)&cs.null, 0}, // GET a clear to clear soft alarm - - {"", "test",_f0, 0, tx_print_nul, help_test, run_test, (float *)&cs.null,0}, // run tests, print test help screen - {"", "defa",_f0, 0, tx_print_nul, help_defa, set_defaults,(float *)&cs.null,0}, // set/print defaults / help screen - {"", "boot",_f0, 0, tx_print_nul, help_boot_loader,hw_run_boot, (float *)&cs.null,0}, - -#ifdef __HELP_SCREENS - {"", "help",_f0, 0, tx_print_nul, help_config, set_nul, (float *)&cs.null,0}, // prints config help screen - {"", "h", _f0, 0, tx_print_nul, help_config, set_nul, (float *)&cs.null,0}, // alias for "help" -#endif - - // Motor parameters - {"1","1ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_1].motor_map, M1_MOTOR_MAP}, - {"1","1sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_1].step_angle, M1_STEP_ANGLE}, - {"1","1tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_1].travel_rev, M1_TRAVEL_PER_REV}, - {"1","1mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_1].microsteps, M1_MICROSTEPS}, - {"1","1po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_1].polarity, M1_POLARITY}, - {"1","1pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_1].power_mode, M1_POWER_MODE}, -#if (MOTORS >= 2) - {"2","2ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_2].motor_map, M2_MOTOR_MAP}, - {"2","2sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_2].step_angle, M2_STEP_ANGLE}, - {"2","2tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_2].travel_rev, M2_TRAVEL_PER_REV}, - {"2","2mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_2].microsteps, M2_MICROSTEPS}, - {"2","2po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_2].polarity, M2_POLARITY}, - {"2","2pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_2].power_mode, M2_POWER_MODE}, -#endif -#if (MOTORS >= 3) - {"3","3ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_3].motor_map, M3_MOTOR_MAP}, - {"3","3sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_3].step_angle, M3_STEP_ANGLE}, - {"3","3tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_3].travel_rev, M3_TRAVEL_PER_REV}, - {"3","3mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_3].microsteps, M3_MICROSTEPS}, - {"3","3po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_3].polarity, M3_POLARITY}, - {"3","3pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_3].power_mode, M3_POWER_MODE}, -#endif -#if (MOTORS >= 4) - {"4","4ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_4].motor_map, M4_MOTOR_MAP}, - {"4","4sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_4].step_angle, M4_STEP_ANGLE}, - {"4","4tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_4].travel_rev, M4_TRAVEL_PER_REV}, - {"4","4mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_4].microsteps, M4_MICROSTEPS}, - {"4","4po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_4].polarity, M4_POLARITY}, - {"4","4pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_4].power_mode, M4_POWER_MODE}, -#endif -#if (MOTORS >= 5) - {"5","5ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_5].motor_map, M5_MOTOR_MAP}, - {"5","5sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_5].step_angle, M5_STEP_ANGLE}, - {"5","5tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_5].travel_rev, M5_TRAVEL_PER_REV}, - {"5","5mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_5].microsteps, M5_MICROSTEPS}, - {"5","5po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_5].polarity, M5_POLARITY}, - {"5","5pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_5].power_mode, M5_POWER_MODE}, -#endif -#if (MOTORS >= 6) - {"6","6ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_6].motor_map, M6_MOTOR_MAP}, - {"6","6sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_6].step_angle, M6_STEP_ANGLE}, - {"6","6tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_6].travel_rev, M6_TRAVEL_PER_REV}, - {"6","6mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_6].microsteps, M6_MICROSTEPS}, - {"6","6po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_6].polarity, M6_POLARITY}, - {"6","6pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_6].power_mode, M6_POWER_MODE}, -#endif - // Axis parameters - {"x","xam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_X].axis_mode, X_AXIS_MODE}, - {"x","xvm",_fipc, 0, cm_print_vm, get_flt, set_flu, (float *)&cm.a[AXIS_X].velocity_max, X_VELOCITY_MAX}, - {"x","xfr",_fipc, 0, cm_print_fr, get_flt, set_flu, (float *)&cm.a[AXIS_X].feedrate_max, X_FEEDRATE_MAX}, - {"x","xtn",_fipc, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_X].travel_min, X_TRAVEL_MIN}, - {"x","xtm",_fipc, 3, cm_print_tm, get_flt, set_flu, (float *)&cm.a[AXIS_X].travel_max, X_TRAVEL_MAX}, - {"x","xjm",_fipc, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_X].jerk_max, X_JERK_MAX}, - {"x","xjh",_fipc, 0, cm_print_jh, get_flt, cm_set_xjh,(float *)&cm.a[AXIS_X].jerk_homing, X_JERK_HOMING}, - {"x","xjd",_fipc, 4, cm_print_jd, get_flt, set_flu, (float *)&cm.a[AXIS_X].junction_dev, X_JUNCTION_DEVIATION}, - {"x","xsn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.mode[0], X_SWITCH_MODE_MIN}, - {"x","xsx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.mode[1], X_SWITCH_MODE_MAX}, - {"x","xsv",_fipc, 0, cm_print_sv, get_flt, set_flu, (float *)&cm.a[AXIS_X].search_velocity, X_SEARCH_VELOCITY}, - {"x","xlv",_fipc, 0, cm_print_lv, get_flt, set_flu, (float *)&cm.a[AXIS_X].latch_velocity, X_LATCH_VELOCITY}, - {"x","xlb",_fipc, 3, cm_print_lb, get_flt, set_flu, (float *)&cm.a[AXIS_X].latch_backoff, X_LATCH_BACKOFF}, - {"x","xzb",_fipc, 3, cm_print_zb, get_flt, set_flu, (float *)&cm.a[AXIS_X].zero_backoff, X_ZERO_BACKOFF}, - - {"y","yam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_Y].axis_mode, Y_AXIS_MODE}, - {"y","yvm",_fipc, 0, cm_print_vm, get_flt, set_flu, (float *)&cm.a[AXIS_Y].velocity_max, Y_VELOCITY_MAX}, - {"y","yfr",_fipc, 0, cm_print_fr, get_flt, set_flu, (float *)&cm.a[AXIS_Y].feedrate_max, Y_FEEDRATE_MAX}, - {"y","ytn",_fipc, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_Y].travel_min, Y_TRAVEL_MIN}, - {"y","ytm",_fipc, 3, cm_print_tm, get_flt, set_flu, (float *)&cm.a[AXIS_Y].travel_max, Y_TRAVEL_MAX}, - {"y","yjm",_fipc, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_Y].jerk_max, Y_JERK_MAX}, - {"y","yjh",_fipc, 0, cm_print_jh, get_flt, cm_set_xjh,(float *)&cm.a[AXIS_Y].jerk_homing, Y_JERK_HOMING}, - {"y","yjd",_fipc, 4, cm_print_jd, get_flt, set_flu, (float *)&cm.a[AXIS_Y].junction_dev, Y_JUNCTION_DEVIATION}, - {"y","ysn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.mode[2], Y_SWITCH_MODE_MIN}, - {"y","ysx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.mode[3], Y_SWITCH_MODE_MAX}, - {"y","ysv",_fipc, 0, cm_print_sv, get_flt, set_flu, (float *)&cm.a[AXIS_Y].search_velocity, Y_SEARCH_VELOCITY}, - {"y","ylv",_fipc, 0, cm_print_lv, get_flt, set_flu, (float *)&cm.a[AXIS_Y].latch_velocity, Y_LATCH_VELOCITY}, - {"y","ylb",_fipc, 3, cm_print_lb, get_flt, set_flu, (float *)&cm.a[AXIS_Y].latch_backoff, Y_LATCH_BACKOFF}, - {"y","yzb",_fipc, 3, cm_print_zb, get_flt, set_flu, (float *)&cm.a[AXIS_Y].zero_backoff, Y_ZERO_BACKOFF}, - - {"z","zam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_Z].axis_mode, Z_AXIS_MODE}, - {"z","zvm",_fipc, 0, cm_print_vm, get_flt, set_flu, (float *)&cm.a[AXIS_Z].velocity_max, Z_VELOCITY_MAX}, - {"z","zfr",_fipc, 0, cm_print_fr, get_flt, set_flu, (float *)&cm.a[AXIS_Z].feedrate_max, Z_FEEDRATE_MAX}, - {"z","ztn",_fipc, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_Z].travel_min, Z_TRAVEL_MIN}, - {"z","ztm",_fipc, 3, cm_print_tm, get_flt, set_flu, (float *)&cm.a[AXIS_Z].travel_max, Z_TRAVEL_MAX}, - {"z","zjm",_fipc, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_Z].jerk_max, Z_JERK_MAX}, - {"z","zjh",_fipc, 0, cm_print_jh, get_flt, cm_set_xjh,(float *)&cm.a[AXIS_Z].jerk_homing, Z_JERK_HOMING}, - {"z","zjd",_fipc, 4, cm_print_jd, get_flt, set_flu, (float *)&cm.a[AXIS_Z].junction_dev, Z_JUNCTION_DEVIATION}, - {"z","zsn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.mode[4], Z_SWITCH_MODE_MIN}, - {"z","zsx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.mode[5], Z_SWITCH_MODE_MAX}, - {"z","zsv",_fipc, 0, cm_print_sv, get_flt, set_flu, (float *)&cm.a[AXIS_Z].search_velocity, Z_SEARCH_VELOCITY}, - {"z","zlv",_fipc, 0, cm_print_lv, get_flt, set_flu, (float *)&cm.a[AXIS_Z].latch_velocity, Z_LATCH_VELOCITY}, - {"z","zlb",_fipc, 3, cm_print_lb, get_flt, set_flu, (float *)&cm.a[AXIS_Z].latch_backoff, Z_LATCH_BACKOFF}, - {"z","zzb",_fipc, 3, cm_print_zb, get_flt, set_flu, (float *)&cm.a[AXIS_Z].zero_backoff, Z_ZERO_BACKOFF}, - - {"a","aam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_A].axis_mode, A_AXIS_MODE}, - {"a","avm",_fip, 0, cm_print_vm, get_flt, set_flt, (float *)&cm.a[AXIS_A].velocity_max, A_VELOCITY_MAX}, - {"a","afr",_fip, 0, cm_print_fr, get_flt, set_flt, (float *)&cm.a[AXIS_A].feedrate_max, A_FEEDRATE_MAX}, - {"a","atn",_fip, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_A].travel_min, A_TRAVEL_MIN}, - {"a","atm",_fip, 3, cm_print_tm, get_flt, set_flt, (float *)&cm.a[AXIS_A].travel_max, A_TRAVEL_MAX}, - {"a","ajm",_fip, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_A].jerk_max, A_JERK_MAX}, - {"a","ajh",_fip, 0, cm_print_jh, get_flt, cm_set_xjh,(float *)&cm.a[AXIS_A].jerk_homing, A_JERK_HOMING}, - {"a","ajd",_fip, 4, cm_print_jd, get_flt, set_flt, (float *)&cm.a[AXIS_A].junction_dev, A_JUNCTION_DEVIATION}, - {"a","ara",_fipc, 3, cm_print_ra, get_flt, set_flt, (float *)&cm.a[AXIS_A].radius, A_RADIUS}, - {"a","asn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.mode[6], A_SWITCH_MODE_MIN}, - {"a","asx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.mode[7], A_SWITCH_MODE_MAX}, - {"a","asv",_fip, 0, cm_print_sv, get_flt, set_flt, (float *)&cm.a[AXIS_A].search_velocity, A_SEARCH_VELOCITY}, - {"a","alv",_fip, 0, cm_print_lv, get_flt, set_flt, (float *)&cm.a[AXIS_A].latch_velocity, A_LATCH_VELOCITY}, - {"a","alb",_fip, 3, cm_print_lb, get_flt, set_flt, (float *)&cm.a[AXIS_A].latch_backoff, A_LATCH_BACKOFF}, - {"a","azb",_fip, 3, cm_print_zb, get_flt, set_flt, (float *)&cm.a[AXIS_A].zero_backoff, A_ZERO_BACKOFF}, - - {"b","bam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_B].axis_mode, B_AXIS_MODE}, - {"b","bvm",_fip, 0, cm_print_vm, get_flt, set_flt, (float *)&cm.a[AXIS_B].velocity_max, B_VELOCITY_MAX}, - {"b","bfr",_fip, 0, cm_print_fr, get_flt, set_flt, (float *)&cm.a[AXIS_B].feedrate_max, B_FEEDRATE_MAX}, - {"b","btn",_fip, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_B].travel_min, B_TRAVEL_MIN}, - {"b","btm",_fip, 3, cm_print_tm, get_flt, set_flt, (float *)&cm.a[AXIS_B].travel_max, B_TRAVEL_MAX}, - {"b","bjm",_fip, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_B].jerk_max, B_JERK_MAX}, - {"b","bjd",_fip, 0, cm_print_jd, get_flt, set_flt, (float *)&cm.a[AXIS_B].junction_dev, B_JUNCTION_DEVIATION}, - {"b","bra",_fipc, 3, cm_print_ra, get_flt, set_flt, (float *)&cm.a[AXIS_B].radius, B_RADIUS}, - - {"c","cam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_C].axis_mode, C_AXIS_MODE}, - {"c","cvm",_fip, 0, cm_print_vm, get_flt, set_flt, (float *)&cm.a[AXIS_C].velocity_max, C_VELOCITY_MAX}, - {"c","cfr",_fip, 0, cm_print_fr, get_flt, set_flt, (float *)&cm.a[AXIS_C].feedrate_max, C_FEEDRATE_MAX}, - {"c","ctn",_fip, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_C].travel_min, C_TRAVEL_MIN}, - {"c","ctm",_fip, 3, cm_print_tm, get_flt, set_flt, (float *)&cm.a[AXIS_C].travel_max, C_TRAVEL_MAX}, - {"c","cjm",_fip, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_C].jerk_max, C_JERK_MAX}, - {"c","cjd",_fip, 0, cm_print_jd, get_flt, set_flt, (float *)&cm.a[AXIS_C].junction_dev, C_JUNCTION_DEVIATION}, - {"c","cra",_fipc, 3, cm_print_ra, get_flt, set_flt, (float *)&cm.a[AXIS_C].radius, C_RADIUS}, - - // PWM settings - {"p1","p1frq",_fip, 0, pwm_print_p1frq, get_flt, set_flt,(float *)&pwm.c[PWM_1].frequency, P1_PWM_FREQUENCY}, - {"p1","p1csl",_fip, 0, pwm_print_p1csl, get_flt, set_flt,(float *)&pwm.c[PWM_1].cw_speed_lo, P1_CW_SPEED_LO}, - {"p1","p1csh",_fip, 0, pwm_print_p1csh, get_flt, set_flt,(float *)&pwm.c[PWM_1].cw_speed_hi, P1_CW_SPEED_HI}, - {"p1","p1cpl",_fip, 3, pwm_print_p1cpl, get_flt, set_flt,(float *)&pwm.c[PWM_1].cw_phase_lo, P1_CW_PHASE_LO}, - {"p1","p1cph",_fip, 3, pwm_print_p1cph, get_flt, set_flt,(float *)&pwm.c[PWM_1].cw_phase_hi, P1_CW_PHASE_HI}, - {"p1","p1wsl",_fip, 0, pwm_print_p1wsl, get_flt, set_flt,(float *)&pwm.c[PWM_1].ccw_speed_lo, P1_CCW_SPEED_LO}, - {"p1","p1wsh",_fip, 0, pwm_print_p1wsh, get_flt, set_flt,(float *)&pwm.c[PWM_1].ccw_speed_hi, P1_CCW_SPEED_HI}, - {"p1","p1wpl",_fip, 3, pwm_print_p1wpl, get_flt, set_flt,(float *)&pwm.c[PWM_1].ccw_phase_lo, P1_CCW_PHASE_LO}, - {"p1","p1wph",_fip, 3, pwm_print_p1wph, get_flt, set_flt,(float *)&pwm.c[PWM_1].ccw_phase_hi, P1_CCW_PHASE_HI}, - {"p1","p1pof",_fip, 3, pwm_print_p1pof, get_flt, set_flt,(float *)&pwm.c[PWM_1].phase_off, P1_PWM_PHASE_OFF}, - - // Coordinate system offsets (G54-G59 and G92) - {"g54","g54x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_X], G54_X_OFFSET}, - {"g54","g54y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_Y], G54_Y_OFFSET}, - {"g54","g54z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_Z], G54_Z_OFFSET}, - {"g54","g54a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_A], G54_A_OFFSET}, - {"g54","g54b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_B], G54_B_OFFSET}, - {"g54","g54c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_C], G54_C_OFFSET}, - - {"g55","g55x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_X], G55_X_OFFSET}, - {"g55","g55y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_Y], G55_Y_OFFSET}, - {"g55","g55z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_Z], G55_Z_OFFSET}, - {"g55","g55a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_A], G55_A_OFFSET}, - {"g55","g55b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_B], G55_B_OFFSET}, - {"g55","g55c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_C], G55_C_OFFSET}, - - {"g56","g56x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_X], G56_X_OFFSET}, - {"g56","g56y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_Y], G56_Y_OFFSET}, - {"g56","g56z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_Z], G56_Z_OFFSET}, - {"g56","g56a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_A], G56_A_OFFSET}, - {"g56","g56b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_B], G56_B_OFFSET}, - {"g56","g56c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_C], G56_C_OFFSET}, - - {"g57","g57x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_X], G57_X_OFFSET}, - {"g57","g57y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_Y], G57_Y_OFFSET}, - {"g57","g57z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_Z], G57_Z_OFFSET}, - {"g57","g57a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_A], G57_A_OFFSET}, - {"g57","g57b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_B], G57_B_OFFSET}, - {"g57","g57c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_C], G57_C_OFFSET}, - - {"g58","g58x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_X], G58_X_OFFSET}, - {"g58","g58y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_Y], G58_Y_OFFSET}, - {"g58","g58z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_Z], G58_Z_OFFSET}, - {"g58","g58a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_A], G58_A_OFFSET}, - {"g58","g58b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_B], G58_B_OFFSET}, - {"g58","g58c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_C], G58_C_OFFSET}, - - {"g59","g59x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_X], G59_X_OFFSET}, - {"g59","g59y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_Y], G59_Y_OFFSET}, - {"g59","g59z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_Z], G59_Z_OFFSET}, - {"g59","g59a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_A], G59_A_OFFSET}, - {"g59","g59b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_B], G59_B_OFFSET}, - {"g59","g59c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_C], G59_C_OFFSET}, - - {"g92","g92x",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_X], 0}, // G92 handled differently - {"g92","g92y",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_Y], 0}, - {"g92","g92z",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_Z], 0}, - {"g92","g92a",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_A], 0}, - {"g92","g92b",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_B], 0}, - {"g92","g92c",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_C], 0}, - - // Coordinate positions (G28, G30) - {"g28","g28x",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_X], 0}, // g28 handled differently - {"g28","g28y",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_Y], 0}, - {"g28","g28z",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_Z], 0}, - {"g28","g28a",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_A], 0}, - {"g28","g28b",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_B], 0}, - {"g28","g28c",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_C], 0}, - - {"g30","g30x",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_X], 0}, // g30 handled differently - {"g30","g30y",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_Y], 0}, - {"g30","g30z",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_Z], 0}, - {"g30","g30a",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_A], 0}, - {"g30","g30b",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_B], 0}, - {"g30","g30c",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_C], 0}, - - // this is a 128bit UUID for identifying a previously committed job state - {"jid","jida",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cs.job_id[0], 0}, - {"jid","jidb",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cs.job_id[1], 0}, - {"jid","jidc",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cs.job_id[2], 0}, - {"jid","jidd",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cs.job_id[3], 0}, - - // System parameters - {"sys","ja", _fipnc,0, cm_print_ja, get_flt, set_flu, (float *)&cm.junction_acceleration, JUNCTION_ACCELERATION}, - {"sys","ct", _fipnc,4, cm_print_ct, get_flt, set_flu, (float *)&cm.chordal_tolerance, CHORDAL_TOLERANCE}, - {"sys","sl", _fipn, 0, cm_print_sl, get_ui8, set_ui8, (float *)&cm.soft_limit_enable, SOFT_LIMIT_ENABLE}, - {"sys","st", _fipn, 0, sw_print_st, get_ui8, sw_set_st, (float *)&sw.switch_type, SWITCH_TYPE}, - {"sys","mt", _fipn, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.motor_power_timeout, MOTOR_IDLE_TIMEOUT}, - {"", "me", _f0, 0, tx_print_str, st_set_me, st_set_me, (float *)&cs.null, 0}, - {"", "md", _f0, 0, tx_print_str, st_set_md, st_set_md, (float *)&cs.null, 0}, - - {"sys","ej", _fipn, 0, js_print_ej, get_ui8, set_01, (float *)&cfg.comm_mode, COMM_MODE}, - {"sys","jv", _fipn, 0, js_print_jv, get_ui8, json_set_jv,(float *)&js.json_verbosity, JSON_VERBOSITY}, - {"sys","js", _fipn, 0, js_print_js, get_ui8, set_01, (float *)&js.json_syntax, JSON_SYNTAX_MODE}, - {"sys","tv", _fipn, 0, tx_print_tv, get_ui8, set_01, (float *)&txt.text_verbosity, TEXT_VERBOSITY}, - {"sys","qv", _fipn, 0, qr_print_qv, get_ui8, set_0123, (float *)&qr.queue_report_verbosity, QUEUE_REPORT_VERBOSITY}, - {"sys","sv", _fipn, 0, sr_print_sv, get_ui8, set_012, (float *)&sr.status_report_verbosity, STATUS_REPORT_VERBOSITY}, - {"sys","si", _fipn, 0, sr_print_si, get_int, sr_set_si, (float *)&sr.status_report_interval, STATUS_REPORT_INTERVAL_MS}, - - {"sys","ec", _fipn, 0, cfg_print_ec, get_ui8, set_ec, (float *)&cfg.enable_cr, COM_EXPAND_CR}, - {"sys","ee", _fipn, 0, cfg_print_ee, get_ui8, set_ee, (float *)&cfg.enable_echo, COM_ENABLE_ECHO}, - {"sys","ex", _fipn, 0, cfg_print_ex, get_ui8, set_ex, (float *)&cfg.enable_flow_control, COM_ENABLE_FLOW_CONTROL}, - {"sys","baud",_fn, 0, cfg_print_baud,get_ui8, set_baud, (float *)&cfg.baud_rate, USART_BAUD_115200}, - - // NOTE: The ordering within the gcode defaults is important for token resolution - {"sys","gpl", _fipn, 0, cm_print_gpl, get_ui8, set_012, (float *)&cm.select_plane, GCODE_DEFAULT_PLANE}, - {"sys","gun", _fipn, 0, cm_print_gun, get_ui8, set_01, (float *)&cm.units_mode, GCODE_DEFAULT_UNITS}, - {"sys","gco", _fipn, 0, cm_print_gco, get_ui8, set_ui8, (float *)&cm.coord_system, GCODE_DEFAULT_COORD_SYSTEM}, - {"sys","gpa", _fipn, 0, cm_print_gpa, get_ui8, set_012, (float *)&cm.path_control, GCODE_DEFAULT_PATH_CONTROL}, - {"sys","gdi", _fipn, 0, cm_print_gdi, get_ui8, set_01, (float *)&cm.distance_mode, GCODE_DEFAULT_DISTANCE_MODE}, - {"", "gc", _f0, 0, tx_print_nul, gc_get_gc, gc_run_gc,(float *)&cs.null, 0}, // gcode block - must be last in this group - - // "hidden" parameters (not in system group) - {"", "ma", _fipc,4, cm_print_ma, get_flt, set_flu, (float *)&cm.arc_segment_len, ARC_SEGMENT_LENGTH}, - {"", "fd", _fip, 0, tx_print_ui8, get_ui8, set_01, (float *)&js.json_footer_depth, JSON_FOOTER_DEPTH}, - - // User defined data groups - {"uda","uda0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[0], USER_DATA_A0}, - {"uda","uda1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[1], USER_DATA_A1}, - {"uda","uda2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[2], USER_DATA_A2}, - {"uda","uda3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[3], USER_DATA_A3}, - - {"udb","udb0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[0], USER_DATA_B0}, - {"udb","udb1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[1], USER_DATA_B1}, - {"udb","udb2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[2], USER_DATA_B2}, - {"udb","udb3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[3], USER_DATA_B3}, - - {"udc","udc0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[0], USER_DATA_C0}, - {"udc","udc1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[1], USER_DATA_C1}, - {"udc","udc2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[2], USER_DATA_C2}, - {"udc","udc3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[3], USER_DATA_C3}, - - {"udd","udd0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[0], USER_DATA_D0}, - {"udd","udd1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[1], USER_DATA_D1}, - {"udd","udd2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[2], USER_DATA_D2}, - {"udd","udd3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[3], USER_DATA_D3}, - - // Diagnostic parameters -#ifdef __DIAGNOSTIC_PARAMETERS - {"_te","_tex",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_X], 0}, // X target endpoint - {"_te","_tey",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_Y], 0}, - {"_te","_tez",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_Z], 0}, - {"_te","_tea",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_A], 0}, - {"_te","_teb",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_B], 0}, - {"_te","_tec",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_C], 0}, - - {"_tr","_trx",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_X], 0}, // X target runtime - {"_tr","_try",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_Y], 0}, - {"_tr","_trz",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_Z], 0}, - {"_tr","_tra",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_A], 0}, - {"_tr","_trb",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_B], 0}, - {"_tr","_trc",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_C], 0}, - -#if (MOTORS >= 1) - {"_ts","_ts1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_1], 0}, // Motor 1 target steps - {"_ps","_ps1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_1], 0}, // Motor 1 position steps - {"_cs","_cs1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_1], 0}, // Motor 1 commanded steps (delayed steps) - {"_es","_es1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_1], 0}, // Motor 1 encoder steps - {"_xs","_xs1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_1].corrected_steps, 0}, // Motor 1 correction steps applied - {"_fe","_fe1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_1], 0}, // Motor 1 following error in steps -#endif -#if (MOTORS >= 2) - {"_ts","_ts2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_2], 0}, - {"_ps","_ps2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_2], 0}, - {"_cs","_cs2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_2], 0}, - {"_es","_es2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_2], 0}, - {"_xs","_xs2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_2].corrected_steps, 0}, - {"_fe","_fe2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_2], 0}, -#endif -#if (MOTORS >= 3) - {"_ts","_ts3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_3], 0}, - {"_ps","_ps3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_3], 0}, - {"_cs","_cs3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_3], 0}, - {"_es","_es3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_3], 0}, - {"_xs","_xs3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_3].corrected_steps, 0}, - {"_fe","_fe3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_3], 0}, -#endif -#if (MOTORS >= 4) - {"_ts","_ts4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_4], 0}, - {"_ps","_ps4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_4], 0}, - {"_cs","_cs4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_4], 0}, - {"_es","_es4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_4], 0}, - {"_xs","_xs4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_4].corrected_steps, 0}, - {"_fe","_fe4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_4], 0}, -#endif -#if (MOTORS >= 5) - {"_ts","_ts5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_5], 0}, - {"_ps","_ps5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_5], 0}, - {"_cs","_cs5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_5], 0}, - {"_es","_es5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_5], 0}, - {"_xs","_xs6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_6].corrected_steps, 0}, - {"_fe","_fe5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_5], 0}, -#endif -#if (MOTORS >= 6) - {"_ts","_ts6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_6], 0}, - {"_ps","_ps6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_6], 0}, - {"_cs","_cs6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_6], 0}, - {"_es","_es6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_6], 0}, - {"_xs","_xs5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_5].corrected_steps, 0}, - {"_fe","_fe6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_6], 0}, -#endif - {"", "_dam",_f0, 0, tx_print_nul, cm_dam, cm_dam, (float *)&cs.null, 0}, // dump active model -#endif // __DIAGNOSTIC_PARAMETERS - - // Persistence for status report - must be in sequence - // *** Count must agree with NV_STATUS_REPORT_LEN in config.h *** - {"","se00",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[0],0}, - {"","se01",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[1],0}, - {"","se02",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[2],0}, - {"","se03",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[3],0}, - {"","se04",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[4],0}, - {"","se05",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[5],0}, - {"","se06",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[6],0}, - {"","se07",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[7],0}, - {"","se08",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[8],0}, - {"","se09",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[9],0}, - {"","se10",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[10],0}, - {"","se11",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[11],0}, - {"","se12",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[12],0}, - {"","se13",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[13],0}, - {"","se14",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[14],0}, - {"","se15",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[15],0}, - {"","se16",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[16],0}, - {"","se17",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[17],0}, - {"","se18",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[18],0}, - {"","se19",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[19],0}, - {"","se20",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[20],0}, - {"","se21",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[21],0}, - {"","se22",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[22],0}, - {"","se23",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[23],0}, - {"","se24",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[24],0}, - {"","se25",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[25],0}, - {"","se26",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[26],0}, - {"","se27",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[27],0}, - {"","se28",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[28],0}, - {"","se29",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[29],0}, - // Count is 30, since se00 counts as one. - - // Group lookups - must follow the single-valued entries for proper sub-string matching - // *** Must agree with NV_COUNT_GROUPS below *** - // *** START COUNTING FROM HERE *** - {"","sys",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // system group - {"","p1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // PWM 1 group - - {"","1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // motor groups - {"","2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, - {"","3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, - {"","4", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, -#if (MOTORS >= 5) - {"","5", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, -#endif -#if (MOTORS >= 6) - {"","6", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, -#endif - - {"","x", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // axis groups - {"","y", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, - {"","z", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, - {"","a", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, - {"","b", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, - {"","c", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, - - {"","g54",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // coord offset groups - {"","g55",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, - {"","g56",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, - {"","g57",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, - {"","g58",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, - {"","g59",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, - {"","g92",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // origin offsets - {"","g28",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // g28 home position - {"","g30",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // g30 home position - - {"","mpo",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // machine position group - {"","pos",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // work position group - {"","ofs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // work offset group - {"","hom",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // axis homing state group - {"","prb",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // probing state group - {"","pwr",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // motor power enagled group - {"","jog",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // axis jogging state group - {"","jid",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // job ID group - - {"","uda", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // user data group - {"","udb", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // user data group - {"","udc", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // user data group - {"","udd", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // user data group - -#ifdef __DIAGNOSTIC_PARAMETERS - {"","_te",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // target axis endpoint group - {"","_tr",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // target axis runtime group - {"","_ts",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // target motor steps group - {"","_ps",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // position motor steps group - {"","_cs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // commanded motor steps group - {"","_es",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // encoder steps group - {"","_xs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // correction steps group - {"","_fe",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // following error group -#endif - - // Uber-group (groups of groups, for text-mode displays only) - // *** Must agree with NV_COUNT_UBER_GROUPS below **** - {"", "m", _f0, 0, tx_print_nul, _do_motors, set_nul,(float *)&cs.null,0}, - {"", "q", _f0, 0, tx_print_nul, _do_axes, set_nul,(float *)&cs.null,0}, - {"", "o", _f0, 0, tx_print_nul, _do_offsets,set_nul,(float *)&cs.null,0}, - {"", "$", _f0, 0, tx_print_nul, _do_all, set_nul,(float *)&cs.null,0} -}; - - -// Make sure these defines line up with any changes in the above table -#define NV_COUNT_UBER_GROUPS 4 // count of uber-groups, above -#define STANDARD_GROUPS 33 // count of standard groups, excluding diagnostic parameter groups - -#if (MOTORS >= 5) -#define MOTOR_GROUP_5 1 -#else -#define MOTOR_GROUP_5 0 -#endif - -#if (MOTORS >= 6) -#define MOTOR_GROUP_6 1 -#else -#define MOTOR_GROUP_6 0 -#endif - -#ifdef __DIAGNOSTIC_PARAMETERS -#define DIAGNOSTIC_GROUPS 8 // count of diagnostic groups only -#else -#define DIAGNOSTIC_GROUPS 0 -#endif -#define NV_COUNT_GROUPS (STANDARD_GROUPS + MOTOR_GROUP_5 + MOTOR_GROUP_6 + DIAGNOSTIC_GROUPS) - -/* */ -#define NV_INDEX_MAX (sizeof cfgArray / sizeof(cfgItem_t)) -#define NV_INDEX_END_SINGLES (NV_INDEX_MAX - NV_COUNT_UBER_GROUPS - NV_COUNT_GROUPS - NV_STATUS_REPORT_LEN) -#define NV_INDEX_START_GROUPS (NV_INDEX_MAX - NV_COUNT_UBER_GROUPS - NV_COUNT_GROUPS) -#define NV_INDEX_START_UBER_GROUPS (NV_INDEX_MAX - NV_COUNT_UBER_GROUPS) -/* */ - -index_t nv_index_max() {return NV_INDEX_MAX;} -uint8_t nv_index_is_single(index_t index) {return (index <= NV_INDEX_END_SINGLES ? true : false);} - -uint8_t nv_index_is_group(index_t index) { - return ((index >= NV_INDEX_START_GROUPS && (index < NV_INDEX_START_UBER_GROUPS)) ? true : false); -} - -uint8_t nv_index_lt_groups(index_t index) {return (index <= NV_INDEX_START_GROUPS ? true : false);} - - -/* - * Set floating point number with G20/G21 units conversion - * - * The number 'setted' will have been delivered in external units (inches or mm). - * It is written to the target memory location in internal canonical units (mm). - * The original nv->value is also changed so persistence works correctly. - * Displays should convert back from internal canonical form to external form. - */ -stat_t set_flu(nvObj_t *nv) { - if (cm_get_units_mode(MODEL) == INCHES) // if in inches... - nv->value *= MM_PER_INCH; // convert to canonical millimeter units - - *((float *)GET_TABLE_WORD(target)) = nv->value; // write value as millimeters or degrees - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - - return STAT_OK; -} - - -/// Pre-process floating point number for units display -void preprocess_float(nvObj_t *nv) { - if (isnan((double)nv->value) || isinf((double)nv->value)) return; // illegal float values - - if (GET_TABLE_BYTE(flags) & F_CONVERT) { // unit conversion required? - if (cm_get_units_mode(MODEL) == INCHES) - nv->value *= INCHES_PER_MM; - } -} - -/**** TinyG UberGroup Operations **************************************************** - * Uber groups are groups of groups organized for convenience: - * - motors - group of all motor groups - * - axes - group of all axis groups - * - offsets - group of all offsets and stored positions - * - all - group of all groups - * - * _do_group_list() - get and print all groups in the list (iteration) - * _do_motors() - get and print motor uber group 1-N - * _do_axes() - get and print axis uber group XYZABC - * _do_offsets() - get and print offset uber group G54-G59, G28, G30, G92 - * _do_all() - get and print all groups uber group - */ -static stat_t _do_group_list(nvObj_t *nv, char list[][TOKEN_LEN + 1]) { // helper to print multiple groups in a list - for (uint8_t i = 0; i < NV_MAX_OBJECTS; i++) { - if (list[i][0] == 0) return STAT_COMPLETE; - - nv_reset_nv_list(); - nv = nv_body; - strncpy(nv->token, list[i], TOKEN_LEN); - nv->index = nv_get_index((const char_t *)"", nv->token); - nv_get_nvObj(nv); - nv_print_list(STAT_OK, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); - } - - return STAT_COMPLETE; -} - - -static stat_t _do_motors(nvObj_t *nv) { // print parameters for all motor groups -#if MOTORS == 2 - char list[][TOKEN_LEN+1] = {"1","2",""}; // must have a terminating element -#endif -#if MOTORS == 3 - char list[][TOKEN_LEN+1] = {"1","2","3",""}; // must have a terminating element -#endif -#if MOTORS == 4 - char list[][TOKEN_LEN+1] = {"1","2","3","4",""}; // must have a terminating element -#endif -#if MOTORS == 5 - char list[][TOKEN_LEN+1] = {"1","2","3","4","5",""}; // must have a terminating element -#endif -#if MOTORS == 6 - char list[][TOKEN_LEN+1] = {"1","2","3","4","5","6",""}; // must have a terminating element -#endif - return _do_group_list(nv, list); -} - - -static stat_t _do_axes(nvObj_t *nv) { // print parameters for all axis groups - char list[][TOKEN_LEN+1] = {"x","y","z","a","b","c",""}; // must have a terminating element - return _do_group_list(nv, list); -} - - -static stat_t _do_offsets(nvObj_t *nv) { // print offset parameters for G54-G59,G92, G28, G30 - char list[][TOKEN_LEN+1] = {"g54","g55","g56","g57","g58","g59","g92","g28","g30",""}; // must have a terminating element - return _do_group_list(nv, list); -} - - -static stat_t _do_all(nvObj_t *nv) { // print all parameters - strcpy(nv->token,"sys"); // print system group - get_grp(nv); - nv_print_list(STAT_OK, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); - - _do_motors(nv); // print all motor groups - _do_axes(nv); // print all axis groups - - strcpy(nv->token,"p1"); // print PWM group - get_grp(nv); - nv_print_list(STAT_OK, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); - - return _do_offsets(nv); // print all offsets -} - - -/**** COMMUNICATIONS FUNCTIONS ****************************************************** - * set_ec() - enable CRLF on TX - * set_ee() - enable character echo - * set_ex() - enable XON/XOFF or RTS/CTS flow control - * set_baud() - set baud rate - * get_rx() - get bytes available in RX buffer - */ -static stat_t _set_comm_helper(nvObj_t *nv, int cmd) { - usart_ctrl(cmd, fp_NOT_ZERO(nv->value)); - return STAT_OK; -} - - -static stat_t set_ec(nvObj_t *nv) { // expand CR to CRLF on TX - if (nv->value > true) return STAT_INPUT_VALUE_RANGE_ERROR; - cfg.enable_cr = (uint8_t)nv->value; - return _set_comm_helper(nv, USART_CRLF); -} - - -static stat_t set_ee(nvObj_t *nv) { // enable character echo - if (nv->value > true) return STAT_INPUT_VALUE_RANGE_ERROR; - cfg.enable_echo = (uint8_t)nv->value; - return _set_comm_helper(nv, USART_ECHO); -} - - -static stat_t set_ex(nvObj_t *nv) { // enable XON/XOFF or RTS/CTS flow control - if (nv->value > FLOW_CONTROL_RTS) return STAT_INPUT_VALUE_RANGE_ERROR; - cfg.enable_flow_control = (uint8_t)nv->value; - return _set_comm_helper(nv, USART_XOFF); -} - - -static stat_t get_rx(nvObj_t *nv) { - nv->value = (float)usart_rx_space(); - nv->valuetype = TYPE_INTEGER; - return STAT_OK; -} - - -/* - * set_baud() - set baud rate - * - * See usart.h for valid values. Works as a callback. - * The initial routine changes the baud config setting and sets a flag - * Then it posts a user message indicating the new baud rate - * Then it waits for the TX buffer to empty (so the message is sent) - * Then it performs the callback to apply the new baud rate - */ -static const char msg_baud0[] PROGMEM = "0"; -static const char msg_baud1[] PROGMEM = "9600"; -static const char msg_baud2[] PROGMEM = "19200"; -static const char msg_baud3[] PROGMEM = "38400"; -static const char msg_baud4[] PROGMEM = "57600"; -static const char msg_baud5[] PROGMEM = "115200"; -static const char msg_baud6[] PROGMEM = "230400"; -static const char *const msg_baud[] PROGMEM = {msg_baud0, msg_baud1, msg_baud2, msg_baud3, msg_baud4, msg_baud5, msg_baud6}; - - -static stat_t set_baud(nvObj_t *nv) { - uint8_t baud = (uint8_t)nv->value; - if ((baud < 1) || (baud > 6)) { - nv_add_conditional_message((const char_t *)"*** WARNING *** Unsupported baud rate specified"); - return STAT_INPUT_VALUE_RANGE_ERROR; - } - - cfg.baud_rate = baud; - cfg.baud_flag = true; - char_t message[NV_MESSAGE_LEN]; - sprintf_P(message, PSTR("*** NOTICE *** Resetting baud rate to %s"),GET_TEXT_ITEM(msg_baud, baud)); - nv_add_conditional_message(message); - - return STAT_OK; -} - - -stat_t set_baud_callback() { - if (cfg.baud_flag == false) return STAT_NOOP; - cfg.baud_flag = false; - usart_set_baud(cfg.baud_rate); - - return STAT_OK; -} - - -#ifdef __TEXT_MODE -static const char fmt_ec[] PROGMEM = "[ec] expand LF to CRLF on TX%6d [0=off,1=on]\n"; -static const char fmt_ee[] PROGMEM = "[ee] enable echo%18d [0=off,1=on]\n"; -static const char fmt_ex[] PROGMEM = "[ex] enable flow control%10d [0=off,1=XON/XOFF, 2=RTS/CTS]\n"; -static const char fmt_baud[] PROGMEM = "[baud] Baud rate%15d [1=9600,2=19200,3=38400,4=57600,5=115200,6=230400]\n"; -static const char fmt_rx[] PROGMEM = "rx:%d\n"; - -void cfg_print_ec(nvObj_t *nv) {text_print_ui8(nv, fmt_ec);} -void cfg_print_ee(nvObj_t *nv) {text_print_ui8(nv, fmt_ee);} -void cfg_print_ex(nvObj_t *nv) {text_print_ui8(nv, fmt_ex);} -void cfg_print_baud(nvObj_t *nv) {text_print_ui8(nv, fmt_baud);} -void cfg_print_rx(nvObj_t *nv) {text_print_ui8(nv, fmt_rx);} -#endif // __TEXT_MODE diff --git a/src/config_app.h b/src/config_app.h deleted file mode 100644 index 39ba52a..0000000 --- a/src/config_app.h +++ /dev/null @@ -1,83 +0,0 @@ -/* - * config_app.h - application-specific part of configuration sub-system - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2014 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2014 Robert Giseburt - * - * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#ifndef CONFIG_APP_H_ONCE -#define CONFIG_APP_H_ONCE - -enum nvType { // classification of commands - NV_TYPE_0 = 0, - NV_TYPE_CONFIG, // configuration commands - NV_TYPE_GCODE, // gcode - NV_TYPE_REPORT, // SR, QR and any other report - NV_TYPE_MESSAGE, // nv object carries a message - NV_TYPE_LINENUM // nv object carries a gcode line number -}; - - -typedef struct cfgParameters { // mostly communications variables - uint16_t magic_start; // magic number to test memory integrity - - // Communications settings - uint8_t comm_mode; // TG_TEXT_MODE or TG_JSON_MODE - uint8_t enable_cr; // enable CR in CRFL expansion on TX - uint8_t enable_echo; // enable text-mode echo - uint8_t enable_flow_control; // enable XON/XOFF or RTS/CTS flow control - - uint8_t baud_rate; // see usart.h for BAUD values - uint8_t baud_flag; // should be in controller singleton - - // User defined data groups - uint32_t user_data_a[4]; - uint32_t user_data_b[4]; - uint32_t user_data_c[4]; - uint32_t user_data_d[4]; - - uint16_t magic_end; -} cfgParameters_t; -extern cfgParameters_t cfg; - - -stat_t set_baud_callback(); - - -// Job config -void job_print_job(nvObj_t *nv); -stat_t job_get(nvObj_t *nv); -stat_t job_set(nvObj_t *nv); -uint8_t job_report_callback(); - - -#ifdef __TEXT_MODE -void cfg_print_ec(nvObj_t *nv); -void cfg_print_ee(nvObj_t *nv); -void cfg_print_ex(nvObj_t *nv); -void cfg_print_baud(nvObj_t *nv); -void cfg_print_net(nvObj_t *nv); -void cfg_print_rx(nvObj_t *nv); -#else -#define cfg_print_ec tx_print_stub -#define cfg_print_ee tx_print_stub -#define cfg_print_ex tx_print_stub -#define cfg_print_baud tx_print_stub -#define cfg_print_net tx_print_stub -#define cfg_print_rx tx_print_stub -#endif // __TEXT_MODE - -#endif // CONFIG_APP_H_ONCE diff --git a/src/controller.c b/src/controller.c index 9e40057..c45a32b 100644 --- a/src/controller.c +++ b/src/controller.c @@ -26,68 +26,38 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" // #1 -#include "config.h" // #2 #include "controller.h" -#include "json_parser.h" -#include "text_parser.h" -#include "gcode_parser.h" + #include "canonical_machine.h" -#include "plan_arc.h" -#include "planner.h" #include "stepper.h" - #include "gpio.h" #include "switch.h" #include "encoder.h" #include "hardware.h" -#include "report.h" -#include "help.h" -#include "util.h" #include "usart.h" +#include "util.h" +#include "rtc.h" +#include "command.h" +#include "report.h" + +#include "plan/arc.h" +#include "plan/planner.h" + +#include +#include controller_t cs; // controller state structure static stat_t _shutdown_idler(); -static stat_t _normal_idler(); static stat_t _limit_switch_handler(); -static stat_t _system_assertions(); static stat_t _sync_to_planner(); static stat_t _sync_to_tx_buffer(); -static stat_t _command_dispatch(); - - -// prep for export to other modules: -stat_t hardware_hard_reset_handler(); -stat_t hardware_bootloader_handler(); void controller_init() { memset(&cs, 0, sizeof(controller_t)); // clear all values, job_id's, pointers and status - controller_init_assertions(); - - cs.fw_build = TINYG_FIRMWARE_BUILD; - cs.fw_version = TINYG_FIRMWARE_VERSION; - cs.hw_platform = TINYG_HARDWARE_PLATFORM; // NB: HW version is set from EEPROM - - cs.state = CONTROLLER_STARTUP; // ready to run startup lines -} - - -/// Check memory integrity of controller -void controller_init_assertions() { - cs.magic_start = MAGICNUM; - cs.magic_end = MAGICNUM; -} - - -stat_t controller_test_assertions() { - if ((cs.magic_start != MAGICNUM) || (cs.magic_end != MAGICNUM)) - return STAT_CONTROLLER_ASSERTION_FAILURE; - - return STAT_OK; } @@ -122,13 +92,9 @@ void controller_run() { DISPATCH(_limit_switch_handler()); // 4. limit switch has been thrown DISPATCH(cm_feedhold_sequencing_callback()); // 5a. feedhold state machine runner DISPATCH(mp_plan_hold_callback()); // 5b. plan a feedhold from line runtime - DISPATCH(_system_assertions()); // 6. system integrity assertions // Planner hierarchy for gcode and cycles DISPATCH(st_motor_power_callback()); // stepper motor power sequencing - DISPATCH(sr_status_report_callback()); // conditionally send status report - DISPATCH(qr_queue_report_callback()); // conditionally send queue report - DISPATCH(rx_report_callback()); // conditionally send rx report DISPATCH(cm_arc_callback()); // arc generation runs behind lines DISPATCH(cm_homing_callback()); // G28.2 continuation DISPATCH(cm_jogging_callback()); // jog function @@ -136,90 +102,13 @@ void controller_run() { DISPATCH(cm_deferred_write_callback()); // persist G10 changes when not in machining cycle // Command readers and parsers - DISPATCH(_sync_to_planner()); // ensure there is at least one free buffer in planning queue + DISPATCH(_sync_to_planner()); // ensure at least one free buffer in planning queue DISPATCH(_sync_to_tx_buffer()); // sync with TX buffer (pseudo-blocking) - DISPATCH(set_baud_callback()); // perform baud rate update (must be after TX sync) - DISPATCH(_command_dispatch()); // read and execute next command - DISPATCH(_normal_idler()); -} - - -stat_t _command_dispatch() { - stat_t status; - - // read input line or return if not a completed line - // usart_gets() is a non-blocking workalike of fgets() - while (true) { - if ((status = usart_gets(cs.in_buf, sizeof(cs.in_buf))) == STAT_OK) { - cs.bufp = cs.in_buf; - break; - } - - return status; // Note: STAT_EAGAIN, errors, etc. will drop through - } - - return gc_gcode_parser(cs.in_buf); + DISPATCH(report_callback()); + DISPATCH(command_dispatch()); // read and execute next command } -/***************************************************************************** - * Dispatch line received from active input device - * - * Reads next command line and dispatches to relevant parser or action - * Accepts commands if the move queue has room - EAGAINS if it doesn't - * Also responsible for prompts and for flow control - */ -stat_t _command_dispatch_old() { - stat_t status; - - // read input line or return if not a completed line - // usart_gets() is a non-blocking workalike of fgets() - while (true) { - if ((status = usart_gets(cs.in_buf, sizeof(cs.in_buf))) == STAT_OK) { - cs.bufp = cs.in_buf; - break; - } - - return status; // Note: STAT_EAGAIN, errors, etc. will drop through - } - - // set up the buffers - cs.linelen = strlen(cs.in_buf) + 1; // linelen only tracks primary input - strncpy(cs.saved_buf, cs.bufp, SAVED_BUFFER_LEN - 1); // save input buffer for reporting - - // dispatch the new text line - switch (*cs.bufp) { // first char - case '!': cm_request_feedhold(); break; // include for diagnostics - case '%': cm_request_queue_flush(); break; - case '~': cm_request_cycle_start(); break; - - case 0: // blank line (just a CR) - if (cfg.comm_mode != JSON_MODE) - text_response(STAT_OK, cs.saved_buf); - break; - - case '$': case '?': case 'H': case 'h': // text mode input - cfg.comm_mode = TEXT_MODE; - text_response(text_parser(cs.bufp), cs.saved_buf); - break; - - case '{': // JSON input - cfg.comm_mode = JSON_MODE; - json_parser(cs.bufp); - break; - - default: // anything else must be Gcode - if (cfg.comm_mode == JSON_MODE) { // run it as JSON... - strncpy(cs.out_buf, cs.bufp, INPUT_BUFFER_LEN -8); // use out_buf as temp - sprintf((char *)cs.bufp,"{\"gc\":\"%s\"}\n", (char *)cs.out_buf); // '-8' is used for JSON chars - json_parser(cs.bufp); - - } else text_response(gc_gcode_parser(cs.bufp), cs.saved_buf); //...or run it as text - } - - return STAT_OK; -} - /// Blink rapidly and prevent further activity from occurring /// Shutdown idler flashes indicator LED rapidly to show everything is not OK. @@ -229,17 +118,12 @@ stat_t _command_dispatch_old() { static stat_t _shutdown_idler() { if (cm_get_machine_state() != MACHINE_SHUTDOWN) return STAT_OK; - if (SysTickTimer_getValue() > cs.led_timer) { - cs.led_timer = SysTickTimer_getValue() + LED_ALARM_TIMER; + if (rtc_get_time() > cs.led_timer) { + cs.led_timer = rtc_get_time() + LED_ALARM_TIMER; indicator_led_toggle(); } - return STAT_EAGAIN; // EAGAIN prevents any lower-priority actions from running -} - - -static stat_t _normal_idler() { - return STAT_OK; + return STAT_EAGAIN; // EAGAIN prevents any lower-priority actions from running } @@ -253,7 +137,8 @@ static stat_t _sync_to_tx_buffer() { /// Return eagain if planner is not ready for a new command static stat_t _sync_to_planner() { - if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) // allow up to N planner buffers for this line + // allow up to N planner buffers for this line + if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) return STAT_EAGAIN; return STAT_OK; @@ -267,18 +152,3 @@ static stat_t _limit_switch_handler() { return cm_hard_alarm(STAT_LIMIT_SWITCH_HIT); } - - -/// _system_assertions() - check memory integrity and other assertions -stat_t _system_assertions() { -#define system_assert(a) if ((status_code = a) != STAT_OK) return cm_hard_alarm(status_code); - - system_assert(config_test_assertions()); - system_assert(controller_test_assertions()); - system_assert(canonical_machine_test_assertions()); - system_assert(planner_test_assertions()); - system_assert(stepper_test_assertions()); - system_assert(encoder_test_assertions()); - - return STAT_OK; -} diff --git a/src/controller.h b/src/controller.h index 5d5421d..c100bcb 100644 --- a/src/controller.h +++ b/src/controller.h @@ -25,61 +25,27 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#ifndef CONTROLLER_H_ONCE -#define CONTROLLER_H_ONCE +#ifndef CONTROLLER_H +#define CONTROLLER_H -#define INPUT_BUFFER_LEN 255 // text buffer size (255 max) -#define SAVED_BUFFER_LEN 100 // saved buffer size (for reporting only) -#define OUTPUT_BUFFER_LEN 512 // text buffer size -// see also: tinyg.h MESSAGE_LEN and config.h NV_ lengths +#include "status.h" -#define LED_NORMAL_TIMER 1000 // blink rate for normal operation (in ms) -#define LED_ALARM_TIMER 100 // blink rate for alarm state (in ms) -typedef struct controllerSingleton { // main TG controller struct - magic_t magic_start; // magic number to test memory integrity - uint8_t state; // controller state - float null; // dumping ground for items with no target - float fw_build; // tinyg firmware build number - float fw_version; // tinyg firmware version number - float hw_platform; // tinyg hardware compatibility - platform type - float hw_version; // tinyg hardware compatibility - platform revision - - uint16_t linelen; // length of currently processing line - uint16_t read_index; // length of line being read - - // system state variables - uint8_t led_state; // LEGACY // 0=off, 1=on - int32_t led_counter; // LEGACY // a convenience for flashing an LED - uint32_t led_timer; // used by idlers to flash indicator LED - uint8_t hard_reset_requested; // flag to perform a hard reset - uint8_t bootloader_requested; // flag to enter the bootloader - uint8_t shared_buf_overrun; // flag for shared string buffer overrun condition +#define LED_NORMAL_TIMER 1000 // blink rate for normal operation (in ms) +#define LED_ALARM_TIMER 100 // blink rate for alarm state (in ms) - int32_t job_id[4]; // uuid to identify the job - // controller serial buffers - char_t *bufp; // pointer to primary or secondary in buffer - char_t in_buf[INPUT_BUFFER_LEN]; // primary input buffer - char_t out_buf[OUTPUT_BUFFER_LEN]; // output buffer - char_t saved_buf[SAVED_BUFFER_LEN]; // save the input buffer - magic_t magic_end; +typedef struct controllerSingleton { // main TG controller struct + // system state variables + uint32_t led_timer; // used by idlers to flash indicator LED + uint8_t hard_reset_requested; // flag to perform a hard reset + uint8_t bootloader_requested; // flag to enter the bootloader } controller_t; -extern controller_t cs; // controller state structure - -enum cmControllerState { // manages startup lines - CONTROLLER_INITIALIZING = 0, // controller is initializing - not ready for use - CONTROLLER_NOT_CONNECTED, // controller has not yet detected connection to comm channel - CONTROLLER_CONNECTED, // controller has connected to comm channel - CONTROLLER_STARTUP, // controller is running startup messages and lines - CONTROLLER_READY // controller is active and ready for use -}; +extern controller_t cs; // controller state structure void controller_init(); -void controller_init_assertions(); -stat_t controller_test_assertions(); void controller_run(); -#endif // End of include guard: CONTROLLER_H_ONCE +#endif // CONTROLLER_H diff --git a/src/cpp_magic.h b/src/cpp_magic.h new file mode 100644 index 0000000..d98a12f --- /dev/null +++ b/src/cpp_magic.h @@ -0,0 +1,471 @@ +/** + * This header file contains a library of advanced C Pre-Processor (CPP) macros + * which implement various useful functions, such as iteration, in the + * pre-processor. + * + * Though the file name (quite validly) labels this as magic, there should be + * enough documentation in the comments for a reader only casually familiar + * with the CPP to be able to understand how everything works. + * + * The majority of the magic tricks used in this file are based on those + * described by pfultz2 in his "Cloak" library: + * + * https://github.com/pfultz2/Cloak/wiki/C-Preprocessor-tricks,-tips,-and-idioms + * + * Major differences are a greater level of detailed explanation in this + * implementation and also a refusal to include any macros which require a O(N) + * macro definitions to handle O(N) arguments (with the exception of DEFERn). + */ + +#ifndef CPP_MAGIC_H +#define CPP_MAGIC_H + +/** + * Force the pre-processor to expand the macro a large number of times. Usage: + * + * EVAL(expression) + * + * This is useful when you have a macro which evaluates to a valid macro + * expression which is not subsequently expanded in the same pass. A contrived, + * but easy to understand, example of such a macro follows. Note that though + * this example is contrived, this behaviour is abused to implement bounded + * recursion in macros such as FOR. + * + * #define A(x) x+1 + * #define EMPTY + * #define NOT_QUITE_RIGHT(x) A EMPTY (x) + * NOT_QUITE_RIGHT(999) + * + * Here's what happens inside the C preprocessor: + * + * 1. It sees a macro "NOT_QUITE_RIGHT" and performs a single macro expansion + * pass on its arguments. Since the argument is "999" and this isn't a macro, + * this is a boring step resulting in no change. + * 2. The NOT_QUITE_RIGHT macro is substituted for its definition giving "A + * EMPTY() (x)". + * 3. The expander moves from left-to-right trying to expand the macro: + * The first token, A, cannot be expanded since there are no brackets + * immediately following it. The second token EMPTY(), however, can be + * expanded (recursively in this manner) and is replaced with "". + * 4. Expansion continues from the start of the substituted test (which in this + * case is just empty), and sees "(999)" but since no macro name is present, + * nothing is done. This results in a final expansion of "A (999)". + * + * Unfortunately, this doesn't quite meet expectations since you may expect that + * "A (999)" would have been expanded into "999+1". Unfortunately this requires + * a second expansion pass but luckily we can force the macro processor to make + * more passes by abusing the first step of macro expansion: the preprocessor + * expands arguments in their own pass. If we define a macro which does nothing + * except produce its arguments e.g.: + * + * #define PASS_THROUGH(...) __VA_ARGS__ + * + * We can now do "PASS_THROUGH(NOT_QUITE_RIGHT(999))" causing "NOT_QUITE_RIGHT" to be + * expanded to "A (999)", as described above, when the arguments are expanded. + * Now when the body of PASS_THROUGH is expanded, "A (999)" gets expanded to + * "999+1". + * + * The EVAL defined below is essentially equivalent to a large nesting of + * "PASS_THROUGH(PASS_THROUGH(PASS_THROUGH(..." which results in the + * preprocessor making a large number of expansion passes over the given + * expression. + */ +#define EVAL(...) EVAL1024(__VA_ARGS__) +#define EVAL1024(...) EVAL512(EVAL512(__VA_ARGS__)) +#define EVAL512(...) EVAL256(EVAL256(__VA_ARGS__)) +#define EVAL256(...) EVAL128(EVAL128(__VA_ARGS__)) +#define EVAL128(...) EVAL64(EVAL64(__VA_ARGS__)) +#define EVAL64(...) EVAL32(EVAL32(__VA_ARGS__)) +#define EVAL32(...) EVAL16(EVAL16(__VA_ARGS__)) +#define EVAL16(...) EVAL8(EVAL8(__VA_ARGS__)) +#define EVAL8(...) EVAL4(EVAL4(__VA_ARGS__)) +#define EVAL4(...) EVAL2(EVAL2(__VA_ARGS__)) +#define EVAL2(...) EVAL1(EVAL1(__VA_ARGS__)) +#define EVAL1(...) __VA_ARGS__ + + +/** + * Macros which expand to common values + */ +#define PASS(...) __VA_ARGS__ +#define EMPTY() +#define COMMA() , +#define SEMI() ; +#define PLUS() + +#define ZERO() 0 +#define ONE() 1 + +/** + * Causes a function-style macro to require an additional pass to be expanded. + * + * This is useful, for example, when trying to implement recursion since the + * recursive step must not be expanded in a single pass as the pre-processor + * will catch it and prevent it. + * + * Usage: + * + * DEFER1(IN_NEXT_PASS)(args, to, the, macro) + * + * How it works: + * + * 1. When DEFER1 is expanded, first its arguments are expanded which are + * simply IN_NEXT_PASS. Since this is a function-style macro and it has no + * arguments, nothing will happen. + * 2. The body of DEFER1 will now be expanded resulting in EMPTY() being + * deleted. This results in "IN_NEXT_PASS (args, to, the macro)". Note that + * since the macro expander has already passed IN_NEXT_PASS by the time it + * expands EMPTY() and so it won't spot that the brackets which remain can be + * applied to IN_NEXT_PASS. + * 3. At this point the macro expansion completes. If one more pass is made, + * IN_NEXT_PASS(args, to, the, macro) will be expanded as desired. + */ +#define DEFER1(id) id EMPTY() + +/** + * As with DEFER1 except here n additional passes are required for DEFERn. + * + * The mechanism is analogous. + * + * Note that there doesn't appear to be a way of combining DEFERn macros in + * order to achieve exponentially increasing defers e.g. as is done by EVAL. + */ +#define DEFER2(id) id EMPTY EMPTY()() +#define DEFER3(id) id EMPTY EMPTY EMPTY()()() +#define DEFER4(id) id EMPTY EMPTY EMPTY EMPTY()()()() +#define DEFER5(id) id EMPTY EMPTY EMPTY EMPTY EMPTY()()()()() +#define DEFER6(id) id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()() +#define DEFER7(id) id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()() +#define DEFER8(id) id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()()() + + +/** + * Indirection around the standard ## concatenation operator. This simply + * ensures that the arguments are expanded (once) before concatenation. + */ +#define CAT(a, ...) a ## __VA_ARGS__ +#define CAT3(a, b, ...) a ## b ## __VA_ARGS__ + + +/** + * Get the first argument and ignore the rest. + */ +#define FIRST(a, ...) a + +/** + * Get the second argument and ignore the rest. + */ +#define SECOND(a, b, ...) b + +/** + * Expects a single input (not containing commas). Returns 1 if the input is + * PROBE() and otherwise returns 0. + * + * This can be useful as the basis of a NOT function. + * + * This macro abuses the fact that PROBE() contains a comma while other valid + * inputs must not. + */ +#define IS_PROBE(...) SECOND(__VA_ARGS__, 0) +#define PROBE() ~, 1 + + +/** + * Logical negation. 0 is defined as false and everything else as true. + * + * When 0, _NOT_0 will be found which evaluates to the PROBE. When 1 (or any other + * value) is given, an appropriately named macro won't be found and the + * concatenated string will be produced. IS_PROBE then simply checks to see if + * the PROBE was returned, cleanly converting the argument into a 1 or 0. + */ +#define NOT(x) IS_PROBE(CAT(_NOT_, x)) +#define _NOT_0 PROBE() + +/** + * Macro version of C's famous "cast to bool" operator (i.e. !!) which takes + * anything and casts it to 0 if it is 0 and 1 otherwise. + */ +#define BOOL(x) NOT(NOT(x)) + +/** + * Logical OR. Simply performs a lookup. + */ +#define OR(a,b) CAT3(_OR_, a, b) +#define _OR_00 0 +#define _OR_01 1 +#define _OR_10 1 +#define _OR_11 1 + +/** + * Logical AND. Simply performs a lookup. + */ +#define AND(a,b) CAT3(_AND_, a, b) +#define _AND_00 0 +#define _AND_01 0 +#define _AND_10 0 +#define _AND_11 1 + + +/** + * Macro if statement. Usage: + * + * IF(c)(expansion when true) + * + * Here's how: + * + * 1. The preprocessor expands the arguments to _IF casting the condition to '0' + * or '1'. + * 2. The casted condition is concatencated with _IF_ giving _IF_0 or _IF_1. + * 3. The _IF_0 and _IF_1 macros either returns the argument or doesn't (e.g. + * they implement the "choice selection" part of the macro). + * 4. Note that the "true" clause is in the extra set of brackets; thus these + * become the arguments to _IF_0 or _IF_1 and thus a selection is made! + */ +#define IF(c) _IF(BOOL(c)) +#define _IF(c) CAT(_IF_,c) +#define _IF_0(...) +#define _IF_1(...) __VA_ARGS__ + +/** + * Macro if/else statement. Usage: + * + * IF_ELSE(c)( \ + * expansion when true, \ + * expansion when false \ + * ) + * + * The mechanism is analogous to IF. + */ +#define IF_ELSE(c) _IF_ELSE(BOOL(c)) +#define _IF_ELSE(c) CAT(_IF_ELSE_,c) +#define _IF_ELSE_0(t,f) f +#define _IF_ELSE_1(t,f) t + + +/** + * Macro which checks if it has any arguments. Returns '0' if there are no + * arguments, '1' otherwise. + * + * Limitation: HAS_ARGS(,1,2,3) returns 0 -- this check essentially only checks + * that the first argument exists. + * + * This macro works as follows: + * + * 1. _END_OF_ARGUMENTS_ is concatenated with the first argument. + * 2. If the first argument is not present then only "_END_OF_ARGUMENTS_" will + * remain, otherwise "_END_OF_ARGUMENTS something_here" will remain. + * 3. In the former case, the _END_OF_ARGUMENTS_() macro expands to a + * 0 when it is expanded. In the latter, a non-zero result remains. + * 4. BOOL is used to force non-zero results into 1 giving the clean 0 or 1 + * output required. + */ +#define HAS_ARGS(...) BOOL(FIRST(_END_OF_ARGUMENTS_ __VA_ARGS__)()) +#define _END_OF_ARGUMENTS_() 0 + + +/** + * Macro map/list comprehension. Usage: + * + * MAP(op, sep, ...) + * + * Produces a 'sep()'-separated list of the result of op(arg) for each arg. + * + * Example Usage: + * + * #define MAKE_HAPPY(x) happy_##x + * #define COMMA() , + * MAP(MAKE_HAPPY, COMMA, 1,2,3) + * + * Which expands to: + * + * happy_1 , happy_2 , happy_3 + * + * How it works: + * + * 1. The MAP macro simply maps the inner MAP_INNER function in an EVAL which + * forces it to be expanded a large number of times, thus enabling many steps + * of iteration (see step 6). + * 2. The MAP_INNER macro is substituted for its body. + * 3. In the body, op(cur_val) is substituted giving the value for this + * iteration. + * 4. The IF macro expands according to whether further iterations are required. + * This expansion either produces _IF_0 or _IF_1. + * 5. Since the IF is followed by a set of brackets containing the "if true" + * clause, these become the argument to _IF_0 or _IF_1. At this point, the + * macro in the brackets will be expanded giving the separator followed by + * _MAP_INNER EMPTY()()(op, sep, __VA_ARGS__). + * 5. If the IF was not taken, the above will simply be discarded and everything + * stops. If the IF is taken, The expression is then processed a second time + * yielding "_MAP_INNER()(op, sep, __VA_ARGS__)". Note that this call looks + * very similar to the essentially the same as the original call except the + * first argument has been dropped. + * 6. At this point expansion of MAP_INNER will terminate. However, since we can + * force more rounds of expansion using EVAL1. In the argument-expansion pass + * of the EVAL1, _MAP_INNER() is expanded to MAP_INNER which is then expanded + * using the arguments which follow it as in step 2-5. This is followed by a + * second expansion pass as the substitution of EVAL1() is expanded executing + * 2-5 a second time. This results in up to two iterations occurring. Using + * many nested EVAL1 macros, i.e. the very-deeply-nested EVAL macro, will in + * this manner produce further iterations, hence the outer MAP macro doing + * this for us. + * + * Important tricks used: + * + * * If we directly produce "MAP_INNER" in an expansion of MAP_INNER, a special + * case in the preprocessor will prevent it being expanded in the future, even + * if we EVAL. As a result, the MAP_INNER macro carefully only expands to + * something containing "_MAP_INNER()" which requires a further expansion step + * to invoke MAP_INNER and thus implementing the recursion. + * * To prevent _MAP_INNER being expanded within the macro we must first defer its + * expansion during its initial pass as an argument to _IF_0 or _IF_1. We must + * then defer its expansion a second time as part of the body of the _IF_0. As + * a result hence the DEFER2. + * * _MAP_INNER seemingly gets away with producing itself because it actually only + * produces MAP_INNER. It just happens that when _MAP_INNER() is expanded in + * this case it is followed by some arguments which get consumed by MAP_INNER + * and produce a _MAP_INNER. As such, the macro expander never marks + * _MAP_INNER as expanding to itself and thus it will still be expanded in + * future productions of itself. + */ +#define MAP(...) \ + IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_INNER(__VA_ARGS__))) +#define MAP_INNER(op,sep,cur_val, ...) \ + op(cur_val) \ + IF(HAS_ARGS(__VA_ARGS__))( \ + sep() DEFER2(_MAP_INNER)()(op, sep, ##__VA_ARGS__) \ + ) +#define _MAP_INNER() MAP_INNER + + +/** + * This is a variant of the MAP macro which also includes as an argument to the + * operation a valid C variable name which is different for each iteration. + * + * Usage: + * MAP_WITH_ID(op, sep, ...) + * + * Where op is a macro op(val, id) which takes a list value and an ID. This ID + * will simply be a unary number using the digit "I", that is, I, II, III, IIII, + * and so on. + * + * Example: + * + * #define MAKE_STATIC_VAR(type, name) static type name; + * MAP_WITH_ID(MAKE_STATIC_VAR, EMPTY, int, int, int, bool, char) + * + * Which expands to: + * + * static int I; static int II; static int III; static bool IIII; static char IIIII; + * + * The mechanism is analogous to the MAP macro. + */ +#define MAP_WITH_ID(op,sep,...) \ + IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_WITH_ID_INNER(op,sep,I, ##__VA_ARGS__))) +#define MAP_WITH_ID_INNER(op,sep,id,cur_val, ...) \ + op(cur_val,id) \ + IF(HAS_ARGS(__VA_ARGS__))( \ + sep() DEFER2(_MAP_WITH_ID_INNER)()(op, sep, CAT(id,I), ##__VA_ARGS__) \ + ) +#define _MAP_WITH_ID_INNER() MAP_WITH_ID_INNER + + +/** + * This is a variant of the MAP macro which iterates over pairs rather than + * singletons. + * + * Usage: + * MAP_PAIRS(op, sep, ...) + * + * Where op is a macro op(val_1, val_2) which takes two list values. + * + * Example: + * + * #define MAKE_STATIC_VAR(type, name) static type name; + * MAP_PAIRS(MAKE_STATIC_VAR, EMPTY, char, my_char, int, my_int) + * + * Which expands to: + * + * static char my_char; static int my_int; + * + * The mechanism is analogous to the MAP macro. + */ +#define MAP_PAIRS(op,sep,...) \ + IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_PAIRS_INNER(op,sep,__VA_ARGS__))) +#define MAP_PAIRS_INNER(op,sep,cur_val_1, cur_val_2, ...) \ + op(cur_val_1,cur_val_2) \ + IF(HAS_ARGS(__VA_ARGS__))( \ + sep() DEFER2(_MAP_PAIRS_INNER)()(op, sep, __VA_ARGS__) \ + ) +#define _MAP_PAIRS_INNER() MAP_PAIRS_INNER + +/** + * This is a variant of the MAP macro which iterates over a two-element sliding + * window. + * + * Usage: + * MAP_SLIDE(op, last_op, sep, ...) + * + * Where op is a macro op(val_1, val_2) which takes the two list values + * currently in the window. last_op is a macro taking a single value which is + * called for the last argument. + * + * Example: + * + * #define SIMON_SAYS_OP(simon, next) IF(NOT(simon()))(next) + * #define SIMON_SAYS_LAST_OP(val) last_but_not_least_##val + * #define SIMON_SAYS() 0 + * + * MAP_SLIDE(SIMON_SAYS_OP, SIMON_SAYS_LAST_OP, EMPTY, wiggle, SIMON_SAYS, dance, move, SIMON_SAYS, boogie, stop) + * + * Which expands to: + * + * dance boogie last_but_not_least_stop + * + * The mechanism is analogous to the MAP macro. + */ +#define MAP_SLIDE(op,last_op,sep,...) \ + IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_SLIDE_INNER(op,last_op,sep,__VA_ARGS__))) +#define MAP_SLIDE_INNER(op,last_op,sep,cur_val, ...) \ + IF(HAS_ARGS(__VA_ARGS__))(op(cur_val,FIRST(__VA_ARGS__))) \ + IF(NOT(HAS_ARGS(__VA_ARGS__)))(last_op(cur_val)) \ + IF(HAS_ARGS(__VA_ARGS__))( \ + sep() DEFER2(_MAP_SLIDE_INNER)()(op, last_op, sep, __VA_ARGS__) \ + ) +#define _MAP_SLIDE_INNER() MAP_SLIDE_INNER + + +/** + * Strip any excess commas from a set of arguments. + */ +#define REMOVE_TRAILING_COMMAS(...) \ + MAP(PASS, COMMA, __VA_ARGS__) + + +/** + * Evaluates an array of macros passing 1 argument + */ +#define EMAP1(...) \ + IF(HAS_ARGS(__VA_ARGS__))(EVAL(EMAP1_INNER(__VA_ARGS__))) + +#define EMAP1_INNER(ARG1, OP, ...) \ + _##OP(ARG1) \ + IF(HAS_ARGS(__VA_ARGS__)) \ + (DEFER2(_EMAP1_INNER)()(ARG1, ##__VA_ARGS__)) + +#define _EMAP1_INNER() EMAP1_INNER + + +/** + * Evaluates an array of macros passing 2 arguments + */ +#define EMAP2(...) \ + IF(HAS_ARGS(__VA_ARGS__))(EVAL(EMAP2_INNER(__VA_ARGS__))) + +#define EMAP2_INNER(ARG1, ARG2, OP, ...) \ + _##OP(ARG1, ARG2) \ + IF(HAS_ARGS(__VA_ARGS__)) \ + (DEFER2(_EMAP2_INNER)()(ARG1, ARG2, ##__VA_ARGS__)) + +#define _EMAP2_INNER() EMAP2_INNER + + +#endif diff --git a/src/cycle_homing.c b/src/cycle_homing.c index 0e601fc..3bca4a9 100644 --- a/src/cycle_homing.c +++ b/src/cycle_homing.c @@ -25,52 +25,53 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" -#include "util.h" -#include "config.h" -#include "json_parser.h" -#include "text_parser.h" #include "canonical_machine.h" -#include "planner.h" #include "switch.h" +#include "util.h" #include "report.h" -/**** Homing singleton structure ****/ - -struct hmHomingSingleton { // persistent homing runtime variables - // controls for homing cycle - int8_t axis; // axis currently being homed - uint8_t min_mode; // mode for min switch for this axis - uint8_t max_mode; // mode for max switch for this axis - - int8_t homing_switch; // homing switch for current axis (index into switch flag table) - int8_t limit_switch; // limit switch for current axis, or -1 if none - - uint8_t homing_closed; // 0=open, 1=closed - uint8_t limit_closed; // 0=open, 1=closed - uint8_t set_coordinates; // G28.4 flag. true = set coords to zero at the end of homing cycle - stat_t (*func)(int8_t axis); // binding for callback function state machine - - // per-axis parameters - float direction; // set to 1 for positive (max), -1 for negative (to min); - float search_travel; // signed distance to travel in search - float search_velocity; // search speed as positive number - float latch_velocity; // latch speed as positive number - float latch_backoff; // max distance to back off switch during latch phase - float zero_backoff; // distance to back off switch before setting zero - float max_clear_backoff; // maximum distance of switch clearing backoffs before erring out - - // state saved from gcode model - uint8_t saved_units_mode; // G20,G21 global setting - uint8_t saved_coord_system; // G54 - G59 setting - uint8_t saved_distance_mode; // G90,G91 global setting - uint8_t saved_feed_rate_mode; // G93,G94 global setting - float saved_feed_rate; // F setting - float saved_jerk; // saved and restored for each axis homed +#include "plan/planner.h" + +#include + +#include +#include +#include + + +struct hmHomingSingleton { // persistent homing runtime variables + // controls for homing cycle + int8_t axis; // axis currently being homed + uint8_t min_mode; // mode for min switch for this axis + uint8_t max_mode; // mode for max switch for this axis + + int8_t homing_switch; // homing switch for current axis (index into switch flag table) + int8_t limit_switch; // limit switch for current axis, or -1 if none + + uint8_t homing_closed; // 0=open, 1=closed + uint8_t limit_closed; // 0=open, 1=closed + uint8_t set_coordinates; // G28.4 flag. true = set coords to zero at the end of homing cycle + stat_t (*func)(int8_t axis); // binding for callback function state machine + + // per-axis parameters + float direction; // set to 1 for positive (max), -1 for negative (to min); + float search_travel; // signed distance to travel in search + float search_velocity; // search speed as positive number + float latch_velocity; // latch speed as positive number + float latch_backoff; // max distance to back off switch during latch phase + float zero_backoff; // distance to back off switch before setting zero + float max_clear_backoff; // maximum distance of switch clearing backoffs before erring out + + // state saved from gcode model + uint8_t saved_units_mode; // G20,G21 global setting + uint8_t saved_coord_system; // G54 - G59 setting + uint8_t saved_distance_mode; // G90,G91 global setting + uint8_t saved_feed_rate_mode; // G93,G94 global setting + float saved_feed_rate; // F setting + float saved_jerk; // saved and restored for each axis homed }; static struct hmHomingSingleton hm; -/**** NOTE: global prototypes and other .h info is located in canonical_machine.h ****/ static stat_t _set_homing_func(stat_t (*func)(int8_t axis)); static stat_t _homing_axis_start(int8_t axis); @@ -141,248 +142,244 @@ static int8_t _get_next_axis(int8_t axis); * to cm_isbusy() is about. */ -stat_t cm_homing_cycle_start() -{ - // save relevant non-axis parameters from Gcode model - hm.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); - hm.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); - hm.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); - hm.saved_feed_rate_mode = cm_get_feed_rate_mode(ACTIVE_MODEL); - hm.saved_feed_rate = cm_get_feed_rate(ACTIVE_MODEL); - - // set working values - cm_set_units_mode(MILLIMETERS); - cm_set_distance_mode(INCREMENTAL_MODE); - cm_set_coord_system(ABSOLUTE_COORDS); // homing is done in machine coordinates - cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); - hm.set_coordinates = true; - - hm.axis = -1; // set to retrieve initial axis - hm.func = _homing_axis_start; // bind initial processing function - cm.cycle_state = CYCLE_HOMING; - cm.homing_state = HOMING_NOT_HOMED; - - return STAT_OK; +stat_t cm_homing_cycle_start() { + // save relevant non-axis parameters from Gcode model + hm.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); + hm.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); + hm.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); + hm.saved_feed_rate_mode = cm_get_feed_rate_mode(ACTIVE_MODEL); + hm.saved_feed_rate = cm_get_feed_rate(ACTIVE_MODEL); + + // set working values + cm_set_units_mode(MILLIMETERS); + cm_set_distance_mode(INCREMENTAL_MODE); + cm_set_coord_system(ABSOLUTE_COORDS); // homing is done in machine coordinates + cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); + hm.set_coordinates = true; + + hm.axis = -1; // set to retrieve initial axis + hm.func = _homing_axis_start; // bind initial processing function + cm.cycle_state = CYCLE_HOMING; + cm.homing_state = HOMING_NOT_HOMED; + + return STAT_OK; } -stat_t cm_homing_cycle_start_no_set() -{ - cm_homing_cycle_start(); - hm.set_coordinates = false; // set flag to not update position variables at the end of the cycle - return STAT_OK; +stat_t cm_homing_cycle_start_no_set() { + cm_homing_cycle_start(); + hm.set_coordinates = false; // set flag to not update position variables at end of cycle + return STAT_OK; } /* Homing axis moves - these execute in sequence for each axis - * cm_homing_callback() - main loop callback for running the homing cycle - * _set_homing_func() - a convenience for setting the next dispatch vector and exiting - * _trigger_feedhold() - callback from switch closure to trigger a feedhold (convenience for casting) - * _bind_switch_settings() - setup switch for homing operation - * _restore_switch_settings() - return switch to normal operation - * _homing_axis_start() - get next axis, initialize variables, call the clear - * _homing_axis_clear() - initiate a clear to move off a switch that is thrown at the start - * _homing_axis_search() - fast search for switch, closes switch - * _homing_axis_latch() - slow reverse until switch opens again - * _homing_axis_final() - backoff from latch location to zero position - * _homing_axis_move() - helper that actually executes the above moves + * cm_homing_callback() - main loop callback for running the homing cycle + * _set_homing_func() - a convenience for setting the next dispatch vector and exiting + * _trigger_feedhold() - callback from switch closure to trigger feedhold + * (convenience for casting) + * _bind_switch_settings() - setup switch for homing operation + * _restore_switch_settings() - return switch to normal operation + * _homing_axis_start() - get next axis, initialize variables, call the clear + * _homing_axis_clear() - initiate a clear to move off a switch that is thrown at the start + * _homing_axis_search() - fast search for switch, closes switch + * _homing_axis_latch() - slow reverse until switch opens again + * _homing_axis_final() - backoff from latch location to zero position + * _homing_axis_move() - helper that actually executes the above moves */ -stat_t cm_homing_callback() -{ - if (cm.cycle_state != CYCLE_HOMING) { return STAT_NOOP;} // exit if not in a homing cycle - if (cm_get_runtime_busy() == true) { return STAT_EAGAIN;} // sync to planner move ends - return hm.func(hm.axis); // execute the current homing move +stat_t cm_homing_callback() { + if (cm.cycle_state != CYCLE_HOMING) return STAT_NOOP; // exit if not in a homing cycle + if (cm_get_runtime_busy() == true) return STAT_EAGAIN; // sync to planner move ends + return hm.func(hm.axis); // execute the current homing move } -static stat_t _set_homing_func(stat_t (*func)(int8_t axis)) -{ - hm.func = func; - return STAT_EAGAIN; + +static stat_t _set_homing_func(stat_t (*func)(int8_t axis)) { + hm.func = func; + return STAT_EAGAIN; } -static stat_t _homing_axis_start(int8_t 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 - cm.homing_state = HOMING_HOMED; - return _set_homing_func(_homing_finalize_exit); - } else if (axis == -2) { // -2 is error - return _homing_error_exit(-2, STAT_HOMING_ERROR_BAD_OR_NO_AXIS); - } - } - // clear the homed flag for axis so we'll be able to move w/o triggering soft limits - cm.homed[axis] = false; - - // trap axis mis-configurations - if (fp_ZERO(cm.a[axis].search_velocity)) return _homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY); - if (fp_ZERO(cm.a[axis].latch_velocity)) return _homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY); - if (cm.a[axis].latch_backoff < 0) return _homing_error_exit(axis, STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF); - - // calculate and test travel distance - float travel_distance = fabs(cm.a[axis].travel_max - cm.a[axis].travel_min) + cm.a[axis].latch_backoff; - if (fp_ZERO(travel_distance)) return _homing_error_exit(axis, STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL); - - // determine the switch setup and that config is OK - hm.min_mode = get_switch_mode(MIN_SWITCH(axis)); - hm.max_mode = get_switch_mode(MAX_SWITCH(axis)); - - if ( ((hm.min_mode & SW_HOMING_BIT) ^ (hm.max_mode & SW_HOMING_BIT)) == 0) { // one or the other must be homing - return _homing_error_exit(axis, STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION); // axis cannot be homed - } - hm.axis = axis; // persist the axis - hm.search_velocity = fabs(cm.a[axis].search_velocity); // search velocity is always positive - hm.latch_velocity = fabs(cm.a[axis].latch_velocity); // latch velocity is always positive - - // setup parameters homing to the minimum switch - if (hm.min_mode & SW_HOMING_BIT) { - hm.homing_switch = MIN_SWITCH(axis); // the min is the homing switch - hm.limit_switch = MAX_SWITCH(axis); // the max would be the limit switch - hm.search_travel = -travel_distance; // search travels in negative direction - hm.latch_backoff = cm.a[axis].latch_backoff; // latch travels in positive direction - hm.zero_backoff = cm.a[axis].zero_backoff; + +static stat_t _homing_axis_start(int8_t 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 + cm.homing_state = HOMING_HOMED; + return _set_homing_func(_homing_finalize_exit); + + } else if (axis == -2) // -2 is error + return _homing_error_exit(-2, STAT_HOMING_ERROR_BAD_OR_NO_AXIS); + } + + // clear the homed flag for axis so we'll be able to move w/o triggering soft limits + cm.homed[axis] = false; + + // trap axis mis-configurations + if (fp_ZERO(cm.a[axis].search_velocity)) + return _homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY); + if (fp_ZERO(cm.a[axis].latch_velocity)) + return _homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY); + if (cm.a[axis].latch_backoff < 0) + return _homing_error_exit(axis, STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF); + + // calculate and test travel distance + float travel_distance = + fabs(cm.a[axis].travel_max - cm.a[axis].travel_min) + cm.a[axis].latch_backoff; + if (fp_ZERO(travel_distance)) + return _homing_error_exit(axis, STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL); + + // determine the switch setup and that config is OK + hm.min_mode = get_switch_mode(MIN_SWITCH(axis)); + hm.max_mode = get_switch_mode(MAX_SWITCH(axis)); + + // one or the other must be homing + if (((hm.min_mode & SW_HOMING_BIT) ^ (hm.max_mode & SW_HOMING_BIT)) == 0) + return _homing_error_exit(axis, STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION); // axis cannot be homed + + hm.axis = axis; // persist the axis + hm.search_velocity = fabs(cm.a[axis].search_velocity); // search velocity is always positive + hm.latch_velocity = fabs(cm.a[axis].latch_velocity); // latch velocity is always positive + + // setup parameters homing to the minimum switch + if (hm.min_mode & SW_HOMING_BIT) { + hm.homing_switch = MIN_SWITCH(axis); // the min is the homing switch + hm.limit_switch = MAX_SWITCH(axis); // the max would be the limit switch + hm.search_travel = -travel_distance; // search travels in negative direction + hm.latch_backoff = cm.a[axis].latch_backoff; // latch travels in positive direction + hm.zero_backoff = cm.a[axis].zero_backoff; // setup parameters for positive travel (homing to the maximum switch) - } else { - hm.homing_switch = MAX_SWITCH(axis); // the max is the homing switch - hm.limit_switch = MIN_SWITCH(axis); // the min would be the limit switch - hm.search_travel = travel_distance; // search travels in positive direction - hm.latch_backoff = -cm.a[axis].latch_backoff; // latch travels in negative direction - hm.zero_backoff = -cm.a[axis].zero_backoff; - } - - // if homing is disabled for the axis then skip to the next axis - uint8_t sw_mode = get_switch_mode(hm.homing_switch); - if ((sw_mode != SW_MODE_HOMING) && (sw_mode != SW_MODE_HOMING_LIMIT)) - return _set_homing_func(_homing_axis_start); - - // disable the limit switch parameter if there is no limit switch - if (get_switch_mode(hm.limit_switch) == SW_MODE_DISABLED) hm.limit_switch = -1; - -// hm.saved_jerk = cm.a[axis].jerk_max; // save the max jerk value - hm.saved_jerk = cm_get_axis_jerk(axis); // save the max jerk value - return _set_homing_func(_homing_axis_clear); // start the clear + } else { + hm.homing_switch = MAX_SWITCH(axis); // the max is the homing switch + hm.limit_switch = MIN_SWITCH(axis); // the min would be the limit switch + hm.search_travel = travel_distance; // search travels in positive direction + hm.latch_backoff = -cm.a[axis].latch_backoff; // latch travels in negative direction + hm.zero_backoff = -cm.a[axis].zero_backoff; + } + + // if homing is disabled for the axis then skip to the next axis + uint8_t sw_mode = get_switch_mode(hm.homing_switch); + if ((sw_mode != SW_MODE_HOMING) && (sw_mode != SW_MODE_HOMING_LIMIT)) + return _set_homing_func(_homing_axis_start); + + // disable the limit switch parameter if there is no limit switch + if (get_switch_mode(hm.limit_switch) == SW_MODE_DISABLED) hm.limit_switch = -1; + + // hm.saved_jerk = cm.a[axis].jerk_max; // save the max jerk value + hm.saved_jerk = cm_get_axis_jerk(axis); // save the max jerk value + + return _set_homing_func(_homing_axis_clear); // start the clear } + // Handle an initial switch closure by backing off the closed switch // NOTE: Relies on independent switches per axis (not shared) -static stat_t _homing_axis_clear(int8_t axis) // first clear move -{ - if (sw.state[hm.homing_switch] == SW_CLOSED) { - _homing_axis_move(axis, hm.latch_backoff, hm.search_velocity); - } else if (sw.state[hm.limit_switch] == SW_CLOSED) { - _homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity); - } - return _set_homing_func(_homing_axis_search); +static stat_t _homing_axis_clear(int8_t axis) { // first clear move + if (sw.state[hm.homing_switch] == SW_CLOSED) + _homing_axis_move(axis, hm.latch_backoff, hm.search_velocity); + else if (sw.state[hm.limit_switch] == SW_CLOSED) + _homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity); + + return _set_homing_func(_homing_axis_search); } -static stat_t _homing_axis_search(int8_t axis) // start the search -{ - cm_set_axis_jerk(axis, cm.a[axis].jerk_homing); // use the homing jerk for search onward - _homing_axis_move(axis, hm.search_travel, hm.search_velocity); - return _set_homing_func(_homing_axis_latch); + +static stat_t _homing_axis_search(int8_t axis) { // start the search + cm_set_axis_jerk(axis, cm.a[axis].jerk_homing); // use the homing jerk for search onward + _homing_axis_move(axis, hm.search_travel, hm.search_velocity); + + return _set_homing_func(_homing_axis_latch); } -static stat_t _homing_axis_latch(int8_t axis) // latch to switch open -{ - // verify assumption that we arrived here because of homing switch closure - // rather than user-initiated feedhold or other disruption - if (sw.state[hm.homing_switch] != SW_CLOSED) - return _set_homing_func(_homing_abort); - _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity); - return _set_homing_func(_homing_axis_zero_backoff); + +static stat_t _homing_axis_latch(int8_t axis) { // latch to switch open + // verify assumption that we arrived here because of homing switch closure + // rather than user-initiated feedhold or other disruption + if (sw.state[hm.homing_switch] != SW_CLOSED) + return _set_homing_func(_homing_abort); + + _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity); + + return _set_homing_func(_homing_axis_zero_backoff); } -static stat_t _homing_axis_zero_backoff(int8_t axis) // backoff to zero position -{ - _homing_axis_move(axis, hm.zero_backoff, hm.search_velocity); - return _set_homing_func(_homing_axis_set_zero); + +static stat_t _homing_axis_zero_backoff(int8_t axis) { // backoff to zero position + _homing_axis_move(axis, hm.zero_backoff, hm.search_velocity); + return _set_homing_func(_homing_axis_set_zero); } -static stat_t _homing_axis_set_zero(int8_t axis) // set zero and finish up -{ - if (hm.set_coordinates != false) { - cm_set_position(axis, 0); - cm.homed[axis] = true; - } else { - // do not set axis if in G28.4 cycle - cm_set_position(axis, cm_get_work_position(RUNTIME, axis)); - } - cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value - return _set_homing_func(_homing_axis_start); +static stat_t _homing_axis_set_zero(int8_t axis) { // set zero and finish up + if (hm.set_coordinates != false) { + cm_set_position(axis, 0); + cm.homed[axis] = true; + + } else + // do not set axis if in G28.4 cycle + cm_set_position(axis, cm_get_work_position(RUNTIME, axis)); + + cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value + + return _set_homing_func(_homing_axis_start); } -static stat_t _homing_axis_move(int8_t axis, float target, float velocity) -{ - float vect[] = {0,0,0,0,0,0}; - float flags[] = {false, false, false, false, false, false}; - - vect[axis] = target; - flags[axis] = true; - cm.gm.feed_rate = velocity; - mp_flush_planner(); // don't use cm_request_queue_flush() here - cm_request_cycle_start(); - ritorno(cm_straight_feed(vect, flags)); - return STAT_EAGAIN; + +static stat_t _homing_axis_move(int8_t axis, float target, float velocity) { + float vect[] = {0,0,0,0,0,0}; + float flags[] = {false, false, false, false, false, false}; + + vect[axis] = target; + flags[axis] = true; + cm.gm.feed_rate = velocity; + mp_flush_planner(); // don't use cm_request_queue_flush() here + cm_request_cycle_start(); + ritorno(cm_straight_feed(vect, flags)); + + return STAT_EAGAIN; } -/* - * _homing_abort() - end homing cycle in progress - */ -static stat_t _homing_abort(int8_t axis) -{ - cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value - _homing_finalize_exit(axis); - sr_request_status_report(SR_TIMED_REQUEST); - return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED +/// End homing cycle in progress +static stat_t _homing_abort(int8_t axis) { + cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value + _homing_finalize_exit(axis); + report_request(); + + return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED } -/* - * _homing_error_exit() - */ -static stat_t _homing_error_exit(int8_t axis, stat_t status) -{ - // Generate the warning message. Since the error exit returns via the homing callback - // - and not the main controller - it requires its own display processing - nv_reset_nv_list(); - - if (axis == -2) { - nv_add_conditional_message((const char_t *)"Homing error - Bad or no axis(es) specified");; - } else { - char message[NV_MESSAGE_LEN]; - sprintf_P(message, PSTR("Homing error - %c axis settings misconfigured"), cm_get_axis_char(axis)); - nv_add_conditional_message((char_t *)message); - } - nv_print_list(STAT_HOMING_CYCLE_FAILED, TEXT_INLINE_VALUES, JSON_RESPONSE_FORMAT); - - _homing_finalize_exit(axis); - return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED +static stat_t _homing_error_exit(int8_t axis, stat_t status) { + // Generate the warning message. + if (axis == -2) fprintf_P(stderr, PSTR("Homing error - Bad or no axis(es) specified")); + else fprintf_P(stderr, PSTR("Homing error - %c axis settings misconfigured"), + cm_get_axis_char(axis)); + + _homing_finalize_exit(axis); + + return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED } -/* - * _homing_finalize_exit() - helper to finalize homing - */ -static stat_t _homing_finalize_exit(int8_t axis) // third part of return to home -{ - mp_flush_planner(); // should be stopped, but in case of switch closure. - // don't use cm_request_queue_flush() here - - cm_set_coord_system(hm.saved_coord_system); // restore to work coordinate system - cm_set_units_mode(hm.saved_units_mode); - cm_set_distance_mode(hm.saved_distance_mode); - cm_set_feed_rate_mode(hm.saved_feed_rate_mode); - cm.gm.feed_rate = hm.saved_feed_rate; - cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); - cm_cycle_end(); - cm.cycle_state = CYCLE_OFF; - return STAT_OK; +/// Helper to finalize homing +static stat_t _homing_finalize_exit(int8_t axis) { // third part of return to home + mp_flush_planner(); // should be stopped, but in case of switch closure + + cm_set_coord_system(hm.saved_coord_system); // restore to work coordinate system + cm_set_units_mode(hm.saved_units_mode); + cm_set_distance_mode(hm.saved_distance_mode); + cm_set_feed_rate_mode(hm.saved_feed_rate_mode); + cm.gm.feed_rate = hm.saved_feed_rate; + cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); + cm_cycle_end(); + cm.cycle_state = CYCLE_OFF; + + return STAT_OK; } + /* - * _get_next_axis() - return next axis in sequence based on axis in arg + * Return next axis in sequence based on axis in arg * * Accepts "axis" arg as the current axis; or -1 to retrieve the first axis * Returns next axis based on "axis" argument and if that axis is flagged for homing in the gf struct @@ -393,60 +390,66 @@ static stat_t _homing_finalize_exit(int8_t axis) // third part of ret * Isolating this function facilitates implementing more complex and * user-specified axis homing orders */ - -static int8_t _get_next_axis(int8_t axis) -{ +static int8_t _get_next_axis(int8_t axis) { #if (HOMING_AXES <= 4) - if (axis == -1) { // inelegant brute force solution - if (fp_TRUE(cm.gf.target[AXIS_Z])) return AXIS_Z; - if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X; - if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; - if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; - return -2; // error - } else if (axis == AXIS_Z) { - if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X; - if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; - if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; - } else if (axis == AXIS_X) { - if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; - if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; - } else if (axis == AXIS_Y) { - if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; - } - return -1; // done + if (axis == -1) { // inelegant brute force solution + if (fp_TRUE(cm.gf.target[AXIS_Z])) return AXIS_Z; + if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X; + if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; + if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; + return -2; // error -#else + } else if (axis == AXIS_Z) { + if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X; + if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; + if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; + + } else if (axis == AXIS_X) { + if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; + if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; - if (axis == -1) { - if (fp_TRUE(cm.gf.target[AXIS_Z])) return AXIS_Z; - if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X; - if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; - if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; - if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B; - if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; - return -2; // error - } else if (axis == AXIS_Z) { - if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X; - if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; - if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; - if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B; - if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; - } else if (axis == AXIS_X) { - if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; - if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; - if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B; - if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; - } else if (axis == AXIS_Y) { - if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; - if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B; - if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; - } else if (axis == AXIS_A) { - if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B; - if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; - } else if (axis == AXIS_B) { - if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; - } - return -1; // done + } else if (axis == AXIS_Y) { + if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; + } + return -1; // done + +#else + if (axis == -1) { + if (fp_TRUE(cm.gf.target[AXIS_Z])) return AXIS_Z; + if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X; + if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; + if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; + if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B; + if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; + return -2; // error + + } else if (axis == AXIS_Z) { + if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X; + if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; + if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; + if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B; + if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; + + } else if (axis == AXIS_X) { + if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; + if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; + if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B; + if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; + + } else if (axis == AXIS_Y) { + if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; + if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B; + if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; + + } else if (axis == AXIS_A) { + if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B; + if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; + + } else if (axis == AXIS_B) { + if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; + } + + return -1; // done #endif } diff --git a/src/cycle_jogging.c b/src/cycle_jogging.c index 3448e8d..f67312b 100644 --- a/src/cycle_jogging.c +++ b/src/cycle_jogging.c @@ -24,33 +24,34 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" -#include "config.h" -#include "json_parser.h" -#include "text_parser.h" #include "canonical_machine.h" -#include "planner.h" -#include "util.h" + +#include "plan/planner.h" + +#include +#include +#include +#include /**** Jogging singleton structure ****/ struct jmJoggingSingleton { // persistent jogging runtime variables - // controls for jogging cycle - int8_t axis; // axis currently being jogged - float dest_pos; // distance relative to start position to travel - float start_pos; - float velocity_start; // initial jog feed - float velocity_max; - - uint8_t (*func)(int8_t axis); // binding for callback function state machine - - // state saved from gcode model - float saved_feed_rate; // F setting - uint8_t saved_units_mode; // G20,G21 global setting - uint8_t saved_coord_system; // G54 - G59 setting - uint8_t saved_distance_mode; // G90,G91 global setting - uint8_t saved_feed_rate_mode; // G93,G94 global setting - float saved_jerk; // saved and restored for each axis homed + // controls for jogging cycle + int8_t axis; // axis currently being jogged + float dest_pos; // distance relative to start position to travel + float start_pos; + float velocity_start; // initial jog feed + float velocity_max; + + uint8_t (*func)(int8_t axis); // binding for callback function state machine + + // state saved from gcode model + float saved_feed_rate; // F setting + uint8_t saved_units_mode; // G20,G21 global setting + uint8_t saved_coord_system; // G54 - G59 setting + uint8_t saved_distance_mode; // G90,G91 global setting + uint8_t saved_feed_rate_mode; // G93,G94 global setting + float saved_jerk; // saved and restored for each axis homed }; static struct jmJoggingSingleton jog; @@ -86,31 +87,31 @@ static stat_t _jogging_finalize_exit(int8_t axis); stat_t cm_jogging_cycle_start(uint8_t axis) { - // save relevant non-axis parameters from Gcode model - jog.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); - jog.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); - jog.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); - jog.saved_feed_rate_mode = cm_get_feed_rate_mode(ACTIVE_MODEL); - jog.saved_feed_rate = cm_get_feed_rate(ACTIVE_MODEL); - jog.saved_jerk = cm_get_axis_jerk(axis); - - // set working values - cm_set_units_mode(MILLIMETERS); - cm_set_distance_mode(ABSOLUTE_MODE); - cm_set_coord_system(ABSOLUTE_COORDS); // jogging is done in machine coordinates - cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); - - jog.velocity_start = JOGGING_START_VELOCITY; // see canonical_machine.h for #define - jog.velocity_max = cm.a[axis].velocity_max; - - jog.start_pos = cm_get_absolute_position(RUNTIME, axis); - jog.dest_pos = cm_get_jogging_dest(); - - jog.axis = axis; - jog.func = _jogging_axis_start; // bind initial processing function - - cm.cycle_state = CYCLE_JOG; - return STAT_OK; + // save relevant non-axis parameters from Gcode model + jog.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); + jog.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); + jog.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); + jog.saved_feed_rate_mode = cm_get_feed_rate_mode(ACTIVE_MODEL); + jog.saved_feed_rate = cm_get_feed_rate(ACTIVE_MODEL); + jog.saved_jerk = cm_get_axis_jerk(axis); + + // set working values + cm_set_units_mode(MILLIMETERS); + cm_set_distance_mode(ABSOLUTE_MODE); + cm_set_coord_system(ABSOLUTE_COORDS); // jogging is done in machine coordinates + cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); + + jog.velocity_start = JOGGING_START_VELOCITY; // see canonical_machine.h for #define + jog.velocity_max = cm.a[axis].velocity_max; + + jog.start_pos = cm_get_absolute_position(RUNTIME, axis); + jog.dest_pos = cm_get_jogging_dest(); + + jog.axis = axis; + jog.func = _jogging_axis_start; // bind initial processing function + + cm.cycle_state = CYCLE_JOG; + return STAT_OK; } @@ -125,82 +126,73 @@ stat_t cm_jogging_cycle_start(uint8_t axis) stat_t cm_jogging_callback() { - if (cm.cycle_state != CYCLE_JOG) { return STAT_NOOP; } // exit if not in a jogging cycle - if (cm_get_runtime_busy() == true) { return STAT_EAGAIN; } // sync to planner move ends - return jog.func(jog.axis); // execute the current homing move + if (cm.cycle_state != CYCLE_JOG) { return STAT_NOOP; } // exit if not in a jogging cycle + if (cm_get_runtime_busy() == true) { return STAT_EAGAIN; } // sync to planner move ends + return jog.func(jog.axis); // execute the current homing move } static stat_t _set_jogging_func(stat_t (*func)(int8_t axis)) { - jog.func = func; - return STAT_EAGAIN; + jog.func = func; + return STAT_EAGAIN; } static stat_t _jogging_axis_start(int8_t axis) { - return _set_jogging_func(_jogging_axis_jog); // register the callback for the jog move + return _set_jogging_func(_jogging_axis_jog); // register the callback for the jog move } static stat_t _jogging_axis_jog(int8_t axis) // run the jog move { - float vect[] = {0,0,0,0,0,0}; - float flags[] = {false, false, false, false, false, false}; - flags[axis] = true; - - float velocity = jog.velocity_start; - float direction = jog.start_pos <= jog.dest_pos ? 1. : -1.; - float delta = abs(jog.dest_pos - jog.start_pos); - - cm.gm.feed_rate = velocity; - mp_flush_planner(); // don't use cm_request_queue_flush() here - cm_request_cycle_start(); - -#if 1 - float ramp_dist = 2.0; - float steps = 0.0; - float max_steps = 25; - float offset = 0.01; - while( delta>ramp_dist && offset < delta && steps < max_steps ) + float vect[] = {0,0,0,0,0,0}; + float flags[] = {false, false, false, false, false, false}; + flags[axis] = true; + + float velocity = jog.velocity_start; + float direction = jog.start_pos <= jog.dest_pos ? 1. : -1.; + float delta = abs(jog.dest_pos - jog.start_pos); + + cm.gm.feed_rate = velocity; + mp_flush_planner(); // don't use cm_request_queue_flush() here + cm_request_cycle_start(); + + float ramp_dist = 2.0; + float steps = 0.0; + float max_steps = 25; + float offset = 0.01; + while( delta>ramp_dist && offset < delta && steps < max_steps ) { - vect[axis] = jog.start_pos + offset * direction; - cm.gm.feed_rate = velocity; - ritorno(cm_straight_feed(vect, flags)); - - steps++; - float scale = pow(10.0, steps/max_steps) / 10.0; - velocity = jog.velocity_start + (jog.velocity_max - jog.velocity_start) * scale; - offset += ramp_dist * steps/max_steps; + vect[axis] = jog.start_pos + offset * direction; + cm.gm.feed_rate = velocity; + ritorno(cm_straight_feed(vect, flags)); + + steps++; + float scale = pow(10.0, steps/max_steps) / 10.0; + velocity = jog.velocity_start + (jog.velocity_max - jog.velocity_start) * scale; + offset += ramp_dist * steps/max_steps; } -#else - // use a really slow jerk so we ramp up speed - // FIXME: need asymmetric accel/deaccel jerk for this to work... -// cm.a[axis].jerk_max = 25; - cm_set_axis_jerk(axis, 25); - //cm.a[axis].jerk_accel = 10; - //cm.a[axis].jerk_deaccel = 900; -#endif - - // final move - cm.gm.feed_rate = jog.velocity_max; - vect[axis] = jog.dest_pos; - ritorno(cm_straight_feed(vect, flags)); - return _set_jogging_func(_jogging_finalize_exit); + + // final move + cm.gm.feed_rate = jog.velocity_max; + vect[axis] = jog.dest_pos; + ritorno(cm_straight_feed(vect, flags)); + return _set_jogging_func(_jogging_finalize_exit); } static stat_t _jogging_finalize_exit(int8_t axis) // finish a jog { - mp_flush_planner(); // FIXME: not sure what to do on exit - cm_set_axis_jerk(axis, jog.saved_jerk); - cm_set_coord_system(jog.saved_coord_system); // restore to work coordinate system - cm_set_units_mode(jog.saved_units_mode); - cm_set_distance_mode(jog.saved_distance_mode); - cm_set_feed_rate_mode(jog.saved_feed_rate_mode); - cm.gm.feed_rate = jog.saved_feed_rate; - cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); - cm_cycle_end(); - cm.cycle_state = CYCLE_OFF; - - printf("{\"jog\":0}\n"); - return STAT_OK; + mp_flush_planner(); // FIXME: not sure what to do on exit + cm_set_axis_jerk(axis, jog.saved_jerk); + cm_set_coord_system(jog.saved_coord_system); // restore to work coordinate system + cm_set_units_mode(jog.saved_units_mode); + cm_set_distance_mode(jog.saved_distance_mode); + cm_set_feed_rate_mode(jog.saved_feed_rate_mode); + cm.gm.feed_rate = jog.saved_feed_rate; + cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); + cm_cycle_end(); + cm.cycle_state = CYCLE_OFF; + + printf("{\"jog\":0}\n"); + return STAT_OK; } diff --git a/src/cycle_probing.c b/src/cycle_probing.c index ddc7ed8..32f1ade 100644 --- a/src/cycle_probing.c +++ b/src/cycle_probing.c @@ -24,38 +24,42 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" -#include "config.h" -#include "json_parser.h" -#include "text_parser.h" + #include "canonical_machine.h" #include "spindle.h" -#include "report.h" #include "switch.h" #include "util.h" -#include "planner.h" + +#include "plan/planner.h" + +#include + +#include +#include +#include +#include /**** Probe singleton structure ****/ #define MINIMUM_PROBE_TRAVEL 0.254 -struct pbProbingSingleton { // persistent probing runtime variables - stat_t (*func)(); // binding for callback function state machine +struct pbProbingSingleton { // persistent probing runtime variables + stat_t (*func)(); // binding for callback function state machine - // switch configuration - uint8_t probe_switch; // which switch should we check? - uint8_t saved_switch_type; // saved switch type NO/NC - uint8_t saved_switch_mode; // save the probe switch's original settings + // switch configuration + uint8_t probe_switch; // which switch should we check? + uint8_t saved_switch_type; // saved switch type NO/NC + uint8_t saved_switch_mode; // save the probe switch's original settings - // state saved from gcode model - uint8_t saved_distance_mode; // G90,G91 global setting - uint8_t saved_coord_system; // G54 - G59 setting - float saved_jerk[AXES]; // saved and restored for each axis + // state saved from gcode model + uint8_t saved_distance_mode; // G90,G91 global setting + uint8_t saved_coord_system; // G54 - G59 setting + float saved_jerk[AXES]; // saved and restored for each axis - // probe destination - float start_position[AXES]; - float target[AXES]; - float flags[AXES]; + // probe destination + float start_position[AXES]; + float target[AXES]; + float flags[AXES]; }; static struct pbProbingSingleton pb; @@ -74,8 +78,8 @@ static stat_t _probing_error_exit(int8_t axis); uint8_t _set_pb_func(uint8_t (*func)()) { - pb.func = func; - return STAT_EAGAIN; + pb.func = func; + return STAT_EAGAIN; } /**************************************************************************************** @@ -101,33 +105,33 @@ uint8_t _set_pb_func(uint8_t (*func)()) uint8_t cm_straight_probe(float target[], float flags[]) { - // trap zero feed rate condition - if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) { - return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - } - - // trap no axes specified - if (fp_NOT_ZERO(flags[AXIS_X]) && fp_NOT_ZERO(flags[AXIS_Y]) && fp_NOT_ZERO(flags[AXIS_Z])) - return STAT_GCODE_AXIS_IS_MISSING; - - // 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(cm.probe_results); // clear the old probe position. - // NOTE: relying on probe_result will not detect a probe to 0,0,0. - - cm.probe_state = PROBE_WAITING; // wait until planner queue empties before completing initialization - pb.func = _probing_init; // bind probing initialization function - return STAT_OK; + // trap zero feed rate condition + if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) { + return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; + } + + // trap no axes specified + if (fp_NOT_ZERO(flags[AXIS_X]) && fp_NOT_ZERO(flags[AXIS_Y]) && fp_NOT_ZERO(flags[AXIS_Z])) + return STAT_GCODE_AXIS_IS_MISSING; + + // 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(cm.probe_results); // clear the old probe position. + // NOTE: relying on probe_result will not detect a probe to 0,0,0. + + cm.probe_state = PROBE_WAITING; // wait until planner queue empties before completing initialization + pb.func = _probing_init; // bind probing initialization function + return STAT_OK; } uint8_t cm_probe_callback() { - if ((cm.cycle_state != CYCLE_PROBE) && (cm.probe_state != PROBE_WAITING)) { - return STAT_NOOP; // exit if not in a probe cycle or waiting for one - } - if (cm_get_runtime_busy() == true) { return STAT_EAGAIN;} // sync to planner move ends - return pb.func(); // execute the current homing move + if ((cm.cycle_state != CYCLE_PROBE) && (cm.probe_state != PROBE_WAITING)) { + return STAT_NOOP; // exit if not in a probe cycle or waiting for one + } + if (cm_get_runtime_busy() == true) { return STAT_EAGAIN;} // sync to planner move ends + return pb.func(); // execute the current homing move } /* @@ -141,51 +145,51 @@ uint8_t cm_probe_callback() static uint8_t _probing_init() { - // so optimistic... ;) - // 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. - cm.probe_state = PROBE_FAILED; - cm.cycle_state = CYCLE_PROBE; - - // initialize the axes - save the jerk settings & switch to the jerk_homing settings - for( uint8_t axis=0; axis +#include + + enEncoders_t en; void encoder_init() { memset(&en, 0, sizeof(en)); // clear all values, pointers and status - encoder_init_assertions(); -} - - -void encoder_init_assertions() { - en.magic_end = MAGICNUM; - en.magic_start = MAGICNUM; -} - - -stat_t encoder_test_assertions() { - if (en.magic_end != MAGICNUM) return STAT_ENCODER_ASSERTION_FAILURE; - if (en.magic_start != MAGICNUM) return STAT_ENCODER_ASSERTION_FAILURE; - - return STAT_OK; } diff --git a/src/encoder.h b/src/encoder.h index 6f0db6b..1d4bcd4 100644 --- a/src/encoder.h +++ b/src/encoder.h @@ -41,7 +41,7 @@ * You only know where the machine should be at known "targets", which are at the end of * each move section (end of head, body, and tail). You need to take encoder readings at * these points. This synchronization is taken care of by the Target, Position, Position_delayed - * sequence in plan_exec. Referring to ASCII art in stepper.h and reproduced here: + * sequence in exec. Referring to ASCII art in stepper.h and reproduced here: * * LOAD/STEP (~5000uSec) [L1][Segment1][L2][Segment2][L3][Segment3][L4][Segment4][Lb1][Segmentb1] * PREP (100 uSec) [P1] [P2] [P3] [P4] [Pb1] [Pb2] @@ -87,8 +87,12 @@ * not be worth the trouble). */ -#ifndef ENCODER_H_ONCE -#define ENCODER_H_ONCE +#ifndef ENCODER_H +#define ENCODER_H + +#include "config.h" + +#include // used to abstract the encoder code out of the stepper so it can be managed in one place #define SET_ENCODER_STEP_SIGN(m, s) en.en[m].step_sign = s; @@ -104,19 +108,14 @@ typedef struct enEncoder { // one real or virtual encoder per controlled motor typedef struct enEncoders { - magic_t magic_start; enEncoder_t en[MOTORS]; // runtime encoder structures - magic_t magic_end; } enEncoders_t; extern enEncoders_t en; void encoder_init(); -void encoder_init_assertions(); -stat_t encoder_test_assertions(); - void en_set_encoder_steps(uint8_t motor, float steps); float en_read_encoder(uint8_t motor); -#endif // ENCODER_H_ONCE +#endif // ENCODER_H diff --git a/src/gcode_parser.c b/src/gcode_parser.c index 9505c04..a93536f 100644 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -16,24 +16,31 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" // #1 -#include "config.h" // #2 -#include "controller.h" + #include "gcode_parser.h" + +#include "controller.h" #include "canonical_machine.h" #include "spindle.h" #include "util.h" +#include +#include +#include +#include +#include + + struct gcodeParserSingleton { // struct to manage globals uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block }; struct gcodeParserSingleton gp; // local helper functions and macros -static void _normalize_gcode_block(char_t *str, char_t **com, char_t **msg, uint8_t *block_delete_flag); +static void _normalize_gcode_block(char *str, char **com, char **msg, uint8_t *block_delete_flag); static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value); static stat_t _point(float value); static stat_t _validate_gcode_block(); -static stat_t _parse_gcode_block(char_t *line); // Parse the block into the GN/GF structs +static stat_t _parse_gcode_block(char *line); // Parse the block into the GN/GF structs static stat_t _execute_gcode_block(); // Execute the gcode block #define SET_MODAL(m,parm,val) ({cm.gn.parm=val; cm.gf.parm=1; gp.modals[m]+=1; break;}) @@ -43,11 +50,11 @@ static stat_t _execute_gcode_block(); // Execute the gcode block /// Parse a block (line) of gcode /// Top level of gcode parser. Normalizes block and looks for special cases -stat_t gc_gcode_parser(char_t *block) { - char_t *str = block; // gcode command or 0 string - char_t none = 0; - char_t *com = &none; // gcode comment or 0 string - char_t *msg = &none; // gcode message or 0 string +stat_t gc_gcode_parser(char *block) { + char *str = block; // gcode command or 0 string + char none = 0; + char *com = &none; // gcode comment or 0 string + char *msg = &none; // gcode message or 0 string uint8_t block_delete_flag; // don't process Gcode blocks if in alarmed state @@ -103,9 +110,9 @@ stat_t gc_gcode_parser(char_t *block) { * - block_delete_flag is set true if block delete encountered, false otherwise */ -static void _normalize_gcode_block(char_t *str, char_t **com, char_t **msg, uint8_t *block_delete_flag) { - char_t *rd = str; // read pointer - char_t *wr = str; // write pointer +static void _normalize_gcode_block(char *str, char **com, char **msg, uint8_t *block_delete_flag) { + char *rd = str; // read pointer + char *wr = str; // write pointer // mark block deletes if (*rd == '/') { *block_delete_flag = true; } @@ -116,7 +123,7 @@ static void _normalize_gcode_block(char_t *str, char_t **com, char_t **msg, uint if (*rd == 0) { *wr = 0; } else if ((*rd == '(') || (*rd == ';')) { *wr = 0; *com = rd+1; } else if ((isalnum((char)*rd)) || (strchr("-.", *rd))) { // all valid characters - *(wr++) = (char_t)toupper((char)*(rd)); + *(wr++) = (char)toupper((char)*(rd)); } } @@ -170,7 +177,7 @@ static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) { // get-value general case char *end; - *value = strtof(*pstr, &end); + *value = strtod(*pstr, &end); if (end == *pstr) return STAT_BAD_NUMBER_FORMAT; // more robust test then checking for value=0; *pstr = end; @@ -216,7 +223,7 @@ static stat_t _validate_gcode_block() { * A number of implicit things happen when the gn struct is zeroed: * - inverse feed rate mode is canceled - set back to units_per_minute mode */ -static stat_t _parse_gcode_block(char_t *buf) { +static stat_t _parse_gcode_block(char *buf) { char *pstr = (char *)buf; // persistent pointer into gcode block for parsing words char letter; // parsed letter, eg.g. G or X or Y float value = 0; // value parsed from letter (e.g. 2 for G2) @@ -487,16 +494,3 @@ static stat_t _execute_gcode_block() { return status; } - - -stat_t gc_get_gc(nvObj_t *nv) { - ritorno(nv_copy_string(nv, cs.saved_buf)); - nv->valuetype = TYPE_STRING; - - return STAT_OK; -} - - -stat_t gc_run_gc(nvObj_t *nv) { - return gc_gcode_parser(*nv->stringp); -} diff --git a/src/gcode_parser.h b/src/gcode_parser.h index 44c5b3f..de632d1 100644 --- a/src/gcode_parser.h +++ b/src/gcode_parser.h @@ -17,11 +17,11 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#ifndef GCODE_PARSER_H_ONCE -#define GCODE_PARSER_H_ONCE +#ifndef GCODE_PARSER_H +#define GCODE_PARSER_H -stat_t gc_gcode_parser(char_t *block); -stat_t gc_get_gc(nvObj_t *nv); -stat_t gc_run_gc(nvObj_t *nv); +#include "status.h" -#endif // GCODE_PARSER_H_ONCE +stat_t gc_gcode_parser(char *block); + +#endif // GCODE_PARSER_H diff --git a/src/gpio.c b/src/gpio.c index c48e4ec..431e6ae 100644 --- a/src/gpio.c +++ b/src/gpio.c @@ -47,32 +47,25 @@ * **** These bits CANNOT be used as 5v inputs **** */ -#include - -#include "tinyg.h" -#include "util.h" -#include "config.h" #include "controller.h" #include "hardware.h" #include "gpio.h" -#include "canonical_machine.h" + +#include void indicator_led_set() { gpio_led_on(INDICATOR_LED); - cs.led_state = 1; } void indicator_led_clear() { gpio_led_off(INDICATOR_LED); - cs.led_state = 0; } void indicator_led_toggle() { gpio_led_toggle(INDICATOR_LED); - cs.led_state = !cs.led_state; } diff --git a/src/gpio.h b/src/gpio.h index d679aa2..0311c26 100644 --- a/src/gpio.h +++ b/src/gpio.h @@ -25,8 +25,10 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#ifndef gpio_h -#define gpio_h +#ifndef GPIO_H +#define GPIO_H + +#include void indicator_led_set(); void indicator_led_clear(); @@ -41,4 +43,4 @@ void gpio_set_bit_on(uint8_t b); void gpio_set_bit_off(uint8_t b); void gpio_set_bit_toggle(uint8_t b); -#endif +#endif // GPIO_H diff --git a/src/hardware.c b/src/hardware.c index 730961c..50055a4 100644 --- a/src/hardware.c +++ b/src/hardware.c @@ -25,16 +25,16 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include // used for software reset - -#include "tinyg.h" // #1 -#include "config.h" // #2 #include "hardware.h" #include "switch.h" #include "controller.h" -#include "text_parser.h" -#include "xmega/xmega_init.h" -#include "xmega/xmega_rtc.h" +#include "clock.h" +#include "rtc.h" + +#include // defines PROGMEM and PSTR +#include // used for software reset + +#include /// Bind XMEGA ports to hardware @@ -58,7 +58,7 @@ static void _port_bindings() { /// Lowest level hardware init void hardware_init() { - xmega_init(); // set system clock + clock_init(); // set system clock _port_bindings(); rtc_init(); // real time counter } @@ -89,7 +89,7 @@ enum { }; -static void _get_id(char_t *id) { +void hw_get_id(char *id) { char printable[33] = "ABCDEFGHJKLMNPQRSTUVWXYZ23456789"; uint8_t i; @@ -139,48 +139,3 @@ stat_t hw_bootloader_handler() { return STAT_EAGAIN; // never gets here but keeps the compiler happy } - - -/// Get device ID (signature) -stat_t hw_get_id(nvObj_t *nv) { - char_t tmp[SYS_ID_LEN]; - _get_id(tmp); - nv->valuetype = TYPE_STRING; - ritorno(nv_copy_string(nv, tmp)); - return STAT_OK; -} - - -/// Invoke boot form the cfgArray -stat_t hw_run_boot(nvObj_t *nv) { - hw_request_bootloader(); - return STAT_OK; -} - - -/// Set hardware version number -stat_t hw_set_hv(nvObj_t *nv) { - if (nv->value > TINYG_HARDWARE_VERSION_MAX) - return STAT_INPUT_EXCEEDS_MAX_VALUE; - - set_flt(nv); // record the hardware version - _port_bindings(nv->value); // reset port bindings - switch_init(); // re-initialize the GPIO ports - return STAT_OK; -} - -#ifdef __TEXT_MODE - -static const char fmt_fb[] PROGMEM = "[fb] firmware build%18.2f\n"; -static const char fmt_fv[] PROGMEM = "[fv] firmware version%16.2f\n"; -static const char fmt_hp[] PROGMEM = "[hp] hardware platform%15.2f\n"; -static const char fmt_hv[] PROGMEM = "[hv] hardware version%16.2f\n"; -static const char fmt_id[] PROGMEM = "[id] TinyG ID%30s\n"; - -void hw_print_fb(nvObj_t *nv) {text_print_flt(nv, fmt_fb);} -void hw_print_fv(nvObj_t *nv) {text_print_flt(nv, fmt_fv);} -void hw_print_hp(nvObj_t *nv) {text_print_flt(nv, fmt_hp);} -void hw_print_hv(nvObj_t *nv) {text_print_flt(nv, fmt_hv);} -void hw_print_id(nvObj_t *nv) {text_print_str(nv, fmt_id);} - -#endif //__TEXT_MODE diff --git a/src/hardware.h b/src/hardware.h index c80a2ee..c05e2fd 100644 --- a/src/hardware.h +++ b/src/hardware.h @@ -46,21 +46,13 @@ * interrupt can release the sleep mode. */ -#ifndef HARDWARE_H_ONCE -#define HARDWARE_H_ONCE +#ifndef HARDWARE_H +#define HARDWARE_H -enum hwPlatform { - HM_PLATFORM_NONE = 0, - HW_PLATFORM_TINYG_XMEGA, // TinyG code base on Xmega boards. -}; - -#define HW_VERSION_TINYGV6 6 -#define HW_VERSION_TINYGV7 7 -#define HW_VERSION_TINYGV8 8 +#include "status.h" +#include "config.h" -#include "config.h" // needed for the stat_t typedef #include -#include "xmega/xmega_rtc.h" #define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) #define SYS_ID_LEN 12 // length of system ID string from sys_get_id() @@ -177,10 +169,10 @@ enum cfgPortBits { // motor control port bit positions #define EXEC_TIMER_ENABLE 1 // turn exec timer clock on (F_CPU = 32 Mhz) #define EXEC_TIMER_WGMODE 0 // normal mode (count to TOP and rollover) -#define TIMER_DDA_ISR_vect TCC0_OVF_vect // must agree with assignment in system.h -#define TIMER_DWELL_ISR_vect TCD0_OVF_vect // must agree with assignment in system.h -#define TIMER_LOAD_ISR_vect TCE0_OVF_vect // must agree with assignment in system.h -#define TIMER_EXEC_ISR_vect TCF0_OVF_vect // must agree with assignment in system.h +#define TIMER_DDA_ISR_vect TCC0_OVF_vect +#define TIMER_DWELL_ISR_vect TCD0_OVF_vect +#define TIMER_LOAD_ISR_vect TCE0_OVF_vect +#define TIMER_EXEC_ISR_vect TCF0_OVF_vect #define TIMER_OVFINTLVL_HI 3 // timer interrupt level (3=hi) #define TIMER_OVFINTLVL_MED 2 // timer interrupt level (2=med) @@ -215,30 +207,12 @@ typedef struct { hwSingleton_t hw; void hardware_init(); // master hardware init +void hw_get_id(char *id); void hw_request_hard_reset(); void hw_hard_reset(); stat_t hw_hard_reset_handler(); void hw_request_bootloader(); stat_t hw_bootloader_handler(); -stat_t hw_run_boot(nvObj_t *nv); - -stat_t hw_set_hv(nvObj_t *nv); -stat_t hw_get_id(nvObj_t *nv); - -#ifdef __TEXT_MODE -void hw_print_fb(nvObj_t *nv); -void hw_print_fv(nvObj_t *nv); -void hw_print_hp(nvObj_t *nv); -void hw_print_hv(nvObj_t *nv); -void hw_print_id(nvObj_t *nv); - -#else -#define hw_print_fb tx_print_stub -#define hw_print_fv tx_print_stub -#define hw_print_hp tx_print_stub -#define hw_print_hv tx_print_stub -#define hw_print_id tx_print_stub -#endif // __TEXT_MODE - -#endif // end of include guard: HARDWARE_H_ONCE + +#endif // HARDWARE_H diff --git a/src/help.c b/src/help.c deleted file mode 100644 index e82a5c7..0000000 --- a/src/help.c +++ /dev/null @@ -1,153 +0,0 @@ -/* - * help.h - collected help routines - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - * - * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include "tinyg.h" // #1 -#include "config.h" // #2 -#include "report.h" -#include "help.h" - - -stat_t help_stub(nvObj_t *nv) {return STAT_OK;} - -#ifdef __HELP_SCREENS - - -static void _status_report_advisory() { - fprintf_P(stderr, PSTR - ("\n" - "Note: TinyG generates automatic status reports by default\n" - "This can be disabled by entering $sv=0\n" - "See the wiki below for more details.\n")); -} - - -static void _postscript() { - fprintf_P(stderr, PSTR - ("\n" - "For detailed TinyG info see: https://github.com/synthetos/TinyG/wiki/\n" - "For the latest firmware see: https://github.com/synthetos/TinyG\n" - "Please log any issues at http://www.synthetos.com/forums\n" - "Have fun\n")); -} - - -/// Help invoked as h from the command line -uint8_t help_general(nvObj_t *nv) { - fprintf_P(stderr, PSTR("\n\n\n### TinyG Help ###\n")); - fprintf_P(stderr, PSTR - ("\n" - "These commands are active from the command line:\n" - " ^x Reset (control x) - software reset\n" - " ? Machine position and gcode model state\n" - " $ Show and set configuration settings\n" - " ! Feedhold - stop motion without losing position\n" - " ~ Cycle Start - restart from feedhold\n" - " h Show this help screen\n" - " $h Show configuration help screen\n" - " $test List self-tests\n" - " $test=N Run self-test N\n" - " $home=1 Run a homing cycle\n" - " $defa=1 Restore all settings to \"factory\" defaults\n")); - _status_report_advisory(); - _postscript(); - rpt_print_system_ready_message(); - return STAT_OK; -} - - -/// elp invoked as $h -stat_t help_config(nvObj_t *nv) { - fprintf_P(stderr, PSTR("\n\n\n### TinyG CONFIGURATION Help ###\n")); - fprintf_P(stderr, PSTR - ("These commands are active for configuration:\n" - " $sys Show system (general) settings\n" - " $1 Show motor 1 settings (or whatever motor you want 1,2,3,4)\n" - " $x Show X axis settings (or whatever axis you want x,y,z,a,b,c)\n" - " $m Show all motor settings\n" - " $q Show all axis settings\n" - " $o Show all offset settings\n" - " $$ Show all settings\n" - " $h Show this help screen\n\n")); - - fprintf_P(stderr, PSTR - ("Each $ command above also displays the token for each setting in [ ] brackets\n " - "To view settings enter a token:\n\n" - " $\n\n" - "For example $yfr to display the Y max feed rate\n\n" - "To update settings enter token equals value:\n\n" - " $=\n\n" - "For example $yfr=800 to set the Y max feed rate to 800 mm/minute\n" - "For configuration details see: https://github.com/synthetos/TinyG/wiki/TinyG-Configuration\n")); - _status_report_advisory(); - _postscript(); - - return STAT_OK; -} - - -/// Help invoked for tests -stat_t help_test(nvObj_t *nv) { - fprintf_P(stderr, PSTR("\n\n\n### TinyG SELF TEST Help ###\n")); - fprintf_P(stderr, PSTR - ("Invoke self test by entering $test=N where N is one of:\n" - " $test=1 smoke test\n" - " $test=2 homing test (you must trip homing switches)\n" - " $test=3 square test (a series of squares)\n" - " $test=4 arc test (some large circles)\n" - " $test=5 dwell test (moves spaced by 1 second dwells)\n" - " $test=6 feedhold test (enter ! and ~ to hold and restart, respectively)\n" - " $test=7 M codes test (M codes intermingled with moves)\n" - " $test=8 JSON test (motion test run using JSON commands)\n" - " $test=9 inverse time test\n" - " $test=10 rotary motion test\n" - " $test=11 small moves test\n" - " $test=12 slow moves test\n" - " $test=13 coordinate system offset test (G92, G54-G59)\n" - "\n" - "Tests assume a centered XY origin and at least 80mm clearance in all directions\n" - "Tests assume Z has at least 40mm posiitive clearance\n" - "Tests start with a G0 X0 Y0 Z0 move\n" - "Homing is the exception. No initial position or clearance is assumed\n")); - _postscript(); - - return STAT_OK; -} - - -/// Help invoked for defaults -stat_t help_defa(nvObj_t *nv) { - fprintf_P(stderr, PSTR("\n\n\n### TinyG RESTORE DEFAULTS Help ###\n")); - fprintf_P(stderr, PSTR - ("Enter $defa=1 to reset the system to the factory default values.\n" - "This will overwrite any changes you have made.\n")); - _postscript(); - - return STAT_OK; -} - - -stat_t help_boot_loader(nvObj_t *nv) { - fprintf_P(stderr, PSTR("\n\n\n### TinyG BOOT LOADER Help ###\n")); - fprintf_P(stderr, PSTR("Enter $boot=1 to enter the boot loader.\n")); - _postscript(); - - return STAT_OK; -} - -#endif // __HELP_SCREENS diff --git a/src/help.h b/src/help.h deleted file mode 100644 index 97eef5b..0000000 --- a/src/help.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * help.h - collected help and assorted display routines - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2013 Alden S. Hart, Jr. - * - * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#ifndef HELP_H_ONCE -#define HELP_H_ONCE - -#ifdef __HELP_SCREENS - -stat_t help_general(nvObj_t *nv); -stat_t help_config(nvObj_t *nv); -stat_t help_test(nvObj_t *nv); -stat_t help_defa(nvObj_t *nv); -stat_t help_boot_loader(nvObj_t *nv); - -#else - -stat_t help_stub(nvObj_t *nv); -#define help_general help_stub -#define help_config help_stub -#define help_test help_stub -#define help_defa help_stub -#define help_boot_loader help_stub - -#endif // __HELP_SCREENS - -#endif // HELP_H_ONCE diff --git a/src/json_parser.c b/src/json_parser.c deleted file mode 100644 index 2427000..0000000 --- a/src/json_parser.c +++ /dev/null @@ -1,597 +0,0 @@ -/* - * json_parser.c - JSON parser for TinyG - * This file is part of the TinyG project - * - * Copyright (c) 2011 - 2015 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include "tinyg.h" -#include "config.h" // JSON sits on top of the config system -#include "controller.h" -#include "json_parser.h" -#include "text_parser.h" -#include "canonical_machine.h" -#include "report.h" -#include "util.h" - -jsSingleton_t js; - -static stat_t _json_parser_kernal(char_t *str); -static stat_t _get_nv_pair(nvObj_t *nv, char_t **pstr, int8_t *depth); -static stat_t _normalize_json_string(char_t *str, uint16_t size); - -/**************************************************************************** - * json_parser() - exposed part of JSON parser - * _json_parser_kernal() - * _normalize_json_string() - * _get_nv_pair_strict() - * - * This is a dumbed down JSON parser to fit in limited memory with no malloc - * or practical way to do recursion ("depth" tracks parent/child levels). - * - * This function will parse the following forms up to the JSON_MAX limits: - * {"name":"value"} - * {"name":12345} - * {"name1":"value1", "n2":"v2", ... "nN":"vN"} - * {"parent_name":""} - * {"parent_name":{"name":"value"}} - * {"parent_name":{"name1":"value1", "n2":"v2", ... "nN":"vN"}} - * - * "value" can be a string, number, true, false, or null (2 types) - * - * Numbers - * - number values are not quoted and can start with a digit or -. - * - numbers cannot start with + or . (period) - * - exponentiated numbers are handled OK. - * - hexadecimal or other non-decimal number bases are not supported - * - * The parser: - * - extracts an array of one or more JSON object structs from the input string - * - once the array is built it executes the object(s) in order in the array - * - passes the executed array to the response handler to generate the response string - * - returns the status and the JSON response string - * - * Separation of concerns - * json_parser() is the only exposed part. It does parsing, display, and status reports. - * _get_nv_pair() only does parsing and syntax; no semantic validation or group handling - * _json_parser_kernal() does index validation and group handling and executes sets and gets - * in an application agnostic way. It should work for other apps than TinyG - */ - -void json_parser(char_t *str) -{ - stat_t status = _json_parser_kernal(str); - nv_print_list(status, TEXT_NO_PRINT, JSON_RESPONSE_FORMAT); - sr_request_status_report(SR_IMMEDIATE_REQUEST); // generate incremental status report to show any changes -} - -static stat_t _json_parser_kernal(char_t *str) -{ - stat_t status; - int8_t depth; - nvObj_t *nv = nv_reset_nv_list(); // get a fresh nvObj list - char_t group[GROUP_LEN+1] = {""}; // group identifier - starts as 0 - int8_t i = NV_BODY_LEN; - - ritorno(_normalize_json_string(str, JSON_OUTPUT_STRING_MAX)); // return if error - - // parse the JSON command into the nv body - do { - if (--i == 0) - return STAT_JSON_TOO_MANY_PAIRS; // length error - - // Use relaxed parser. Will read eitehr strict or relaxed mode. To use strict-only parser refer - // to build earlier than 407.03. Substitute _get_nv_pair_strict() for _get_nv_pair() - if ((status = _get_nv_pair(nv, &str, &depth)) > STAT_EAGAIN) { // erred out - return status; - } - // propagate the group from previous NV pair (if relevant) - if (group[0] != 0) { - strncpy(nv->group, group, GROUP_LEN); // copy the parent's group to this child - } - // validate the token and get the index - if ((nv->index = nv_get_index(nv->group, nv->token)) == NO_MATCH) { - return STAT_UNRECOGNIZED_NAME; - } - if ((nv_index_is_group(nv->index)) && (nv_group_is_prefixed(nv->token))) { - strncpy(group, nv->token, GROUP_LEN); // record the group ID - } - if ((nv = nv->nx) == 0) - return STAT_JSON_TOO_MANY_PAIRS; // Not supposed to encounter a 0 - } while (status != STAT_OK); // breaks when parsing is complete - - // execute the command - nv = nv_body; - if (nv->valuetype == TYPE_0){ // means GET the value - ritorno(nv_get(nv)); // ritorno returns w/status on any errors - } else { - if (cm.machine_state == MACHINE_ALARM) - return STAT_MACHINE_ALARMED; - ritorno(nv_set(nv)); // set value or call a function (e.g. gcode) - nv_persist(nv); - } - return STAT_OK; // only successful commands exit through this point -} - -/* - * _normalize_json_string - normalize a JSON string in place - * - * Validate string size limits, remove all whitespace and convert - * to lower case, with the exception of gcode comments - */ - -static stat_t _normalize_json_string(char_t *str, uint16_t size) -{ - char_t *wr; // write pointer - uint8_t in_comment = false; - - if (strlen(str) > size) - return STAT_INPUT_EXCEEDS_MAX_LENGTH; - - for (wr = str; *str != 0; str++) { - if (!in_comment) { // normal processing - if (*str == '(') in_comment = true; - if ((*str <= ' ') || (*str == 0x7f)) continue; // toss ctrls, WS & DEL - *wr++ = tolower(*str); - } else { // Gcode comment processing - if (*str == ')') in_comment = false; - *wr++ = *str; - } - } - *wr = 0; - return STAT_OK; -} - -/* - * _get_nv_pair() - get the next name-value pair w/relaxed JSON rules. Also parses strict JSON. - * - * Parse the next statement and populate the command object (nvObj). - * - * Leaves string pointer (str) on the first character following the object. - * Which is the character just past the ',' separator if it's a multi-valued - * object or the terminating 0 if single object or the last in a multi. - * - * Keeps track of tree depth and closing braces as much as it has to. - * If this were to be extended to track multiple parents or more than two - * levels deep it would have to track closing curlies - which it does not. - * - * ASSUMES INPUT STRING HAS FIRST BEEN NORMALIZED BY _normalize_json_string() - * - * If a group prefix is passed in it will be pre-pended to any name parsed - * to form a token string. For example, if "x" is provided as a group and - * "fr" is found in the name string the parser will search for "xfr" in the - * cfgArray. - */ -/* RELAXED RULES - * - * Quotes are accepted but not needed on names - * Quotes are required for string values - * - * See build 406.xx or earlier for strict JSON parser - deleted in 407.03 - */ - -#define MAX_PAD_CHARS 8 -#define MAX_NAME_CHARS 32 - -static stat_t _get_nv_pair(nvObj_t *nv, char_t **pstr, int8_t *depth) -{ - uint8_t i; - char_t *tmp; - char_t leaders[] = {"{,\""}; // open curly, quote and leading comma - char_t separators[] = {":\""}; // colon and quote - char_t terminators[] = {"},\""}; // close curly, comma and quote - char_t value[] = {"{\".-+"}; // open curly, quote, period, minus and plus - - nv_reset_nv(nv); // wipes the object and sets the depth - - // --- Process name part --- - // Find, terminate and set pointers for the name. Allow for leading and trailing name quotes. - char_t * name = *pstr; - for (i=0; true; i++, (*pstr)++) { - if (strchr(leaders, (int)**pstr) == 0) { // find leading character of name - name = (*pstr)++; - break; - } - if (i == MAX_PAD_CHARS) - return STAT_JSON_SYNTAX_ERROR; - } - - // Find the end of name, 0 terminate and copy token - for (i=0; true; i++, (*pstr)++) { - if (strchr(separators, (int)**pstr) != 0) { - *(*pstr)++ = 0; - strncpy(nv->token, name, TOKEN_LEN+1); // copy the string to the token - break; - } - if (i == MAX_NAME_CHARS) - return STAT_JSON_SYNTAX_ERROR; - } - - // --- Process value part --- (organized from most to least frequently encountered) - - // Find the start of the value part - for (i=0; true; i++, (*pstr)++) { - if (isalnum((int)**pstr)) break; - if (strchr(value, (int)**pstr) != 0) break; - if (i == MAX_PAD_CHARS) - return STAT_JSON_SYNTAX_ERROR; - } - - // nulls (gets) - if ((**pstr == 'n') || ((**pstr == '\"') && (*(*pstr+1) == '\"'))) { // process null value - nv->valuetype = TYPE_0; - nv->value = TYPE_0; - - // numbers - } else if (isdigit(**pstr) || (**pstr == '-')) {// value is a number - nv->value = (float)strtod(*pstr, &tmp); // tmp is the end pointer - if(tmp == *pstr) - return STAT_BAD_NUMBER_FORMAT; - nv->valuetype = TYPE_FLOAT; - - // object parent - } else if (**pstr == '{') { - nv->valuetype = TYPE_PARENT; -// *depth += 1; // nv_reset_nv() sets the next object's level so this is redundant - (*pstr)++; - return STAT_EAGAIN; // signal that there is more to parse - - // strings - } else if (**pstr == '\"') { // value is a string - (*pstr)++; - nv->valuetype = TYPE_STRING; - if ((tmp = strchr(*pstr, '\"')) == 0) - return STAT_JSON_SYNTAX_ERROR; // find the end of the string - *tmp = 0; - - // if string begins with 0x it might be data, needs to be at least 3 chars long - if( strlen(*pstr)>=3 && (*pstr)[0]=='0' && (*pstr)[1]=='x') - { - uint32_t *v = (uint32_t*)&nv->value; - *v = strtoul((const char *)*pstr, 0L, 0); - nv->valuetype = TYPE_DATA; - } else { - ritorno(nv_copy_string(nv, *pstr)); - } - - *pstr = ++tmp; - - // boolean true/false - } else if (**pstr == 't') { - nv->valuetype = TYPE_BOOL; - nv->value = true; - } else if (**pstr == 'f') { - nv->valuetype = TYPE_BOOL; - nv->value = false; - - // arrays - } else if (**pstr == '[') { - nv->valuetype = TYPE_ARRAY; - ritorno(nv_copy_string(nv, *pstr)); // copy array into string for error displays - return STAT_UNSUPPORTED_TYPE; // return error as the parser doesn't do input arrays yet - - // general error condition - } else { - return STAT_JSON_SYNTAX_ERROR; // ill-formed JSON - } - - // process comma separators and end curlies - if ((*pstr = strpbrk(*pstr, terminators)) == 0) { // advance to terminator or err out - return STAT_JSON_SYNTAX_ERROR; - } - if (**pstr == '}') { - *depth -= 1; // pop up a nesting level - (*pstr)++; // advance to comma or whatever follows - } - if (**pstr == ',') - return STAT_EAGAIN; // signal that there is more to parse - - (*pstr)++; - return STAT_OK; // signal that parsing is complete -} - -/**************************************************************************** - * json_serialize() - make a JSON object string from JSON object array - * - * *nv is a pointer to the first element in the nv list to serialize - * *out_buf is a pointer to the output string - usually what was the input string - * Returns the character count of the resulting string - * - * Operation: - * - The nvObj list is processed start to finish with no recursion - * - * - Assume the first object is depth 0 or greater (the opening curly) - * - * - Assume remaining depths have been set correctly; but might not achieve closure; - * e.g. list starts on 0, and ends on 3, in which case provide correct closing curlies - * - * - Assume there can be multiple, independent, non-contiguous JSON objects at a - * given depth value. These are processed correctly - e.g. 0,1,1,0,1,1,0,1,1 - * - * - The list must have a terminating nvObj where nv->nx == 0. - * The terminating object may or may not have data (empty or not empty). - * - * Returns: - * Returns length of string - * - * Desired behaviors: - * - Allow self-referential elements that would otherwise cause a recursive loop - * - Skip over empty objects (TYPE_EMPTY) - * - If a JSON object is empty represent it as {} - * --- OR --- - * - If a JSON object is empty omit the object altogether (no curlies) - */ - -#define BUFFER_MARGIN 8 // safety margin to avoid buffer overruns during footer checksum generation - -uint16_t json_serialize(nvObj_t *nv, char_t *out_buf, uint16_t size) -{ -#ifdef __SILENCE_JSON_RESPONSES - return 0; -#else - char_t *str = out_buf; - char_t *str_max = out_buf + size - BUFFER_MARGIN; - int8_t initial_depth = nv->depth; - int8_t prev_depth = 0; - uint8_t need_a_comma = false; - - *str++ = '{'; // write opening curly - - while (true) { - if (nv->valuetype != TYPE_EMPTY) { - if (need_a_comma) { *str++ = ',';} - need_a_comma = true; - if (js.json_syntax == JSON_SYNTAX_RELAXED) { // write name - str += sprintf((char *)str, "%s:", nv->token); - } else { - str += sprintf((char *)str, "\"%s\":", nv->token); - } - - // check for illegal float values - if (nv->valuetype == TYPE_FLOAT) { - if (isnan((double)nv->value) || isinf((double)nv->value)) { nv->value = 0;} - } - - // serialize output value - if (nv->valuetype == TYPE_0) { str += (char_t)sprintf((char *)str, "null");} // Note that that "" is NOT null. - else if (nv->valuetype == TYPE_INTEGER) { - str += (char_t)sprintf((char *)str, "%1.0f", (double)nv->value); - } - else if (nv->valuetype == TYPE_DATA) { - uint32_t *v = (uint32_t*)&nv->value; - str += (char_t)sprintf((char *)str, "\"0x%lx\"", *v); - } - else if (nv->valuetype == TYPE_STRING) { str += (char_t)sprintf((char *)str, "\"%s\"",(char *)*nv->stringp);} - else if (nv->valuetype == TYPE_ARRAY) { str += (char_t)sprintf((char *)str, "[%s]", (char *)*nv->stringp);} - else if (nv->valuetype == TYPE_FLOAT) { preprocess_float(nv); -// str += fntoa((char *)str, nv->value, nv->precision); - str += fntoa(str, nv->value, nv->precision); - } - else if (nv->valuetype == TYPE_BOOL) { - if (fp_FALSE(nv->value)) { str += sprintf((char *)str, "false");} - else { str += (char_t)sprintf((char *)str, "true"); } - } - if (nv->valuetype == TYPE_PARENT) { - *str++ = '{'; - need_a_comma = false; - } - } - if (str >= str_max) { return -1;} // signal buffer overrun - if ((nv = nv->nx) == 0) { break;} // end of the list - - while (nv->depth < prev_depth--) { // iterate the closing curlies - need_a_comma = true; - *str++ = '}'; - } - prev_depth = nv->depth; - } - - // closing curlies and NEWLINE - while (prev_depth-- > initial_depth) { *str++ = '}';} - str += sprintf((char *)str, "}\n"); // using sprintf for this last one ensures a 0 termination - if (str > out_buf + size) { return -1;} - return str - out_buf; -#endif -} - -/* - * json_print_object() - serialize and print the nvObj array directly (w/o header & footer) - * - * Ignores JSON verbosity settings and everything else - just serializes the list & prints - * Useful for reports and other simple output. - * Object list should be terminated by nv->nx == 0 - */ -void json_print_object(nvObj_t *nv) -{ -#ifdef __SILENCE_JSON_RESPONSES - return; -#endif - - json_serialize(nv, cs.out_buf, sizeof(cs.out_buf)); - fprintf(stderr, "%s", (char *)cs.out_buf); -} - -/* - * json_print_list() - command to select and produce a JSON formatted output - */ - -void json_print_list(stat_t status, uint8_t flags) -{ - switch (flags) { - case JSON_NO_PRINT: { break; } - case JSON_OBJECT_FORMAT: { json_print_object(nv_body); break; } - case JSON_RESPONSE_FORMAT: { json_print_response(status); break; } - } -} - -/* - * json_print_response() - JSON responses with headers, footers and observing JSON verbosity - * - * A footer is returned for every setting except $jv=0 - * - * JV_SILENT = 0, // no response is provided for any command - * JV_FOOTER, // responses contain footer only; no command echo, gcode blocks or messages - * JV_CONFIGS, // echo configs; gcode blocks are not echoed; messages are not echoed - * JV_MESSAGES, // echo configs; gcode messages only (if present); no block echo or line numbers - * JV_LINENUM, // echo configs; gcode blocks return messages and line numbers as present - * JV_VERBOSE // echos all configs and gcode blocks, line numbers and messages - * - * This gets a bit complicated. The first nvObj is the header, which must be set by reset_nv_list(). - * The first object in the body will always have the gcode block or config command in it, - * which you may or may not want to display. This is followed by zero or more displayable objects. - * Then if you want a gcode line number you add that here to the end. Finally, a footer goes - * on all the (non-silent) responses. - */ -#define MAX_TAIL_LEN 8 - -void json_print_response(uint8_t status) -{ -#ifdef __SILENCE_JSON_RESPONSES - return; -#endif - - if (js.json_verbosity == JV_SILENT) return; // silent responses - - // Body processing - nvObj_t *nv = nv_body; - if (status == STAT_JSON_SYNTAX_ERROR) { - nv_reset_nv_list(); - nv_add_string((const char_t *)"err", escape_string(cs.in_buf, cs.saved_buf)); - - } else if (cm.machine_state != MACHINE_INITIALIZING) { // always do full echo during startup - uint8_t nv_type; - do { - if ((nv_type = nv_get_type(nv)) == NV_TYPE_0) break; - - if (nv_type == NV_TYPE_GCODE) { - if (js.echo_json_gcode_block == false) { // kill command echo if not enabled - nv->valuetype = TYPE_EMPTY; - } - -//++++ } else if (nv_type == NV_TYPE_CONFIG) { // kill config echo if not enabled -//fix me if (js.echo_json_configs == false) { -// nv->valuetype = TYPE_EMPTY; -// } - - } else if (nv_type == NV_TYPE_MESSAGE) { // kill message echo if not enabled - if (js.echo_json_messages == false) { - nv->valuetype = TYPE_EMPTY; - } - - } else if (nv_type == NV_TYPE_LINENUM) { // kill line number echo if not enabled - if ((js.echo_json_linenum == false) || (fp_ZERO(nv->value))) { // do not report line# 0 - nv->valuetype = TYPE_EMPTY; - } - } - } while ((nv = nv->nx) != 0); - } - - // Footer processing - while(nv->valuetype != TYPE_EMPTY) { // find a free nvObj at end of the list... - if ((nv = nv->nx) == 0) { //...or hit the 0 and return w/o a footer - json_serialize(nv_header, cs.out_buf, sizeof(cs.out_buf)); - return; - } - } - char_t footer_string[NV_FOOTER_LEN]; - sprintf((char *)footer_string, "%d,%d,%d,0", FOOTER_REVISION, status, cs.linelen); - cs.linelen = 0; // reset linelen so it's only reported once - - nv_copy_string(nv, footer_string); // link string to nv object -// nv->depth = 0; // footer 'f' is a peer to response 'r' (hard wired to 0) - nv->depth = js.json_footer_depth; // 0=footer is peer to response 'r', 1=child of response 'r' - nv->valuetype = TYPE_ARRAY; - strcpy(nv->token, "f"); // terminate the list - nv->nx = 0; - - // do all this to avoid having to serialize it twice - int16_t strcount = json_serialize(nv_header, cs.out_buf, sizeof(cs.out_buf));// make JSON string w/o checksum - if (strcount < 0) { return;} // encountered an overrun during serialization - if (strcount > OUTPUT_BUFFER_LEN - MAX_TAIL_LEN) { return;} // would overrun during checksum generation - int16_t strcount2 = strcount; - char tail[MAX_TAIL_LEN]; - - while (cs.out_buf[strcount] != '0') { strcount--; } // find end of checksum - strcpy(tail, cs.out_buf + strcount + 1); // save the json termination - - while (cs.out_buf[strcount2] != ',') { strcount2--; }// find start of checksum - sprintf((char *)cs.out_buf + strcount2 + 1, "%d%s", compute_checksum(cs.out_buf, strcount2), tail); - fprintf(stderr, "%s", cs.out_buf); -} - -/*********************************************************************************** - * CONFIGURATION AND INTERFACE FUNCTIONS - * Functions to get and set variables from the cfgArray table - ***********************************************************************************/ - -/* - * json_set_jv() - */ - -stat_t json_set_jv(nvObj_t *nv) -{ - if (nv->value > JV_VERBOSE) - return STAT_INPUT_VALUE_RANGE_ERROR; - js.json_verbosity = nv->value; - - js.echo_json_footer = false; - js.echo_json_messages = false; - js.echo_json_configs = false; - js.echo_json_linenum = false; - js.echo_json_gcode_block = false; - - if (nv->value >= JV_FOOTER) { js.echo_json_footer = true;} - if (nv->value >= JV_MESSAGES) { js.echo_json_messages = true;} - if (nv->value >= JV_CONFIGS) { js.echo_json_configs = true;} - if (nv->value >= JV_LINENUM) { js.echo_json_linenum = true;} - if (nv->value >= JV_VERBOSE) { js.echo_json_gcode_block = true;} - - return STAT_OK; -} - - -/*********************************************************************************** - * TEXT MODE SUPPORT - * Functions to print variables from the cfgArray table - ***********************************************************************************/ - -#ifdef __TEXT_MODE - -/* - * js_print_ej() - * js_print_jv() - * js_print_j2() - * js_print_fs() - */ - -static const char fmt_ej[] PROGMEM = "[ej] enable json mode%13d [0=text,1=JSON]\n"; -static const char fmt_jv[] PROGMEM = "[jv] json verbosity%15d [0=silent,1=footer,2=messages,3=configs,4=linenum,5=verbose]\n"; -static const char fmt_js[] PROGMEM = "[js] json serialize style%9d [0=relaxed,1=strict]\n"; -static const char fmt_fs[] PROGMEM = "[fs] footer style%17d [0=new,1=old]\n"; - -void js_print_ej(nvObj_t *nv) { text_print_ui8(nv, fmt_ej);} -void js_print_jv(nvObj_t *nv) { text_print_ui8(nv, fmt_jv);} -void js_print_js(nvObj_t *nv) { text_print_ui8(nv, fmt_js);} -void js_print_fs(nvObj_t *nv) { text_print_ui8(nv, fmt_fs);} - -#endif // __TEXT_MODE diff --git a/src/json_parser.h b/src/json_parser.h deleted file mode 100644 index cc524b7..0000000 --- a/src/json_parser.h +++ /dev/null @@ -1,109 +0,0 @@ -/* - * json_parser.h - JSON parser and JSON support for TinyG - * This file is part of the TinyG project - * - * Copyright (c) 2011 - 2014 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#ifndef _JSON_PARSER_H_ONCE -#define _JSON_PARSER_H_ONCE - -/**** Configs, Definitions and Structures ****/ - -/* JSON array definitions / revisions */ -// for now there is only one JSON array in use - the footer -// if you add these make sure there are no collisions w/present or past numbers - -#define FOOTER_REVISION 1 - -#define JSON_OUTPUT_STRING_MAX (OUTPUT_BUFFER_LEN) - -enum jsonVerbosity { - JV_SILENT = 0, // no response is provided for any command - JV_FOOTER, // returns footer only (no command echo, gcode blocks or messages) - JV_MESSAGES, // returns footer, messages (exception and gcode messages) - JV_CONFIGS, // returns footer, messages, config commands - JV_LINENUM, // returns footer, messages, config commands, gcode line numbers if present - JV_VERBOSE // returns footer, messages, config commands, gcode blocks -}; - -enum jsonFormats { // json output print modes - JSON_NO_PRINT = 0, // don't print anything if you find yourself in JSON mode - JSON_OBJECT_FORMAT, // print just the body as a json object - JSON_RESPONSE_FORMAT // print the header/body/footer as a response object -}; - -enum jsonSyntaxMode { - JSON_SYNTAX_RELAXED = 0, // Does not require quotes on names - JSON_SYNTAX_STRICT // requires quotes on names -}; - -typedef struct jsSingleton { - - /*** config values (PUBLIC) ***/ - uint8_t json_verbosity; // see enum in this file for settings - uint8_t json_footer_depth; // 0=footer is peer to response 'r', 1=child of response 'r' -// uint8_t json_footer_style; // select footer style - uint8_t json_syntax; // 0=relaxed syntax, 1=strict syntax - - uint8_t echo_json_footer; // flags for JSON responses serialization - uint8_t echo_json_messages; - uint8_t echo_json_configs; - uint8_t echo_json_linenum; - uint8_t echo_json_gcode_block; - - /*** runtime values (PRIVATE) ***/ - -} jsSingleton_t; - -/**** Externs - See report.c for allocation ****/ - -extern jsSingleton_t js; - -/**** Function Prototypes ****/ - -void json_parser(char_t *str); -uint16_t json_serialize(nvObj_t *nv, char_t *out_buf, uint16_t size); -void json_print_object(nvObj_t *nv); -void json_print_response(uint8_t status); -void json_print_list(stat_t status, uint8_t flags); - -stat_t json_set_jv(nvObj_t *nv); - -#ifdef __TEXT_MODE - - void js_print_ej(nvObj_t *nv); - void js_print_jv(nvObj_t *nv); - void js_print_js(nvObj_t *nv); - void js_print_fs(nvObj_t *nv); - -#else - - #define js_print_ej tx_print_stub - #define js_print_jv tx_print_stub - #define js_print_js tx_print_stub - #define js_print_fs tx_print_stub - -#endif // __TEXT_MODE - -#endif // End of include guard: JSON_PARSER_H_ONCE diff --git a/src/main.c b/src/main.c index c2351ac..9793e76 100644 --- a/src/main.c +++ b/src/main.c @@ -18,24 +18,21 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" // #1 There are some dependencies -#include "config.h" // #2 #include "hardware.h" -#include "persistence.h" #include "controller.h" #include "canonical_machine.h" -#include "report.h" -#include "planner.h" #include "stepper.h" #include "encoder.h" #include "switch.h" -#include "test.h" #include "pwm.h" #include "usart.h" #include "tmc2660.h" +#include "plan/planner.h" #include -#include "xmega/xmega_interrupts.h" +#include + +#include static void init() { @@ -46,8 +43,6 @@ static void init() { // do these first hardware_init(); // system hardware setup - must be first - persistence_init(); // set up EEPROM or other NVM - must be second - rtc_init(); // real time counter usart_init(); // serial port // do these next @@ -58,19 +53,20 @@ static void init() { pwm_init(); // pulse width modulation drivers controller_init(); // must be first app init - config_init(); // config records from eeprom - must be next app init planner_init(); // motion planning subsystem canonical_machine_init(); // canonical machine - must follow config_init() - // now bring up the interrupts and get started - PMIC_SetVectorLocationToApplication(); // as opposed to boot ROM - PMIC_EnableHighLevel(); // all levels are used, so don't bother to abstract them - PMIC_EnableMediumLevel(); - PMIC_EnableLowLevel(); + // set vector location to application, as opposed to boot ROM + uint8_t temp = PMIC.CTRL & ~PMIC_IVSEL_bm; + CCP = CCP_IOREG_gc; + PMIC.CTRL = temp; + + // all levels are used, so don't bother to abstract them + PMIC.CTRL |= PMIC_HILVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm; sei(); // enable global interrupts - rpt_print_system_ready_message(); // (LAST) announce system is ready + fprintf_P(stderr, PSTR("TinyG firmware\n")); } @@ -78,7 +74,7 @@ int main() { init(); // main loop - while (true) controller_run(); // single pass through the controller + while (1) controller_run(); // single pass through the controller return 0; } diff --git a/src/messages.def b/src/messages.def new file mode 100644 index 0000000..db6a9fa --- /dev/null +++ b/src/messages.def @@ -0,0 +1,276 @@ +// Must keep these aligned with defs in status.h + +MSG(00, "OK") +MSG(01, "Error") +MSG(02, "Eagain") +MSG(03, "Noop") +MSG(04, "Complete") +MSG(05, "Terminated") +MSG(06, "Hard reset") +MSG(07, "End of line") +MSG(08, "End of file") +MSG(09, "File not open") + +MSG(10, "Max file size exceeded") +MSG(11, "No such device") +MSG(12, "Buffer empty") +MSG(13, "Buffer full") +MSG(14, "Buffer full - fatal") +MSG(15, "Initializing") +MSG(16, "Entering boot loader") +MSG(17, "Function is stubbed") +MSG(18, "18") +MSG(19, "19") + +MSG(20, "Internal error") +MSG(21, "Internal range error") +MSG(22, "Floating point error") +MSG(23, "Divide by zero") +MSG(24, "Invalid Address") +MSG(25, "Read-only address") +MSG(26, "Initialization failure") +MSG(27, "System alarm - shutting down") +MSG(28, "Failed to get planner buffer") +MSG(29, "Generic exception report") + +MSG(30, "Move time is infinite") +MSG(31, "Move time is NAN") +MSG(32, "Float is infinite") +MSG(33, "Float is NAN") +MSG(34, "Persistence error") +MSG(35, "Bad status report setting") +MSG(36, "36") +MSG(37, "37") +MSG(38, "38") +MSG(39, "39") + +MSG(40, "40") +MSG(41, "41") +MSG(42, "42") +MSG(43, "43") +MSG(44, "44") +MSG(45, "45") +MSG(46, "46") +MSG(47, "47") +MSG(48, "48") +MSG(49, "49") +MSG(50, "50") +MSG(51, "51") +MSG(52, "52") +MSG(53, "53") +MSG(54, "54") +MSG(55, "55") +MSG(56, "56") +MSG(57, "57") +MSG(58, "58") +MSG(59, "59") +MSG(60, "60") +MSG(61, "61") +MSG(62, "62") +MSG(63, "63") +MSG(64, "64") +MSG(65, "65") +MSG(66, "66") +MSG(67, "67") +MSG(68, "68") +MSG(69, "69") +MSG(70, "70") +MSG(71, "71") +MSG(72, "72") +MSG(73, "73") +MSG(74, "74") +MSG(75, "75") +MSG(76, "76") +MSG(77, "77") +MSG(78, "78") +MSG(79, "79") +MSG(80, "80") +MSG(81, "81") +MSG(82, "82") +MSG(83, "83") +MSG(84, "84") +MSG(85, "85") +MSG(86, "86") +MSG(87, "87") +MSG(88, "88") +MSG(89, "89") + +MSG(90, "Config sub-system assertion failure") +MSG(91, "IO sub-system assertion failure") +MSG(92, "Encoder assertion failure") +MSG(93, "Stepper assertion failure") +MSG(94, "Planner assertion failure") +MSG(95, "Canonical machine assertion failure") +MSG(96, "Controller assertion failure") +MSG(97, "Stack overflow detected") +MSG(98, "Memory fault detected") +MSG(99, "Generic assertion failure") + +MSG(100, "Unrecognized command or config name") +MSG(101, "Invalid or malformed command") +MSG(102, "Bad number format") +MSG(103, "Unsupported number or JSON type") +MSG(104, "Parameter is read-only") +MSG(105, "Parameter cannot be read") +MSG(106, "Command not accepted at this time") +MSG(107, "Input exceeds max length") +MSG(108, "Input less than minimum value") +MSG(109, "Input exceeds maximum value") + +MSG(110, "Input value range error") +MSG(111, "JSON syntax error") +MSG(112, "JSON input has too many pairs") +MSG(113, "JSON string too long") +MSG(114, "114") +MSG(115, "115") +MSG(116, "116") +MSG(117, "117") +MSG(118, "118") +MSG(119, "119") + +MSG(120, "120") +MSG(121, "121") +MSG(122, "122") +MSG(123, "123") +MSG(124, "124") +MSG(125, "125") +MSG(126, "126") +MSG(127, "127") +MSG(128, "128") +MSG(129, "129") + +MSG(130, "Generic Gcode input error") +MSG(131, "Gcode command unsupported") +MSG(132, "M code unsupported") +MSG(133, "Gcode modal group violation") +MSG(134, "Axis word missing") +MSG(135, "Axis cannot be present") +MSG(136, "Axis is invalid for this command") +MSG(137, "Axis is disabled") +MSG(138, "Axis target position is missing") +MSG(139, "Axis target position is invalid") + +MSG(140, "Selected plane is missing") +MSG(141, "Selected plane is invalid") +MSG(142, "Feedrate not specified") +MSG(143, "Inverse time mode cannot be used with this command") +MSG(144, "Rotary axes cannot be used with this command") +MSG(145, "G0 or G1 must be active for G53") +MSG(146, "Requested velocity exceeds limits") +MSG(147, "Cutter compensation cannot be enabled") +MSG(148, "Programmed point same as current point") +MSG(149, "Spindle speed below minimum") + +MSG(150, "Spindle speed exceeded maximum") +MSG(151, "Spindle S word is missing") +MSG(152, "Spindle S word is invalid") +MSG(153, "Spindle must be off for this command") +MSG(154, "Spindle must be turning for this command") +MSG(155, "Arc specification error") +MSG(156, "Arc specification error - missing axis(es)") +MSG(157, "Arc specification error - missing offset(s)") +// current longest message: 56 char +MSG(158, "Arc specification error - radius arc out of tolerance") +MSG(159, "Arc specification error - endpoint is starting point") + +MSG(160, "P word is missing") +MSG(161, "P word is invalid") +MSG(162, "P word is zero") +MSG(163, "P word is negative") +MSG(164, "P word is not an integer") +MSG(165, "P word is not a valid tool number") +MSG(166, "D word is missing") +MSG(167, "D word is invalid") +MSG(168, "E word is missing") +MSG(169, "E word is invalid") + +MSG(170, "H word is missing") +MSG(171, "H word is invalid") +MSG(172, "L word is missing") +MSG(173, "L word is invalid") +MSG(174, "Q word is missing") +MSG(175, "Q word is invalid") +MSG(176, "R word is missing") +MSG(177, "R word is invalid") +MSG(178, "T word is missing") +MSG(179, "T word is invalid") + +MSG(180, "180") +MSG(181, "181") +MSG(182, "182") +MSG(183, "183") +MSG(184, "184") +MSG(185, "185") +MSG(186, "186") +MSG(187, "187") +MSG(188, "188") +MSG(189, "189") + +MSG(190, "190") +MSG(191, "191") +MSG(192, "192") +MSG(193, "193") +MSG(194, "194") +MSG(195, "195") +MSG(196, "196") +MSG(197, "197") +MSG(198, "198") +MSG(199, "199") + +MSG(200, "Generic TinyG error") +MSG(201, "Move less than minimum length") +MSG(202, "Move less than minimum time") +MSG(203, "Machine is alarmed - Command not processed") +MSG(204, "Limit switch hit - Shutdown occurred") +MSG(205, "Trapezoid planner failed to converge") +MSG(206, "206") +MSG(207, "207") +MSG(208, "208") +MSG(209, "209") + +MSG(210, "210") +MSG(211, "211") +MSG(212, "212") +MSG(213, "213") +MSG(214, "214") +MSG(215, "215") +MSG(216, "216") +MSG(217, "217") +MSG(218, "218") +MSG(219, "219") + +MSG(220, "Soft limit exceeded") +MSG(221, "Soft limit exceeded - X min") +MSG(222, "Soft limit exceeded - X max") +MSG(223, "Soft limit exceeded - Y min") +MSG(224, "Soft limit exceeded - Y max") +MSG(225, "Soft limit exceeded - Z min") +MSG(226, "Soft limit exceeded - Z max") +MSG(227, "Soft limit exceeded - A min") +MSG(228, "Soft limit exceeded - A max") +MSG(229, "Soft limit exceeded - B min") +MSG(230, "Soft limit exceeded - B max") +MSG(231, "Soft limit exceeded - C min") +MSG(232, "Soft limit exceeded - C max") +MSG(233, "233") +MSG(234, "234") +MSG(235, "235") +MSG(236, "236") +MSG(237, "237") +MSG(238, "238") +MSG(239, "239") + +MSG(240, "Homing cycle failed") +MSG(241, "Homing Error - Bad or no axis specified") +MSG(242, "Homing Error - Search velocity is zero") +MSG(243, "Homing Error - Latch velocity is zero") +MSG(244, "Homing Error - Travel min & max are the same") +MSG(245, "Homing Error - Negative latch backoff") +MSG(246, "Homing Error - Homing switches misconfigured") +MSG(247, "247") +MSG(248, "248") +MSG(249, "249") + +MSG(250, "Probe cycle failed") +MSG(251, "Probe endpoint is starting point") +MSG(252, "Jogging cycle failed") diff --git a/src/persistence.c b/src/persistence.c deleted file mode 100644 index 9aebe47..0000000 --- a/src/persistence.c +++ /dev/null @@ -1,70 +0,0 @@ -/* - * persistence.c - persistence functions - * This file is part of the TinyG project - * - * Copyright (c) 2013 - 2015 Alden S. Hart Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -#include "tinyg.h" -#include "persistence.h" -#include "report.h" -#include "canonical_machine.h" -#include "util.h" -#include "xmega/xmega_eeprom.h" - -nvmSingleton_t nvm; - - -void persistence_init() { - nvm.base_addr = NVM_BASE_ADDR; - nvm.profile_base = 0; - return; -} - - -// It's the responsibility of the caller to make sure the index does not exceed range - -/// return value (as float) by index -stat_t read_persistent_value(nvObj_t *nv) { - nvm.address = nvm.profile_base + (nv->index * NVM_VALUE_LEN); - EEPROM_ReadBytes(nvm.address, nvm.byte_array, NVM_VALUE_LEN); - memcpy(&nv->value, &nvm.byte_array, NVM_VALUE_LEN); - return STAT_OK; -} - - -/// write to NVM by index, but only if the value has changed -stat_t write_persistent_value(nvObj_t *nv) { - if (cm.cycle_state != CYCLE_OFF) - return rpt_exception(STAT_FILE_NOT_OPEN); // can't write when machine is moving - - nvm.tmp_value = nv->value; - ritorno(read_persistent_value(nv)); - if ((isnan((double)nv->value)) || (isinf((double)nv->value)) || (fp_NE(nv->value, nvm.tmp_value))) { - memcpy(&nvm.byte_array, &nvm.tmp_value, NVM_VALUE_LEN); - nvm.address = nvm.profile_base + (nv->index * NVM_VALUE_LEN); - EEPROM_WriteBytes(nvm.address, nvm.byte_array, NVM_VALUE_LEN); - } - nv->value =nvm.tmp_value; // always restore value - - return STAT_OK; -} diff --git a/src/persistence.h b/src/persistence.h deleted file mode 100644 index ae45688..0000000 --- a/src/persistence.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - * persistence.h - persistence code - * This file is part of the TinyG project - * - * Copyright (c) 2013 - 2014 Alden S. Hart Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#ifndef PERSISTENCE_H_ONCE -#define PERSISTENCE_H_ONCE - -#include "config.h" // needed for nvObj_t definition - -#define NVM_VALUE_LEN 4 // NVM value length (float, fixed length) -#define NVM_BASE_ADDR 0x0000 // base address of usable NVM - -//**** persistence singleton **** - -typedef struct nvmSingleton { - uint16_t base_addr; // NVM base address - uint16_t profile_base; // NVM base address of current profile] - uint16_t address; - float tmp_value; - int8_t byte_array[NVM_VALUE_LEN]; -} nvmSingleton_t; - -//**** persistence function prototypes **** - -void persistence_init(); -stat_t read_persistent_value(nvObj_t *nv); -stat_t write_persistent_value(nvObj_t *nv); - -#endif // End of include guard: PERSISTENCE_H_ONCE diff --git a/src/plan/arc.c b/src/plan/arc.c new file mode 100644 index 0000000..07d82ba --- /dev/null +++ b/src/plan/arc.c @@ -0,0 +1,460 @@ +/* + * arc.c - arc planning and motion execution + * This file is part of the TinyG project + * + * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + * + * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* This module actually contains some parts that belong ion the canonical machine, + * and other parts that belong at the motion planner level, but the whole thing is + * treated as if it were part of the motion planner. + */ + +#include "arc.h" + +#include "planner.h" +#include "util.h" + +#include +#include +#include + +// Allocate arc planner singleton structure + +arc_t arc; + +// Local functions +static stat_t _compute_arc(); +static stat_t _compute_arc_offsets_from_radius(); +static void _estimate_arc_time(); + +/***************************************************************************** + * Canonical Machining arc functions (arc prep for planning and runtime) + * + * cm_arc_init() - initialize arcs + * cm_arc_feed() - canonical machine entry point for arc + * cm_arc_callback() - mail-loop callback for arc generation + * cm_abort_arc() - stop an arc in process + */ + +/// Initialize arc structures +void cm_arc_init() {} + +/* + * cm_arc_feed() - canonical machine entry point for arc + * + * Generates an arc by queuing line segments to the move buffer. The arc is + * approximated by generating a large number of tiny, linear arc_segments. + */ +stat_t cm_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 +{ + //////////////////////////////////////////////////// + // Set axis plane and trap arc specification errors + + // trap missing feed rate + if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) { + return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; + } + + // set radius mode flag and do simple test(s) + bool radius_f = fp_NOT_ZERO(cm.gf.arc_radius); // set true if radius arc + if ((radius_f) && (cm.gn.arc_radius < MIN_ARC_RADIUS)) { // radius value must be + and > minimum radius + return STAT_ARC_RADIUS_OUT_OF_TOLERANCE; + } + + // setup some flags + bool target_x = fp_NOT_ZERO(flags[AXIS_X]); // set true if X axis has been specified + bool target_y = fp_NOT_ZERO(flags[AXIS_Y]); + bool target_z = fp_NOT_ZERO(flags[AXIS_Z]); + + bool offset_i = fp_NOT_ZERO(cm.gf.arc_offset[0]); // set true if offset I has been specified + bool offset_j = fp_NOT_ZERO(cm.gf.arc_offset[1]); // J + bool offset_k = fp_NOT_ZERO(cm.gf.arc_offset[2]); // K + + // Set the arc plane for the current G17/G18/G19 setting and test arc specification + // Plane axis 0 and 1 are the arc plane, the linear axis is normal to the arc plane. + if (cm.gm.select_plane == CANON_PLANE_XY) { // G17 - the vast majority of arcs are in the G17 (XY) plane + arc.plane_axis_0 = AXIS_X; + arc.plane_axis_1 = AXIS_Y; + arc.linear_axis = AXIS_Z; + if (radius_f) { + if (!(target_x || target_y)) { // must have at least one endpoint specified + return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; + } + } else { // center format arc tests + if (offset_k) { // it's OK to be missing either or both i and j, but error if k is present + return STAT_ARC_SPECIFICATION_ERROR; + } + } + + } else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18 + arc.plane_axis_0 = AXIS_X; + arc.plane_axis_1 = AXIS_Z; + arc.linear_axis = AXIS_Y; + if (radius_f) { + if (!(target_x || target_z)) + return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; + } else { + if (offset_j) + return STAT_ARC_SPECIFICATION_ERROR; + } + + } else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19 + arc.plane_axis_0 = AXIS_Y; + arc.plane_axis_1 = AXIS_Z; + arc.linear_axis = AXIS_X; + if (radius_f) { + if (!(target_y || target_z)) + return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; + } else { + if (offset_i) + return STAT_ARC_SPECIFICATION_ERROR; + } + } + + // set values in the Gcode model state & copy it (linenum was already captured) + cm_set_model_target(target, flags); + + // in radius mode it's an error for start == end + if(radius_f) { + if ((fp_EQ(cm.gmx.position[AXIS_X], cm.gm.target[AXIS_X])) && + (fp_EQ(cm.gmx.position[AXIS_Y], cm.gm.target[AXIS_Y])) && + (fp_EQ(cm.gmx.position[AXIS_Z], cm.gm.target[AXIS_Z]))) { + return STAT_ARC_ENDPOINT_IS_STARTING_POINT; + } + } + + // now get down to the rest of the work setting up the arc for execution + cm.gm.motion_mode = motion_mode; + cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to gm + memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // copy GCode context to arc singleton - some will be overwritten to run segments + copy_vector(arc.position, cm.gmx.position); // set initial arc position from gcode model + + arc.radius = _to_millimeters(radius); // set arc radius or zero + + arc.offset[0] = _to_millimeters(i); // copy offsets with conversion to canonical form (mm) + arc.offset[1] = _to_millimeters(j); + arc.offset[2] = _to_millimeters(k); + + arc.rotations = floor(fabs(cm.gn.parameter)); // P must be a positive integer - force it if not + + // determine if this is a full circle arc. Evaluates true if no target is set + arc.full_circle = (fp_ZERO(flags[arc.plane_axis_0]) & fp_ZERO(flags[arc.plane_axis_1])); + + // compute arc runtime values + ritorno(_compute_arc()); + + if (fp_ZERO(arc.length)) { + return STAT_MINIMUM_LENGTH_MOVE; // trap zero length arcs that _compute_arc can throw + } + + cm_cycle_start(); // if not already started + arc.run_state = MOVE_RUN; // enable arc to be run from the callback + cm_finalize_move(); + return STAT_OK; +} + +/* + * cm_arc_callback() - generate an arc + * + * cm_arc_callback() is called from the controller main loop. Each time it's called it + * queues as many arc segments (lines) as it can before it blocks, then returns. + * + * Parts of this routine were originally sourced from the grbl project. + */ + +stat_t cm_arc_callback() +{ + if (arc.run_state == MOVE_OFF) + return STAT_NOOP; + + if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) + return STAT_EAGAIN; + + arc.theta += arc.arc_segment_theta; + arc.gm.target[arc.plane_axis_0] = arc.center_0 + sin(arc.theta) * arc.radius; + arc.gm.target[arc.plane_axis_1] = arc.center_1 + cos(arc.theta) * arc.radius; + arc.gm.target[arc.linear_axis] += arc.arc_segment_linear_travel; + mp_aline(&arc.gm); // run the line + copy_vector(arc.position, arc.gm.target); // update arc current position + + if (--arc.arc_segment_count > 0) + return STAT_EAGAIN; + arc.run_state = MOVE_OFF; + return STAT_OK; +} + +/* + * cm_abort_arc() - stop arc movement without maintaining position + * + * OK to call if no arc is running + */ + +void cm_abort_arc() +{ + arc.run_state = MOVE_OFF; +} + +/* + * _compute_arc() - compute arc from I and J (arc center point) + * + * The theta calculation sets up an clockwise or counterclockwise arc from the current + * position to the target position around the center designated by the offset vector. + * All theta-values measured in radians of deviance from the positive y-axis. + * + * | <- theta == 0 + * * * * + * * * + * * * + * * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2) + * * / + * C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4)) + * + * Parts of this routine were originally sourced from the grbl project. + */ + +static stat_t _compute_arc() +{ + // Compute radius. A non-zero radius value indicates a radius arc + if (fp_NOT_ZERO(arc.radius)) { // indicates a radius arc + _compute_arc_offsets_from_radius(); + } else { // compute start radius + arc.radius = hypotf(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]); + } + + // Test arc specification for correctness according to: + // http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc + // "It is an error if: when the arc is projected on the selected plane, the distance from + // the current point to the center differs from the distance from the end point to the + // center by more than (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)." + + // Compute end radius from the center of circle (offsets) to target endpoint + float end_0 = arc.gm.target[arc.plane_axis_0] - arc.position[arc.plane_axis_0] - arc.offset[arc.plane_axis_0]; + float end_1 = arc.gm.target[arc.plane_axis_1] - arc.position[arc.plane_axis_1] - arc.offset[arc.plane_axis_1]; + float err = fabs(hypotf(end_0, end_1) - arc.radius); // end radius - start radius + if ( (err > ARC_RADIUS_ERROR_MAX) || + ((err < ARC_RADIUS_ERROR_MIN) && + (err > arc.radius * ARC_RADIUS_TOLERANCE)) ) { + // return STAT_ARC_HAS_IMPOSSIBLE_CENTER_POINT; + return STAT_ARC_SPECIFICATION_ERROR; + } + + // Calculate the theta (angle) of the current point (position) + // arc.theta is angular starting point for the arc (also needed later for calculating center point) + arc.theta = atan2(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]); + + // g18_correction is used to invert G18 XZ plane arcs for proper CW orientation + float g18_correction = (cm.gm.select_plane == CANON_PLANE_XZ) ? -1 : 1; + + if (arc.full_circle) { // if full circle you can skip the stuff in the else clause + arc.angular_travel = 0; // angular travel always starts as zero for full circles + if (fp_ZERO(arc.rotations)) { // handle the valid case of a full circle arc w/P=0 + arc.rotations = 1.0; + } + } else { // ... it's not a full circle + arc.theta_end = atan2(end_0, end_1); + + // Compute the angular travel + if (fp_EQ(arc.theta_end, arc.theta)) { + arc.angular_travel = 0; // very large radii arcs can have zero angular travel (thanks PartKam) + } else { + if (arc.theta_end < arc.theta) { // make the difference positive so we have clockwise travel + arc.theta_end += (2*M_PI * g18_correction); + } + arc.angular_travel = arc.theta_end - arc.theta; // compute positive angular travel + if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) { // reverse travel direction if it's CCW arc + arc.angular_travel -= (2*M_PI * g18_correction); + } + } + } + + // Add in travel for rotations + if (cm.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); + } + + // 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 the helix (or just a planar arc) + arc.linear_travel = arc.gm.target[arc.linear_axis] - arc.position[arc.linear_axis]; + arc.planar_travel = arc.angular_travel * arc.radius; + arc.length = hypotf(arc.planar_travel, arc.linear_travel); // NB: hypot is insensitive to +/- signs + _estimate_arc_time(); // get an estimate of execution time to inform arc_segment calculation + + // Find the minimum number of arc_segments that meets these constraints... + float arc_segments_for_chordal_accuracy = arc.length / sqrt(4*cm.chordal_tolerance * (2 * arc.radius - cm.chordal_tolerance)); + float arc_segments_for_minimum_distance = arc.length / cm.arc_segment_len; + float arc_segments_for_minimum_time = arc.arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC; + + arc.arc_segments = floor(min3(arc_segments_for_chordal_accuracy, + arc_segments_for_minimum_distance, + arc_segments_for_minimum_time)); + + arc.arc_segments = max(arc.arc_segments, 1); //...but is at least 1 arc_segment + arc.gm.move_time = arc.arc_time / arc.arc_segments; // gcode state struct gets arc_segment_time, not arc time + arc.arc_segment_count = (int32_t)arc.arc_segments; + arc.arc_segment_theta = arc.angular_travel / arc.arc_segments; + arc.arc_segment_linear_travel = arc.linear_travel / arc.arc_segments; + arc.center_0 = arc.position[arc.plane_axis_0] - sin(arc.theta) * arc.radius; + arc.center_1 = arc.position[arc.plane_axis_1] - cos(arc.theta) * arc.radius; + arc.gm.target[arc.linear_axis] = arc.position[arc.linear_axis]; // initialize the linear target + return STAT_OK; +} + +/* + * _compute_arc_offsets_from_radius() - compute arc center (offset) from radius. + * + * Needs to calculate the center of the circle that has the designated radius and + * passes through both the current position and the target position + * + * This method calculates the following set of equations where: + * ` [x,y] is the vector from current to target position, + * d == magnitude of that vector, + * h == hypotenuse of the triangle formed by the radius of the circle, + * the distance to the center of the travel vector. + * + * A vector perpendicular to the travel vector [-y,x] is scaled to the length + * of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] + * to form the new point [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the + * center of the arc. + * + * d^2 == x^2 + y^2 + * h^2 == r^2 - (d/2)^2 + * i == x/2 - y/d*h + * j == y/2 + x/d*h + * O <- [i,j] + * - | + * r - | + * - | + * - | h + * - | + * [0,0] -> C -----------------+--------------- T <- [x,y] + * | <------ d/2 ---->| + * + * C - Current position + * T - Target position + * O - center of circle that pass through both C and T + * d - distance from C to T + * r - designated radius + * h - distance from center of CT to O + * + * Expanding the equations: + * d -> sqrt(x^2 + y^2) + * h -> sqrt(4 * r^2 - x^2 - y^2)/2 + * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + * + * Which can be written: + * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + * + * Which we for size and speed reasons optimize to: + * h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) + * i = (x - (y * h_x2_div_d))/2 + * j = (y + (x * h_x2_div_d))/2 + * + * ----Computing clockwise vs counter-clockwise motion ---- + * + * The counter clockwise circle lies to the left of the target direction. + * When offset is positive the left hand circle will be generated - + * when it is negative the right hand circle is generated. + * + * T <-- Target position + * + * ^ + * Clockwise circles with | Clockwise circles with + * this center will have | this center will have + * > 180 deg of angular travel | < 180 deg of angular travel, + * \ | which is a good thing! + * \ | / + * center of arc when -> x <----- | -----> x <- center of arc when + * h_x2_div_d is positive | h_x2_div_d is negative + * | + * C <-- Current position + * + * + * Assumes arc singleton has been pre-loaded with target and position. + * Parts of this routine were originally sourced from the grbl project. + */ +static stat_t _compute_arc_offsets_from_radius() +{ + // Calculate the change in position along each selected axis + float x = cm.gm.target[arc.plane_axis_0] - cm.gmx.position[arc.plane_axis_0]; + float y = cm.gm.target[arc.plane_axis_1] - cm.gmx.position[arc.plane_axis_1]; + + // *** From Forrest Green - Other Machine Co, 3/27/14 + // If the distance between endpoints is greater than the arc diameter, disc + // will be negative indicating that the arc is offset into the complex plane + // beyond the reach of any real CNC. However, numerical errors can flip the + // sign of disc as it approaches zero (which happens as the arc angle approaches + // 180 degrees). To avoid mishandling these arcs we use the closest real + // solution (which will be 0 when disc <= 0). This risks obscuring g-code errors + // where the radius is actually too small (they will be treated as half circles), + // but ensures that all valid arcs end up reasonably close to their intended + // paths regardless of any numerical issues. + float disc = 4 * square(arc.radius) - (square(x) + square(y)); + + // h_x2_div_d == -(h * 2 / d) + 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) + if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d;} + + // Negative R is g-code-alese for "I want a circle with more than 180 degrees + // of travel" (go figure!), even though it is advised against ever generating + // such circles in a single line of g-code. By inverting the sign of + // h_x2_div_d the center of the circles is placed on the opposite side of + // the line of travel and thus we get the unadvisably long arcs as prescribed. + if (arc.radius < 0) { h_x2_div_d = -h_x2_div_d; } + + // Complete the operation by calculating the actual center of the arc + arc.offset[arc.plane_axis_0] = (x-(y*h_x2_div_d))/2; + arc.offset[arc.plane_axis_1] = (y+(x*h_x2_div_d))/2; + arc.offset[arc.linear_axis] = 0; + return STAT_OK; +} + +/* + * _estimate_arc_time () + * + * Returns a naiive estimate of arc execution time to inform segment calculation. + * The arc time is computed not to exceed the time taken in the slowest dimension + * in the arc plane or in linear travel. Maximum feed rates are compared in each + * dimension, but the comparison assumes that the arc will have at least one segment + * where the unit vector is 1 in that dimension. This is not true for any arbitrary arc, + * with the result that the time returned may be less than optimal. + */ +static void _estimate_arc_time () +{ + // Determine move time at requested feed rate + if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) { + arc.arc_time = cm.gm.feed_rate; // inverse feed rate has been normalized to minutes + cm.gm.feed_rate = 0; // reset feed rate so next block requires an explicit feed rate setting + cm.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE; + } else { + arc.arc_time = arc.length / cm.gm.feed_rate; + } + + // Downgrade the time if there is a rate-limiting axis + arc.arc_time = max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_0].feedrate_max); + arc.arc_time = max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_1].feedrate_max); + if (fabs(arc.linear_travel) > 0) { + arc.arc_time = max(arc.arc_time, fabs(arc.linear_travel/cm.a[arc.linear_axis].feedrate_max)); + } +} diff --git a/src/plan/arc.h b/src/plan/arc.h new file mode 100644 index 0000000..73d6997 --- /dev/null +++ b/src/plan/arc.h @@ -0,0 +1,71 @@ +/* + * arc.h - arc planning and motion execution + * This file is part of the TinyG project + * + * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + * + * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#ifndef PLAN_ARC_H +#define PLAN_ARC_H + +#include "canonical_machine.h" + +// Arc radius tests. See http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc +#define ARC_RADIUS_ERROR_MAX ((float)1.0) // max allowable mm between start and end radius +#define ARC_RADIUS_ERROR_MIN ((float)0.005) // min mm where 1% rule applies +#define ARC_RADIUS_TOLERANCE ((float)0.001) // 0.1% radius variance test + +// See planner.h for MM_PER_ARC_SEGMENT and other arc setting #defines + +typedef struct arArcSingleton { // persistent planner and runtime variables + uint8_t run_state; // runtime state machine sequence + + float position[AXES]; // accumulating runtime position + float offset[3]; // IJK offsets + + float length; // length of line or helix in mm + float theta; // total angle specified by arc + float theta_end; + float radius; // Raw R value, or computed via offsets + float angular_travel; // travel along the arc + float linear_travel; // travel along linear axis of arc + float planar_travel; + uint8_t full_circle; // set true if full circle arcs specified + uint32_t rotations; // Number of full rotations for full circles (P value) + + uint8_t plane_axis_0; // arc plane axis 0 - e.g. X for G17 + uint8_t plane_axis_1; // arc plane axis 1 - e.g. Y for G17 + uint8_t linear_axis; // linear axis (normal to plane) + + float arc_time; // total running time for arc (derived) + float arc_segments; // number of segments in arc or blend + int32_t arc_segment_count; // count of running segments + float arc_segment_theta; // angular motion per segment + float arc_segment_linear_travel; // linear motion per segment + float center_0; // center of circle at plane axis 0 (e.g. X for G17) + float center_1; // center of circle at plane axis 1 (e.g. Y for G17) + + GCodeState_t gm; // Gcode state struct is passed for each arc segment. Usage: +} arc_t; +extern arc_t arc; + + +/* arc function prototypes */ // NOTE: See canonical_machine.h for cm_arc_feed() prototype + +void cm_arc_init(); +stat_t cm_arc_callback(); +void cm_abort_arc(); + +#endif // PLAN_ARC_H diff --git a/src/plan/exec.c b/src/plan/exec.c new file mode 100644 index 0000000..7f6905f --- /dev/null +++ b/src/plan/exec.c @@ -0,0 +1,744 @@ +/* + * exec.c - execution function for acceleration managed lines + * This file is part of the TinyG project + * + * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2015 Rob Giseburt + * + * 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 . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "planner.h" +#include "kinematics.h" +#include "stepper.h" +#include "encoder.h" +#include "util.h" +#include "report.h" + +#include +#include +#include + +// Execute routines (NB: These are all called from the LO interrupt) +static stat_t _exec_aline_head(); +static stat_t _exec_aline_body(); +static stat_t _exec_aline_tail(); +static stat_t _exec_aline_segment(); + +#ifndef __JERK_EXEC +static void _init_forward_diffs(float Vi, float Vt); +#endif + + +/************************************************************************* + * Execute runtime functions to prep move for steppers + * + * Dequeues the buffer queue and executes the move continuations. + * Manages run buffers and other details + */ +stat_t mp_exec_move() { + mpBuf_t *bf; + + if ((bf = mp_get_run_buffer()) == 0) { // 0 means nothing's running + st_prep_null(); + return STAT_NOOP; + } + + // Manage cycle and motion state transitions + // Cycle auto-start for lines only + if (bf->move_type == MOVE_TYPE_ALINE && cm.motion_state == MOTION_STOP) + cm_set_motion_state(MOTION_RUN); + + if (bf->bf_func == 0) + return cm_hard_alarm(STAT_INTERNAL_ERROR); // never supposed to get here + + return bf->bf_func(bf); // run the move callback in the planner buffer +} + + +/*************************************************************************/ +/**** ALINE EXECUTION ROUTINES *******************************************/ +/************************************************************************* + * ---> Everything here fires from interrupts and must be interrupt safe + * + * _exec_aline() - acceleration line main routine + * _exec_aline_head() - helper for acceleration section + * _exec_aline_body() - helper for cruise section + * _exec_aline_tail() - helper for deceleration section + * _exec_aline_segment() - helper for running a segment + * + * Returns: + * STAT_OK move is done + * STAT_EAGAIN move is not finished - has more segments to run + * STAT_NOOP cause no operation from the steppers - do not load the move + * STAT_xxxxx fatal error. Ends the move and frees the bf buffer + * + * This routine is called from the (LO) interrupt level. The interrupt + * sequencing relies on the behaviors of the routines being exactly correct. + * Each call to _exec_aline() must execute and prep *one and only one* + * segment. If the segment is the not the last segment in the bf buffer the + * _aline() must return STAT_EAGAIN. If it's the last segment it must return + * STAT_OK. If it encounters a fatal error that would terminate the move it + * should return a valid error code. Failure to obey this will introduce + * subtle and very difficult to diagnose bugs (trust me on this). + * + * Note 1 Returning STAT_OK ends the move and frees the bf buffer. + * Returning STAT_OK at this point does NOT advance position meaning any + * position error will be compensated by the next move. + * + * Note 2 Solves a potential race condition where the current move ends but the + * new move has not started because the previous move is still being run + * by the steppers. Planning can overwrite the new move. + */ +/* OPERATION: + * Aline generates jerk-controlled S-curves as per Ed Red's course notes: + * http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf + * http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations + * + * A full trapezoid is divided into 5 periods Periods 1 and 2 are the + * first and second halves of the acceleration ramp (the concave and convex + * parts of the S curve in the "head"). Periods 3 and 4 are the first + * and second parts of the deceleration ramp (the tail). There is also + * a period for the constant-velocity plateau of the trapezoid (the body). + * There are various degraded trapezoids possible, including 2 section + * combinations (head and tail; head and body; body and tail), and single + * sections - any one of the three. + * + * The equations that govern the acceleration and deceleration ramps are: + * + * Period 1 V = Vi + Jm*(T^2)/2 + * Period 2 V = Vh + As*T - Jm*(T^2)/2 + * 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 all works out. move_time is the actual time of the + * move, accel_time is the time valaue needed to compute the velocity - which + * takes the initial velocity into account (move_time does not need to). + */ +/* --- State transitions - hierarchical state machine --- + * + * bf->move_state transitions: + * from _NEW to _RUN on first call (sub_state set to _OFF) + * from _RUN to _OFF on final call + * or just remains _OFF + * + * mr.move_state transitions on first call from _OFF to one of _HEAD, _BODY, _TAIL + * Within each section state may be + * _NEW - trigger initialization + * _RUN1 - run the first part + * _RUN2 - run the second part + * + * Note: For a direct math implementation see build 357.xx or earlier + * Builds 358 onward have only forward difference code + */ + +stat_t mp_exec_aline(mpBuf_t *bf) { + if (bf->move_state == MOVE_OFF) return STAT_NOOP; + + // start a new move by setting up local context (singleton) + if (mr.move_state == MOVE_OFF) { + if (cm.hold_state == FEEDHOLD_HOLD) return STAT_NOOP; // stops here if holding + + // initialization to process the new incoming bf buffer (Gcode block) + memcpy(&mr.gm, &(bf->gm), sizeof(GCodeState_t)); // copy in the gcode model state + bf->replannable = false; + + // too short lines have already been removed + if (fp_ZERO(bf->length)) { // ...looks for an actual zero here + mr.move_state = MOVE_OFF; // reset mr buffer + mr.section_state = SECTION_OFF; + bf->nx->replannable = false; // prevent overplanning (Note 2) + st_prep_null(); // call this to keep the loader happy + if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & end cycle if planner is empty + return STAT_NOOP; + } + + bf->move_state = MOVE_RUN; + mr.move_state = MOVE_RUN; + mr.section = SECTION_HEAD; + mr.section_state = SECTION_NEW; + mr.jerk = bf->jerk; +#ifdef __JERK_EXEC + mr.jerk_div2 = bf->jerk/2; // only needed by __JERK_EXEC +#endif + mr.head_length = bf->head_length; + mr.body_length = bf->body_length; + mr.tail_length = bf->tail_length; + + mr.entry_velocity = bf->entry_velocity; + mr.cruise_velocity = bf->cruise_velocity; + mr.exit_velocity = bf->exit_velocity; + + copy_vector(mr.unit, bf->unit); + copy_vector(mr.target, bf->gm.target); // save the final target of the move + + // generate the waypoints for position correction at section ends + for (uint8_t axis = 0; axis < AXES; axis++) { + mr.waypoint[SECTION_HEAD][axis] = mr.position[axis] + mr.unit[axis] * mr.head_length; + mr.waypoint[SECTION_BODY][axis] = + mr.position[axis] + mr.unit[axis] * (mr.head_length + mr.body_length); + mr.waypoint[SECTION_TAIL][axis] = + mr.position[axis] + mr.unit[axis] * (mr.head_length + mr.body_length + mr.tail_length); + } + } + // NB: from this point on the contents of the bf buffer do not affect execution + + //**** main dispatcher to process segments *** + stat_t status = STAT_OK; + + if (mr.section == SECTION_HEAD) status = _exec_aline_head(); + else if (mr.section == SECTION_BODY) status = _exec_aline_body(); + else if (mr.section == SECTION_TAIL) status = _exec_aline_tail(); + else if (mr.move_state == MOVE_SKIP_BLOCK) status = STAT_OK; + else return cm_hard_alarm(STAT_INTERNAL_ERROR); // never supposed to get here + + // Feedhold processing. Refer to canonical_machine.h for state machine + // Catch the feedhold request and start the planning the hold + if (cm.hold_state == FEEDHOLD_SYNC) { cm.hold_state = FEEDHOLD_PLAN;} + + // Look for the end of the decel to go into HOLD state + if ((cm.hold_state == FEEDHOLD_DECEL) && (status == STAT_OK)) { + cm.hold_state = FEEDHOLD_HOLD; + cm_set_motion_state(MOTION_HOLD); + report_request(); + } + + // There are 3 things that can happen here depending on return conditions: + // status bf->move_state Description + // ----------- -------------- ---------------------------------------- + // STAT_EAGAIN mr buffer has more segments to run + // STAT_OK MOVE_RUN mr and bf buffers are done + // STAT_OK MOVE_NEW mr done; bf must be run again (it's been reused) + if (status == STAT_EAGAIN) report_request(); + else { + mr.move_state = MOVE_OFF; // reset mr buffer + mr.section_state = SECTION_OFF; + bf->nx->replannable = false; // prevent overplanning (Note 2) + + if (bf->move_state == MOVE_RUN && mp_free_run_buffer()) + cm_cycle_end(); // free buffer & end cycle if planner is empty + } + + return status; +} + + +/* Forward difference math explained: + * + * 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 + * + * The Bezier curve takes the form: + * + * V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + P_4 * B_4(t) + P_5 * B_5(t) + * + * Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are the control points, and B_0(t) + * through B_5(t) are the Bernstein basis as follows: + * + * B_0(t) = (1-t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1 + * B_1(t) = 5(1-t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t + * B_2(t) = 10(1-t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2 + * B_3(t) = 10(1-t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3 + * B_4(t) = 5(1-t) * t^4 = -5t^5 + 5t^4 + * B_5(t) = t^5 = t^5 + * ^ ^ ^ ^ ^ ^ + * | | | | | | + * A B C D E F + * + * + * We use forward-differencing to calculate each position through the curve. + * This requires a formula of the form: + * + * V_f(t) = A*t^5 + B*t^4 + C*t^3 + D*t^2 + E*t + F + * + * Looking at the above B_0(t) through B_5(t) expanded forms, if we take the coefficients of t^5 + * through t of the Bezier form of V(t), we can determine that: + * + * A = -P_0 + 5*P_1 - 10*P_2 + 10*P_3 - 5*P_4 + P_5 + * B = 5*P_0 - 20*P_1 + 30*P_2 - 20*P_3 + 5*P_4 + * C = -10*P_0 + 30*P_1 - 30*P_2 + 10*P_3 + * D = 10*P_0 - 20*P_1 + 10*P_2 + * 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: + * + * A = - 6*P_i + 6*P_t + * B = 15*P_i - 15*P_t + * C = -10*P_i + 10*P_t + * D = 0 + * 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: + * + * V += F_5 + * F_5 += F_4 + * F_4 += F_3 + * F_3 += F_2 + * F_2 += F_1 + * + * See http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417 for an example of + * how to calculate F_0 - F_5 for a cubic bezier curve. Since this is a quintic bezier curve, we + * need to extend the formulas somewhat. I'll not go into the long-winded step-by-step here, + * but it gives the resulting formulas: + * + * a = A, b = B, c = C, d = D, e = E, f = F + * F_5(t+h)-F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 + (10ah^3 + 6bh^2 + 3ch)t^2 + + * (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 + bh^4 + ch^3 + dh^2 + eh + * + * a = 5ah + * b = 10ah^2 + 4bh + * c = 10ah^3 + 6bh^2 + 3ch + * d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh + * + * (After substitution, simplification, and rearranging): + * F_4(t+h)-F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 + (70ah^4 + 24bh^3 + 6ch^2)t + + * 30ah^5 + 14bh^4 + 6ch^3 + 2dh^2 + * + * a = (20ah^2) + * b = (60ah^3 + 12bh^2) + * c = (70ah^4 + 24bh^3 + 6ch^2) + * + * (After substitution, simplification, and rearranging): + * F_3(t+h)-F_3(t) = (60ah^3)t^2 + (180ah^4 + 24bh^3)t + 150ah^5 + 36bh^4 + 6ch^3 + * + * (You get the picture...) + * F_2(t+h)-F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4 + * F_1(t+h)-F_1(t) = 120ah^5 + * + * 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): + * + * F_5 = (121Ah^5)/16 + 5Bh^4 + (13Ch^3)/4 + 2Dh^2 + Eh + * F_4 = (165Ah^5)/2 + 29Bh^4 + 9Ch^3 + 2Dh^2 + * F_3 = 255Ah^5 + 48Bh^4 + 6Ch^3 + * F_2 = 300Ah^5 + 24Bh^4 + * F_1 = 120Ah^5 + * + * Note that with our current control points, D and E are actually 0. + */ +#ifndef __JERK_EXEC + +static void _init_forward_diffs(float Vi, float Vt) { + float A = -6.0*Vi + 6.0*Vt; + float B = 15.0*Vi - 15.0*Vt; + float C = -10.0*Vi + 10.0*Vt; + // D = 0 + // E = 0 + // F = Vi + + float h = 1 / mr.segments; + + float Ah_5 = A * h * h * h * h * h; + float Bh_4 = B * h * h * h * h; + float Ch_3 = C * h * h * h; + + mr.forward_diff_5 = (121.0 / 16.0) * Ah_5 + 5.0 * Bh_4 + (13.0 / 4.0) * Ch_3; + mr.forward_diff_4 = (165.0 / 2.0) * Ah_5 + 29.0 * Bh_4 + 9.0 * Ch_3; + mr.forward_diff_3 = 255.0 * Ah_5 + 48.0 * Bh_4 + 6.0 * Ch_3; + mr.forward_diff_2 = 300.0 * Ah_5 + 24.0 * Bh_4; + mr.forward_diff_1 = 120.0 * Ah_5; + +#ifdef __KAHAN + mr.forward_diff_5_c = 0; + mr.forward_diff_4_c = 0; + mr.forward_diff_3_c = 0; + mr.forward_diff_2_c = 0; + mr.forward_diff_1_c = 0; +#endif + + // Calculate the initial velocity by calculating V(h/2) + float half_h = h / 2.0; + float half_Ch_3 = C * half_h * half_h * half_h; + float half_Bh_4 = B * half_h * half_h * half_h * half_h; + float half_Ah_5 = C * half_h * half_h * half_h * half_h * half_h; + mr.segment_velocity = half_Ah_5 + half_Bh_4 + half_Ch_3 + Vi; +} +#endif + +#ifdef __JERK_EXEC + +static stat_t _exec_aline_head() { + if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) + if (fp_ZERO(mr.head_length)) { + mr.section = SECTION_BODY; + return _exec_aline_body(); // skip ahead to the body generator + } + + mr.midpoint_velocity = (mr.entry_velocity + mr.cruise_velocity) / 2; + mr.gm.move_time = mr.head_length / mr.midpoint_velocity; // time for entire accel region + mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC)); // # of segments in *each half* + mr.segment_time = mr.gm.move_time / (2 * mr.segments); + mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.entry_velocity) / mr.jerk); + mr.midpoint_acceleration = 2 * (mr.cruise_velocity - mr.entry_velocity) / mr.accel_time; + mr.segment_accel_time = mr.accel_time / (2 * mr.segments); // time to advance for each segment + mr.elapsed_accel_time = mr.segment_accel_time / 2; // elapsed time starting point (offset) + mr.segment_count = (uint32_t)mr.segments; + + if (mr.segment_time < MIN_SEGMENT_TIME) + return STAT_MINIMUM_TIME_MOVE; // exit without advancing position + + mr.section = SECTION_HEAD; + mr.section_state = SECTION_1st_HALF; + } + + if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF (concave part of accel curve) + mr.segment_velocity = mr.entry_velocity + (square(mr.elapsed_accel_time) * mr.jerk_div2); + + if (_exec_aline_segment() == STAT_OK) { // set up for second half + mr.segment_count = (uint32_t)mr.segments; + mr.section_state = SECTION_2nd_HALF; + mr.elapsed_accel_time = mr.segment_accel_time / 2; // start time from midpoint of segment + } + + return STAT_EAGAIN; + } + + if (mr.section_state == SECTION_2nd_HALF) { // SECOND HAF (convex part of accel curve) + mr.segment_velocity = mr.midpoint_velocity + + (mr.elapsed_accel_time * mr.midpoint_acceleration) - + (square(mr.elapsed_accel_time) * mr.jerk_div2); + + if (_exec_aline_segment() == STAT_OK) { // OK means this section is done + if ((fp_ZERO(mr.body_length)) && (fp_ZERO(mr.tail_length))) + return STAT_OK; // ends the move + + mr.section = SECTION_BODY; + mr.section_state = SECTION_NEW; + } + } + + return STAT_EAGAIN; +} +#else // __ JERK_EXEC + +static stat_t _exec_aline_head() { + if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) + if (fp_ZERO(mr.head_length)) { + mr.section = SECTION_BODY; + return _exec_aline_body(); // skip ahead to the body generator + } + + // Time for entire accel region + mr.gm.move_time = 2 * mr.head_length / (mr.entry_velocity + mr.cruise_velocity); + mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC);// # of segments for the section + mr.segment_time = mr.gm.move_time / mr.segments; + _init_forward_diffs(mr.entry_velocity, mr.cruise_velocity); + mr.segment_count = (uint32_t)mr.segments; + + if (mr.segment_time < MIN_SEGMENT_TIME) + return STAT_MINIMUM_TIME_MOVE; // exit without advancing position + + mr.section = SECTION_HEAD; + mr.section_state = SECTION_1st_HALF; // Note: Set to SECTION_1st_HALF for one segment + } + + // For forward differencing we should have one segment in SECTION_1st_HALF + // However, if it returns from that as STAT_OK, then there was only one segment in this section. + if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF (concave part of accel curve) + if (_exec_aline_segment() == STAT_OK) { // set up for second half + mr.section = SECTION_BODY; + mr.section_state = SECTION_NEW; + + } else mr.section_state = SECTION_2nd_HALF; + + return STAT_EAGAIN; + } + + if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF (convex part of accel curve) +#ifndef __KAHAN + mr.segment_velocity += mr.forward_diff_5; + +#else // Use Kahan summation algorithm to mitigate floating-point errors for the above + float y = mr.forward_diff_5 - mr.forward_diff_5_c; + float v = mr.segment_velocity + y; + mr.forward_diff_5_c = (v - mr.segment_velocity) - y; + mr.segment_velocity = v; +#endif + + if (_exec_aline_segment() == STAT_OK) { // set up for body + if ((fp_ZERO(mr.body_length)) && (fp_ZERO(mr.tail_length))) + return STAT_OK; // ends the move + + mr.section = SECTION_BODY; + mr.section_state = SECTION_NEW; + + } else { +#ifndef __KAHAN + mr.forward_diff_5 += mr.forward_diff_4; + mr.forward_diff_4 += mr.forward_diff_3; + mr.forward_diff_3 += mr.forward_diff_2; + mr.forward_diff_2 += mr.forward_diff_1; +#else + y = mr.forward_diff_4 - mr.forward_diff_4_c; + v = mr.forward_diff_5 + y; + mr.forward_diff_4_c = (v - mr.forward_diff_5) - y; + mr.forward_diff_5 = v; + + y = mr.forward_diff_3 - mr.forward_diff_3_c; + v = mr.forward_diff_4 + y; + mr.forward_diff_3_c = (v - mr.forward_diff_4) - y; + mr.forward_diff_4 = v; + + y = mr.forward_diff_2 - mr.forward_diff_2_c; + v = mr.forward_diff_3 + y; + mr.forward_diff_2_c = (v - mr.forward_diff_3) - y; + mr.forward_diff_3 = v; + + y = mr.forward_diff_1 - mr.forward_diff_1_c; + v = mr.forward_diff_2 + y; + mr.forward_diff_1_c = (v - mr.forward_diff_2) - y; + mr.forward_diff_2 = v; +#endif + } + } + + return STAT_EAGAIN; +} +#endif // __ JERK_EXEC + + +/// The body is broken into little segments even though it is a straight line so that +/// feedholds can happen in the middle of a line with a minimum of latency +static stat_t _exec_aline_body() { + if (mr.section_state == SECTION_NEW) { + if (fp_ZERO(mr.body_length)) { + mr.section = SECTION_TAIL; + return _exec_aline_tail(); // skip ahead to tail periods + } + + mr.gm.move_time = mr.body_length / mr.cruise_velocity; + mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC); + mr.segment_time = mr.gm.move_time / mr.segments; + mr.segment_velocity = mr.cruise_velocity; + mr.segment_count = (uint32_t)mr.segments; + + if (mr.segment_time < MIN_SEGMENT_TIME) + return STAT_MINIMUM_TIME_MOVE; // exit without advancing position + + mr.section = SECTION_BODY; + mr.section_state = SECTION_2nd_HALF; // uses PERIOD_2 so last segment detection works + } + + if (mr.section_state == SECTION_2nd_HALF) // straight part (period 3) + if (_exec_aline_segment() == STAT_OK) { // OK means this section is done + if (fp_ZERO(mr.tail_length)) return STAT_OK; // ends the move + mr.section = SECTION_TAIL; + mr.section_state = SECTION_NEW; + } + + return STAT_EAGAIN; +} + + +#ifdef __JERK_EXEC + +static stat_t _exec_aline_tail() { + if (mr.section_state == SECTION_NEW) { // INITIALIZATION + if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move + + mr.midpoint_velocity = (mr.cruise_velocity + mr.exit_velocity) / 2; + mr.gm.move_time = mr.tail_length / mr.midpoint_velocity; + mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC));// # of segments in *each half* + mr.segment_time = mr.gm.move_time / (2 * mr.segments); // time to advance for each segment + mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.exit_velocity) / mr.jerk); + mr.midpoint_acceleration = 2 * (mr.cruise_velocity - mr.exit_velocity) / mr.accel_time; + mr.segment_accel_time = mr.accel_time / (2 * mr.segments); // time to advance for each segment + mr.elapsed_accel_time = mr.segment_accel_time / 2; // compute time from midpoint of segment + mr.segment_count = (uint32_t)mr.segments; + if (mr.segment_time < MIN_SEGMENT_TIME) return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position + mr.section = SECTION_TAIL; + mr.section_state = SECTION_1st_HALF; + } + + if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF - convex part (period 4) + mr.segment_velocity = mr.cruise_velocity - (square(mr.elapsed_accel_time) * mr.jerk_div2); + + if (_exec_aline_segment() == STAT_OK) { // set up for second half + mr.segment_count = (uint32_t)mr.segments; + mr.section_state = SECTION_2nd_HALF; + mr.elapsed_accel_time = mr.segment_accel_time / 2; // start time from midpoint of segment + } + + return STAT_EAGAIN; + } + + if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF - concave part (period 5) + mr.segment_velocity = mr.midpoint_velocity - + (mr.elapsed_accel_time * mr.midpoint_acceleration) + + (square(mr.elapsed_accel_time) * mr.jerk_div2); + return _exec_aline_segment(); // ends the move or continues EAGAIN + } + + return STAT_EAGAIN; // should never get here +} + + +#else // __JERK_EXEC -- run forward differencing math + +static stat_t _exec_aline_tail() { + if (mr.section_state == SECTION_NEW) { // INITIALIZATION + if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move + + mr.gm.move_time = 2 * mr.tail_length / (mr.cruise_velocity + mr.exit_velocity); // len/avg. velocity + mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC); // # of segments for the section + mr.segment_time = mr.gm.move_time / mr.segments; // time to advance for each segment + _init_forward_diffs(mr.cruise_velocity, mr.exit_velocity); + mr.segment_count = (uint32_t)mr.segments; + if (mr.segment_time < MIN_SEGMENT_TIME) return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position + mr.section = SECTION_TAIL; + mr.section_state = SECTION_1st_HALF; + } + + if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF - convex part (period 4) + if (_exec_aline_segment() == STAT_OK) { + // For forward differencing we should have one segment in SECTION_1st_HALF. + // However, if it returns from that as STAT_OK, then there was only one segment in this section. + // Show that we did complete section 2 ... effectively. + mr.section_state = SECTION_2nd_HALF; + return STAT_OK; + + } else mr.section_state = SECTION_2nd_HALF; + + return STAT_EAGAIN; + } + + if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF - concave part (period 5) +#ifndef __KAHAN + mr.segment_velocity += mr.forward_diff_5; +#else // Use Kahan summation algorithm to mitigate floating-point errors for the above + float y = mr.forward_diff_5 - mr.forward_diff_5_c; + float v = mr.segment_velocity + y; + mr.forward_diff_5_c = (v - mr.segment_velocity) - y; + mr.segment_velocity = v; +#endif + + if (_exec_aline_segment() == STAT_OK) return STAT_OK; // set up for body + else { +#ifndef __KAHAN + mr.forward_diff_5 += mr.forward_diff_4; + mr.forward_diff_4 += mr.forward_diff_3; + mr.forward_diff_3 += mr.forward_diff_2; + mr.forward_diff_2 += mr.forward_diff_1; +#else + y = mr.forward_diff_4 - mr.forward_diff_4_c; + v = mr.forward_diff_5 + y; + mr.forward_diff_4_c = (v - mr.forward_diff_5) - y; + mr.forward_diff_5 = v; + + y = mr.forward_diff_3 - mr.forward_diff_3_c; + v = mr.forward_diff_4 + y; + mr.forward_diff_3_c = (v - mr.forward_diff_4) - y; + mr.forward_diff_4 = v; + + y = mr.forward_diff_2 - mr.forward_diff_2_c; + v = mr.forward_diff_3 + y; + mr.forward_diff_2_c = (v - mr.forward_diff_3) - y; + mr.forward_diff_3 = v; + + y = mr.forward_diff_1 - mr.forward_diff_1_c; + v = mr.forward_diff_2 + y; + mr.forward_diff_1_c = (v - mr.forward_diff_2) - y; + mr.forward_diff_2 = v; +#endif + } + } + + return STAT_EAGAIN; // should never get here +} +#endif // __JERK_EXEC + + +/********************************************************************************************* + * Segment runner helper + * + * NOTES ON STEP ERROR CORRECTION: + * + * The commanded_steps are the target_steps delayed by one more segment. + * This lines them up in time with the encoder readings so a following error can be generated + * + * The following_error term is positive if the encoder reading is greater than (ahead of) + * the commanded steps, and negative (behind) if the encoder reading is less than the + * commanded steps. The following error is not affected by the direction of movement - + * it's purely a statement of relative position. Examples: + * + * Encoder Commanded Following Err + * 100 90 +10 encoder is 10 steps ahead of commanded steps + * -90 -100 +10 encoder is 10 steps ahead of commanded steps + * 90 100 -10 encoder is 10 steps behind commanded steps + * -100 -90 -10 encoder is 10 steps behind commanded steps + */ +static stat_t _exec_aline_segment() { + uint8_t i; + float travel_steps[MOTORS]; + + // Set target position for the segment + // If the segment ends on a section 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 ((--mr.segment_count == 0) && (mr.section_state == SECTION_2nd_HALF) && + (cm.motion_state == MOTION_RUN) && (cm.cycle_state == CYCLE_MACHINING)) { + copy_vector(mr.gm.target, mr.waypoint[mr.section]); + + } else { + float segment_length = mr.segment_velocity * mr.segment_time; + + for (i = 0; i < AXES; i++) + mr.gm.target[i] = mr.position[i] + (mr.unit[i] * segment_length); + } + + // Convert target position to steps + // Bucket-brigade the old target down the chain before getting the new target from kinematics + // + // NB: The direct manipulation of steps to compute travel_steps only works for Cartesian kinematics. + // Other kinematics may require transforming travel distance as opposed to simply subtracting steps. + + for (i = 0; i < MOTORS; i++) { + mr.commanded_steps[i] = mr.position_steps[i]; // previous segment's position, delayed by 1 segment + mr.position_steps[i] = mr.target_steps[i]; // previous segment's target becomes position + mr.encoder_steps[i] = en_read_encoder(i); // current encoder position (aligns to commanded_steps) + mr.following_error[i] = mr.encoder_steps[i] - mr.commanded_steps[i]; + } + + ik_kinematics(mr.gm.target, mr.target_steps); // now determine the target steps... + + for (i = 0; i < MOTORS; i++) // and compute the distances to be traveled + travel_steps[i] = mr.target_steps[i] - mr.position_steps[i]; + + // Call the stepper prep function + ritorno(st_prep_line(travel_steps, mr.following_error, mr.segment_time)); + copy_vector(mr.position, mr.gm.target); // update position from target +#ifdef __JERK_EXEC + mr.elapsed_accel_time += mr.segment_accel_time; // needed by jerk-based exec (NB: ignored if running body) +#endif + + if (mr.segment_count == 0) return STAT_OK; // this section has run all its segments + return STAT_EAGAIN; // this section still has more segments to run +} diff --git a/src/kinematics.c b/src/plan/kinematics.c similarity index 98% rename from src/kinematics.c rename to src/plan/kinematics.c index 554f672..d6b5a96 100644 --- a/src/kinematics.c +++ b/src/plan/kinematics.c @@ -25,12 +25,12 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" -#include "config.h" +#include "kinematics.h" + #include "canonical_machine.h" #include "stepper.h" -#include "kinematics.h" +#include /* * Wrapper routine for inverse kinematics diff --git a/src/kinematics.h b/src/plan/kinematics.h similarity index 95% rename from src/kinematics.h rename to src/plan/kinematics.h index 4b0e901..4e62647 100644 --- a/src/kinematics.h +++ b/src/plan/kinematics.h @@ -25,9 +25,9 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#ifndef KINEMATICS_H_ONCE -#define KINEMATICS_H_ONCE +#ifndef KINEMATICS_H +#define KINEMATICS_H void ik_kinematics(const float travel[], float steps[]); -#endif // KINEMATICS_H_ONCE +#endif // KINEMATICS_H diff --git a/src/plan/line.c b/src/plan/line.c new file mode 100644 index 0000000..1638789 --- /dev/null +++ b/src/plan/line.c @@ -0,0 +1,767 @@ +/* + * line.c - acceleration managed line planning and motion execution + * This file is part of the TinyG project + * + * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2015 Rob Giseburt + * + * 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 . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "planner.h" + +#include "controller.h" +#include "canonical_machine.h" +#include "stepper.h" +#include "util.h" +#include "report.h" + +#include +#include +#include + + +static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const float axis_square[]); +static void _plan_block_list(mpBuf_t *bf, uint8_t *mr_flag); +static float _get_junction_vmax(const float a_unit[], const float b_unit[]); +static void _reset_replannable_list(); + + +/* Runtime-specific setters and getters + * + * mp_zero_segment_velocity() - correct velocity in last segment for reporting purposes + * mp_get_runtime_velocity() - returns current velocity (aggregate) + * mp_get_runtime_machine_position() - returns current axis position in machine coordinates + * mp_set_runtime_work_offset() - set offsets in the MR struct + * mp_get_runtime_work_position() - returns current axis position in work coordinates + * that were in effect at move planning time + */ +void mp_zero_segment_velocity() {mr.segment_velocity = 0;} +float mp_get_runtime_velocity() {return mr.segment_velocity;} +float mp_get_runtime_absolute_position(uint8_t axis) {return mr.position[axis];} +void mp_set_runtime_work_offset(float offset[]) {copy_vector(mr.gm.work_offset, offset);} +float mp_get_runtime_work_position(uint8_t axis) {return mr.position[axis] - mr.gm.work_offset[axis];} + +/* + * Return TRUE if motion control busy (i.e. robot is moving) + * + * Use this function to sync to the queue. If you wait until it returns + * FALSE you know the queue is empty and the motors have stopped. + */ +uint8_t mp_get_runtime_busy() { + return st_runtime_isbusy() || mr.move_state == MOVE_RUN; +} + + +/**************************************************************************************** + * Plan a line with acceleration / deceleration + * + * This function uses constant jerk motion equations to plan acceleration and deceleration + * The jerk is the rate of change of acceleration; it's the 1st derivative of acceleration, + * and the 3rd derivative of position. Jerk is a measure of impact to the machine. + * Controlling jerk smooths transitions between moves and allows for faster feeds while + * controlling machine oscillations and other undesirable side-effects. + * + * Note All math is done in absolute coordinates using single precision floating point (float). + * + * Note: Returning a status that is not STAT_OK means the endpoint is NOT advanced. So lines + * that are too short to move will accumulate and get executed once the accumulated error + * exceeds the minimums. + */ +/* + #define axis_length bf->body_length + #define axis_velocity bf->cruise_velocity + #define axis_tail bf->tail_length + #define longest_tail bf->head_length +*/ +stat_t mp_aline(GCodeState_t *gm_in) { + mpBuf_t *bf; // current move pointer + float exact_stop = 0; // preset this value OFF + float junction_velocity; + uint8_t mr_flag = false; + + // compute some reusable terms + float axis_length[AXES]; + float axis_square[AXES]; + float length_square = 0; + + for (uint8_t axis = 0; axis < AXES; axis++) { + axis_length[axis] = gm_in->target[axis] - mm.position[axis]; + axis_square[axis] = square(axis_length[axis]); + length_square += axis_square[axis]; + } + + float length = sqrt(length_square); + if (fp_ZERO(length)) { + report_request(); + return STAT_OK; + } + + // If _calc_move_times() says the move will take less than the minimum move time + // get a more accurate time estimate based on starting velocity and acceleration. + // The time of the move is determined by its initial velocity (Vi) and how much + // acceleration will be occur. For this we need to look at the exit velocity of + // the previous block. There are 3 possible cases: + // (1) There is no previous block. Vi = 0 + // (2) Previous block is optimally planned. Vi = previous block's exit_velocity + // (3) Previous block is not optimally planned. Vi <= previous block's entry_velocity + delta_velocity + + _calc_move_times(gm_in, axis_length, axis_square); // set move & minimum time in state + + if (gm_in->move_time < MIN_BLOCK_TIME) { + float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; // max velocity change for this move + float entry_velocity = 0; // pre-set as if no previous block + + if ((bf = mp_get_run_buffer()) != 0) { + if (bf->replannable) // not optimally planned + entry_velocity = bf->entry_velocity + bf->delta_vmax; + else entry_velocity = bf->exit_velocity; // optimally planned + } + + // compute execution time for this move + float move_time = (2 * length) / (2 * entry_velocity + delta_velocity); + if (move_time < MIN_BLOCK_TIME) return STAT_MINIMUM_TIME_MOVE; + } + + // get a cleared buffer and setup move variables + if ((bf = mp_get_write_buffer()) == 0) + return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // never supposed to fail + + bf->bf_func = mp_exec_aline; // register callback to exec function + bf->length = length; + memcpy(&bf->gm, gm_in, sizeof(GCodeState_t)); // copy model state into planner buffer + + // Compute the unit vector and find the right jerk to use (combined operations) + // To determine the jerk value to use for the block we want to find the axis for which + // the jerk cannot be exceeded - the 'jerk-limit' axis. This is the axis for which + // the time to decelerate from the target velocity to zero would be the longest. + // + // We can determine the "longest" deceleration in terms of time or distance. + // + // The math for time-to-decelerate T from speed S to speed E with constant + // jerk J is: + // + // T = 2*sqrt((S-E)/J[n]) + // + // Since E is always zero in this calculation, we can simplify: + // T = 2*sqrt(S/J[n]) + // + // The math for distance-to-decelerate l from speed S to speed E with constant + // jerk J is: + // + // l = (S+E)*sqrt((S-E)/J) + // + // Since E is always zero in this calculation, we can simplify: + // l = S*sqrt(S/J) + // + // The final value we want is to know which one is *longest*, compared to the others, + // so we don't need the actual value. This means that the scale of the value doesn't + // matter, so for T we can remove the "2 *" and For L we can remove the "S*". + // This leaves both to be simply "sqrt(S/J)". Since we don't need the scale, + // it doesn't matter what speed we're coming from, so S can be replaced with 1. + // + // However, we *do* need to compensate for the fact that each axis contributes + // differently to the move, so we will scale each contribution C[n] by the + // proportion of the axis movement length D[n] to the total length of the move L. + // Using that, we construct the following, with these definitions: + // + // J[n] = max_jerk for axis n + // D[n] = distance traveled for this move for axis n + // L = total length of the move in all axes + // C[n] = "axis contribution" of axis n + // + // For each axis n: C[n] = sqrt(1/J[n]) * (D[n]/L) + // + // Keeping in mind that we only need a rank compared to the other axes, we can further + // optimize the calculations:: + // + // Square the expression to remove the square root: + // C[n]^2 = (1/J[n]) * (D[n]/L)^2 (We don't care the C is squared, we'll use it that way.) + // + // Re-arranged to optimize divides for pre-calculated values, we create a value + // M that we compute once: + // M = (1/(L^2)) + // Then we use it in the calculations for every axis: + // C[n] = (1/J[n]) * D[n]^2 * M + // + // Also note that we already have (1/J[n]) calculated for each axis, which simplifies it further. + // + // Finally, the selected jerk term needs to be scaled by the reciprocal of the absolute value + // of the jerk-limit axis's unit vector term. This way when the move is finally decomposed into + // its constituent axes for execution the jerk for that axis will be at it's maximum value. + + float C; // contribution term. C = T * a + float maxC = 0; + float recip_L2 = 1 / length_square; + + for (uint8_t axis = 0; axis < AXES; axis++) + if (fabs(axis_length[axis]) > 0) { // You cannot use the fp_XXX comparisons here! + bf->unit[axis] = axis_length[axis] / bf->length; // compute unit vector term (zeros are already zero) + C = axis_square[axis] * recip_L2 * cm.a[axis].recip_jerk; // squaring axis_length ensures it's + + + if (C > maxC) { + maxC = C; + bf->jerk_axis = axis; // also needed for junction vmax calculation + } + } + + // set up and pre-compute the jerk terms needed for this round of planning + bf->jerk = cm.a[bf->jerk_axis].jerk_max * JERK_MULTIPLIER / fabs(bf->unit[bf->jerk_axis]); // scale jerk + + if (fabs(bf->jerk - mm.jerk) > JERK_MATCH_PRECISION) { // specialized comparison for tolerance of delta + mm.jerk = bf->jerk; // used before this point next time around + mm.recip_jerk = 1/bf->jerk; // compute cached jerk terms used by planning + mm.cbrt_jerk = cbrt(bf->jerk); + } + + bf->recip_jerk = mm.recip_jerk; + bf->cbrt_jerk = mm.cbrt_jerk; + + // finish up the current block variables + if (cm_get_path_control(MODEL) != PATH_EXACT_STOP) { // exact stop cases already zeroed + bf->replannable = true; + exact_stop = 8675309; // an arbitrarily large floating point number + } + + bf->cruise_vmax = bf->length / bf->gm.move_time; // target velocity requested + junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit); + bf->entry_vmax = min3(bf->cruise_vmax, junction_velocity, exact_stop); + bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); + bf->exit_vmax = min3(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax), exact_stop); + bf->braking_velocity = bf->delta_vmax; + + // Note: these next lines must remain in exact order. Position must update before committing the buffer. + _plan_block_list(bf, &mr_flag); // replan block list + copy_vector(mm.position, bf->gm.target); // set the planner position + mp_commit_write_buffer(MOVE_TYPE_ALINE); // commit current block (must follow the position update) + + return STAT_OK; +} + + +/***** ALINE HELPERS ***** + * _calc_move_times() + * _plan_block_list() + * _get_junction_vmax() + * _reset_replannable_list() + */ + +/* + * Compute optimal and minimum move times into the gcode_state + * + * "Minimum time" is the fastest the move can be performed given the velocity constraints on each + * participating axis - regardless of the feed rate requested. The minimum time is the time limited + * by the rate-limiting axis. The minimum time is needed to compute the optimal time and is + * recorded for possible feed override computation.. + * + * "Optimal time" is either the time resulting from the requested feed rate or the minimum time if + * the requested feed rate is not achievable. Optimal times for traverses are always the minimum time. + * + * The gcode state must have targets set prior by having cm_set_target(). Axis modes are taken into + * account by this. + * + * The following times are compared and the longest is returned: + * - G93 inverse time (if G93 is active) + * - time for coordinated move at requested feed rate + * - time that the slowest axis would require for the move + * + * Sets the following variables in the gcode_state struct + * - move_time is set to optimal time + * - minimum_time is set to minimum time + */ +/* --- NIST RS274NGC_v3 Guidance --- + * + * The following is verbatim text from NIST RS274NGC_v3. As I interpret A for moves that + * combine both linear and rotational movement, the feed rate should apply to the XYZ + * movement, with the rotational axis (or axes) timed to start and end at the same time + * the linear move is performed. It is possible under this case for the rotational move + * to rate-limit the linear move. + * + * 2.1.2.5 Feed Rate + * + * The rate at which the controlled point or the axes move is nominally a steady rate + * which may be set by the user. In the Interpreter, the interpretation of the feed + * rate is as follows unless inverse time feed rate mode is being used in the + * RS274/NGC view (see Section 3.5.19). The canonical machining functions view of feed + * rate, as described in Section 4.3.5.1, has conditions under which the set feed rate + * is applied differently, but none of these is used in the Interpreter. + * + * A. For motion involving one or more of the X, Y, and Z axes (with or without + * simultaneous rotational axis motion), the feed rate means length units per + * minute along the programmed XYZ path, as if the rotational axes were not moving. + * + * B. For motion of one rotational axis with X, Y, and Z axes not moving, the + * feed rate means degrees per minute rotation of the rotational axis. + * + * C. For motion of two or three rotational axes with X, Y, and Z axes not moving, + * the rate is applied as follows. Let dA, dB, and dC be the angles in degrees + * through which the A, B, and C axes, respectively, must move. + * Let D = sqrt(dA^2 + dB^2 + dC^2). Conceptually, D is a measure of total + * angular motion, using the usual Euclidean metric. Let T be the amount of + * time required to move through D degrees at the current feed rate in degrees + * per minute. The rotational axes should be moved in coordinated linear motion + * so that the elapsed time from the start to the end of the motion is T plus + * any time required for acceleration or deceleration. + */ + +static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const float axis_square[]) { + // gms = Gcode model state + float inv_time = 0; // inverse time if doing a feed in G93 mode + float xyz_time = 0; // coordinated move linear part at requested feed rate + float abc_time = 0; // coordinated move rotary part at requested feed rate + float max_time = 0; // time required for the rate-limiting axis + float tmp_time = 0; // used in computation + gms->minimum_time = 8675309; // arbitrarily large number + + // compute times for feed motion + if (gms->motion_mode != MOTION_MODE_STRAIGHT_TRAVERSE) { + if (gms->feed_rate_mode == INVERSE_TIME_MODE) { + inv_time = gms->feed_rate; // NB: feed rate was un-inverted to minutes by cm_set_feed_rate() + gms->feed_rate_mode = UNITS_PER_MINUTE_MODE; + + } else { + // compute length of linear move in millimeters. Feed rate is provided as mm/min + xyz_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + axis_square[AXIS_Z]) / gms->feed_rate; + + // if no linear axes, compute length of multi-axis rotary move in degrees. + // Feed rate is provided as degrees/min + if (fp_ZERO(xyz_time)) + abc_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + axis_square[AXIS_C]) / gms->feed_rate; + } + } + + for (uint8_t axis = 0; axis < AXES; axis++) { + if (gms->motion_mode == MOTION_MODE_STRAIGHT_TRAVERSE) + tmp_time = fabs(axis_length[axis]) / cm.a[axis].velocity_max; + + else // MOTION_MODE_STRAIGHT_FEED + tmp_time = fabs(axis_length[axis]) / cm.a[axis].feedrate_max; + + max_time = max(max_time, tmp_time); + + if (tmp_time > 0) // collect minimum time if this axis is not zero + gms->minimum_time = min(gms->minimum_time, tmp_time); + + } + + gms->move_time = max4(inv_time, max_time, xyz_time, abc_time); +} + + +/* 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 "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 or there is only + * one block and it is already running. + * + * If blocks following the first block are already optimally planned (non replannable) + * the first block that is not optimally planned becomes the effective first block. + * + * _plan_block_list() plans all blocks between and including the (effective) first block + * and the bf. It sets entry, exit and cruise v's from vmax's then calls trapezoid generation. + * + * Variables that must be provided in the mpBuffers that will be processed: + * + * bf (function arg) - end of block list (last block in time) + * bf->replannable - start of block list set by last FALSE value [Note 1] + * bf->move_type - typically MOVE_TYPE_ALINE. Other move_types should be set to + * length=0, entry_vmax=0 and exit_vmax=0 and are treated + * as a momentary stop (plan to zero and from zero). + * + * bf->length - provides block length + * bf->entry_vmax - used during forward planning to set entry velocity + * bf->cruise_vmax - used during forward planning to set cruise velocity + * bf->exit_vmax - used during forward planning to set exit velocity + * bf->delta_vmax - used during forward planning to set exit velocity + * + * bf->recip_jerk - used during trapezoid generation + * bf->cbrt_jerk - used during trapezoid generation + * + * Variables that will be set during processing: + * + * bf->replannable - set if the block becomes optimally planned + * + * bf->braking_velocity - set during backward planning + * bf->entry_velocity - set during forward planning + * bf->cruise_velocity - set during forward planning + * bf->exit_velocity - set during forward planning + * + * bf->head_length - set during trapezoid generation + * bf->body_length - set during trapezoid generation + * bf->tail_length - set during trapezoid generation + * + * Variables that are ignored but here's what you would expect them to be: + * bf->move_state - NEW for all blocks but the earliest + * bf->target[] - block target position + * bf->unit[] - block unit vector + * bf->time - gets set later + * bf->jerk - source of the other jerk variables. Used in mr. + */ +/* Notes: + * [1] Whether or not a block is planned is controlled by the bf->replannable + * setting (set TRUE if it should be). Replan flags are checked during the + * backwards pass and prune the replan list to include only the the latest + * blocks that require planning + * + * In normal operation the first block (currently running block) is not + * replanned, but may be for feedholds and feed overrides. In these cases + * the prep routines modify the contents of the mr buffer and re-shuffle + * the block list, re-enlisting the current bf buffer with new parameters. + * These routines also set all blocks in the list to be replannable so the + * list can be recomputed regardless of exact stops and previous replanning + * optimizations. + * + * [2] The mr_flag is used to tell replan to account for mr buffer's exit velocity (Vx) + * mr's Vx is always found in the provided bf buffer. Used to replan feedholds + */ +static void _plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) { + mpBuf_t *bp = bf; + + // Backward planning pass. Find first block and update the braking velocities. + // At the end *bp points to the buffer before the first block. + while ((bp = mp_get_prev_buffer(bp)) != bf) { + if (!bp->replannable) break; + bp->braking_velocity = min(bp->nx->entry_vmax, bp->nx->braking_velocity) + bp->delta_vmax; + } + + // forward planning pass - recomputes trapezoids in the list from the first block to the bf block. + while ((bp = mp_get_next_buffer(bp)) != bf) { + if ((bp->pv == bf) || (*mr_flag == true)) { + bp->entry_velocity = bp->entry_vmax; // first block in the list + *mr_flag = false; + + } else bp->entry_velocity = bp->pv->exit_velocity; // other blocks in the list + + bp->cruise_velocity = bp->cruise_vmax; + bp->exit_velocity = min4( bp->exit_vmax, + bp->nx->entry_vmax, + bp->nx->braking_velocity, + (bp->entry_velocity + bp->delta_vmax) ); + + mp_calculate_trapezoid(bp); + + // test for optimally planned trapezoids - only need to check various exit conditions + if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) || fp_EQ(bp->exit_velocity, bp->nx->entry_vmax)) || + (!bp->pv->replannable && fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax)))) + bp->replannable = false; + } + + // finish up the last block move + bp->entry_velocity = bp->pv->exit_velocity; + bp->cruise_velocity = bp->cruise_vmax; + bp->exit_velocity = 0; + mp_calculate_trapezoid(bp); +} + + +/// Resets all blocks in the planning list to be replannable +static void _reset_replannable_list() { + mpBuf_t *bf = mp_get_first_buffer(); + if (!bf) return; + + mpBuf_t *bp = bf; + + do { + bp->replannable = true; + } while (((bp = mp_get_next_buffer(bp)) != bf) && (bp->move_state != MOVE_OFF)); +} + + +/* + * Sonny's algorithm - simple + * + * Computes the maximum allowable junction speed by finding the velocity that will yield + * the centripetal acceleration in the corner_acceleration value. The value of delta sets + * the effective radius of curvature. Here's Sonny's (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), + * 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 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 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. + * + * (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: + * sin(theta/2) = R/(R+delta) Re-arranging in terms of circle radius (R) + * R = delta*sin(theta/2)/(1-sin(theta/2). + * + * Theta is the angle between line segments given by: + * 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 angle identity: + * 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." + * + * How to compute the radius using brute-force trig: + * float theta = acos(costheta); + * 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. + * + * 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: + * + * U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i]) + * Usum Length of sums Ux + Uy + * d Delta of sums (Dx*Ux+DY*UY)/Usum + */ + +static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { + float costheta = + -a_unit[AXIS_X] * b_unit[AXIS_X] - + a_unit[AXIS_Y] * b_unit[AXIS_Y] - + a_unit[AXIS_Z] * b_unit[AXIS_Z] - + a_unit[AXIS_A] * b_unit[AXIS_A] - + a_unit[AXIS_B] * b_unit[AXIS_B] - + a_unit[AXIS_C] * b_unit[AXIS_C]; + + if (costheta < -0.99) return 10000000; // straight line cases + if (costheta > 0.99) return 0; // reversal cases + + // Fuse the junction deviations into a vector sum + float a_delta = square(a_unit[AXIS_X] * cm.a[AXIS_X].junction_dev); + a_delta += square(a_unit[AXIS_Y] * cm.a[AXIS_Y].junction_dev); + a_delta += square(a_unit[AXIS_Z] * cm.a[AXIS_Z].junction_dev); + a_delta += square(a_unit[AXIS_A] * cm.a[AXIS_A].junction_dev); + a_delta += square(a_unit[AXIS_B] * cm.a[AXIS_B].junction_dev); + a_delta += square(a_unit[AXIS_C] * cm.a[AXIS_C].junction_dev); + + float b_delta = square(b_unit[AXIS_X] * cm.a[AXIS_X].junction_dev); + b_delta += square(b_unit[AXIS_Y] * cm.a[AXIS_Y].junction_dev); + b_delta += square(b_unit[AXIS_Z] * cm.a[AXIS_Z].junction_dev); + b_delta += square(b_unit[AXIS_A] * cm.a[AXIS_A].junction_dev); + b_delta += square(b_unit[AXIS_B] * cm.a[AXIS_B].junction_dev); + b_delta += square(b_unit[AXIS_C] * cm.a[AXIS_C].junction_dev); + + float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2; + float sintheta_over2 = sqrt((1 - costheta) / 2); + float radius = delta * sintheta_over2 / (1 - sintheta_over2); + float velocity = sqrt(radius * cm.junction_acceleration); + + return velocity; +} + + +/************************************************************************* + * feedholds - functions for performing holds + * + * mp_plan_hold_callback() - replan block list to execute hold + * mp_end_hold() - release the hold and restart block list + * + * Feedhold is executed as cm.hold_state transitions executed inside + * _exec_aline() and main loop callbacks to these functions: + * mp_plan_hold_callback() and mp_end_hold(). + */ +/* Holds work like this: + * + * - Hold is asserted by calling cm_feedhold() (usually invoked via a ! char) + * If hold_state is OFF and motion_state is RUNning it sets + * hold_state to SYNC and motion_state to HOLD. + * + * - Hold state == SYNC tells the aline exec routine to execute the next aline + * segment then set hold_state to PLAN. This gives the planner sufficient + * time to replan the block list for the hold before the next aline segment + * needs to be processed. + * + * - Hold state == PLAN tells the planner to replan the mr buffer, the current + * run buffer (bf), and any subsequent bf buffers as necessary to execute a + * hold. Hold planning replans the planner buffer queue down to zero and then + * back up from zero. Hold state is set to DECEL when planning is complete. + * + * - Hold state == DECEL persists until the aline execution runs to zero + * velocity, at which point hold state transitions to HOLD. + * + * - Hold state == HOLD persists until the cycle is restarted. A cycle start + * is an asynchronous event that sets the cycle_start_flag TRUE. It can + * occur any time after the hold is requested - either before or after + * motion stops. + * + * - mp_end_hold() is executed from cm_feedhold_sequencing_callback() once the + * hold state == HOLD and a cycle_start has been requested.This sets the hold + * state to OFF which enables _exec_aline() to continue processing. Move + * execution begins with the first buffer after the hold. + * + * Terms used: + * - mr is the runtime buffer. It was initially loaded from the bf buffer + * - bp+0 is the "companion" bf buffer to the mr buffer. + * - bp+1 is the bf buffer following bp+0. This runs through bp+N + * - bp (by itself) just refers to the current buffer being adjusted / replanned + * + * Details: Planning re-uses bp+0 as an "extra" buffer. Normally bp+0 is returned + * to the buffer pool as it is redundant once mr is loaded. Use the extra + * buffer to split the move in two where the hold decelerates to zero. Use + * one buffer to go to zero, the other to replan up from zero. All buffers past + * that point are unaffected other than that they need to be replanned for velocity. + * + * Note: There are multiple opportunities for more efficient organization of + * code in this module, but the code is so complicated I just left it + * organized for clarity and hoped for the best from compiler optimization. + */ + +static float _compute_next_segment_velocity() { + if (mr.section == SECTION_BODY) return mr.segment_velocity; +#ifdef __JERK_EXEC + return mr.segment_velocity; // an approximation +#else + return mr.segment_velocity + mr.forward_diff_5; +#endif +} + + +stat_t mp_plan_hold_callback() { + if (cm.hold_state != FEEDHOLD_PLAN) return STAT_NOOP; // not planning a feedhold + + mpBuf_t *bp; // working buffer pointer + if ((bp = mp_get_run_buffer()) == 0) return STAT_NOOP; // Oops! nothing's running + + uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx + float mr_available_length; // available length left in mr buffer for deceleration + float braking_velocity; // velocity left to shed to brake to zero + float braking_length; // distance required to brake to zero from braking_velocity + + // examine and process mr buffer + mr_available_length = get_axis_vector_length(mr.target, mr.position); + + // compute next_segment velocity + braking_velocity = _compute_next_segment_velocity(); + braking_length = mp_get_target_length(braking_velocity, 0, bp); // bp is OK to use here + + // Hack to prevent Case 2 moves for perfect-fit decels. Happens in homing situations + // The real fix: The braking velocity cannot simply be the mr.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 ((braking_length > mr_available_length) && (fp_ZERO(bp->exit_velocity))) + braking_length = mr_available_length; + + // Case 1: deceleration fits entirely into the length remaining in mr buffer + if (braking_length <= mr_available_length) { + // set mr to a tail to perform the deceleration + mr.exit_velocity = 0; + mr.tail_length = braking_length; + mr.cruise_velocity = braking_velocity; + mr.section = SECTION_TAIL; + mr.section_state = SECTION_NEW; + + // re-use bp+0 to be the hold point and to run the remaining block length + bp->length = mr_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->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer + + _reset_replannable_list(); // make it replan all the blocks + _plan_block_list(mp_get_last_buffer(), &mr_flag); + cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit + + return STAT_OK; + } + + // Case 2: deceleration exceeds length remaining in mr buffer + // First, replan mr to minimum (but non-zero) exit velocity + + mr.section = SECTION_TAIL; + mr.section_state = SECTION_NEW; + mr.tail_length = mr_available_length; + mr.cruise_velocity = braking_velocity; + mr.exit_velocity = braking_velocity - mp_get_target_velocity(0, mr_available_length, bp); + + // Find the point where deceleration reaches zero. This could span multiple buffers. + braking_velocity = mr.exit_velocity; // adjust braking velocity downward + bp->move_state = MOVE_NEW; // tell _exec to re-use buffer + + for (uint8_t i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) { // a safety to avoid wraparound + mp_copy_buffer(bp, bp->nx); // copy bp+1 into bp+0 (and onward...) + + if (bp->move_type != MOVE_TYPE_ALINE) { // skip any non-move buffers + bp = mp_get_next_buffer(bp); // point to next buffer + continue; + } + + bp->entry_vmax = braking_velocity; // velocity we need to shed + braking_length = mp_get_target_length(braking_velocity, 0, bp); + + if (braking_length > bp->length) { // decel does not fit in bp buffer + bp->exit_vmax = braking_velocity - mp_get_target_velocity(0, bp->length, bp); + braking_velocity = bp->exit_vmax; // braking velocity for next buffer + bp = mp_get_next_buffer(bp); // point to next buffer + continue; + } + + break; + } + + // Deceleration now fits in the current bp buffer + // Plan the first buffer of the pair as the decel, the second as the accel + bp->length = braking_length; + bp->exit_vmax = 0; + + bp = mp_get_next_buffer(bp); // point to the acceleration buffer + bp->entry_vmax = 0; + bp->length -= braking_length; // the buffers were identical (and hence their lengths) + bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); + bp->exit_vmax = bp->delta_vmax; + + _reset_replannable_list(); // make it replan all the blocks + _plan_block_list(mp_get_last_buffer(), &mr_flag); + cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit + + return STAT_OK; +} + + +/// End a feedhold +stat_t mp_end_hold() { + if (cm.hold_state == FEEDHOLD_END_HOLD) { + cm.hold_state = FEEDHOLD_OFF; + mpBuf_t *bf; + + if ((bf = mp_get_run_buffer()) == 0) { // 0 means nothing's running + cm_set_motion_state(MOTION_STOP); + return STAT_NOOP; + } + + cm.motion_state = MOTION_RUN; + st_request_exec_move(); // restart the steppers + } + + return STAT_OK; +} diff --git a/src/plan/planner.c b/src/plan/planner.c new file mode 100644 index 0000000..0696044 --- /dev/null +++ b/src/plan/planner.c @@ -0,0 +1,412 @@ +/* + * planner.c - Cartesian trajectory planning and motion execution + * This file is part of the TinyG project + * + * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2015 Rob Giseburt + * + * 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 . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* --- Planner Notes ---- + * + * The planner works below the canonical machine and above the motor mapping + * and stepper execution layers. A rudimentary multitasking capability is + * implemented for long-running commands such as lines, arcs, and dwells. + * These functions are coded as non-blocking continuations - which are simple + * state machines that are re-entered multiple times until a particular + * operation is complete. These functions have 2 parts - the initial call, + * which sets up the local context, and callbacks (continuations) that are + * called from the main loop (in controller.c). + * + * One important concept is isolation of the three layers of the data model - + * the Gcode model (gm), planner model (bf queue & mm), and runtime model (mr). + * These are designated as "model", "planner" and "runtime" in function names. + * + * The Gcode model is owned by the canonical machine and should only be accessed + * by cm_xxxx() functions. Data from the Gcode model is transferred to the planner + * by the mp_xxx() functions called by the canonical machine. + * + * The planner should only use data in the planner model. When a move (block) + * is ready for execution the planner data is transferred to the runtime model, + * which should also be isolated. + * + * Lower-level models should never use data from upper-level models as the data + * may have changed and lead to unpredictable results. + */ + +#include "planner.h" +#include "arc.h" +#include "canonical_machine.h" +#include "kinematics.h" +#include "stepper.h" +#include "encoder.h" + +#include +#include +#include + + +mpBufferPool_t mb; // move buffer queue +mpMoveMasterSingleton_t mm; // context for line planning +mpMoveRuntimeSingleton_t mr; // context for line runtime + +#define _bump(a) ((a < PLANNER_BUFFER_POOL_SIZE - 1) ? a + 1 : 0) // buffer incr & wrap +#define spindle_speed move_time // local alias for spindle_speed to the time variable +#define value_vector gm.target // alias for vector of values +#define flag_vector unit // alias for vector of flags + + +// execution routines (NB: These are all called from the LO interrupt) +static stat_t _exec_dwell(mpBuf_t *bf); +static stat_t _exec_command(mpBuf_t *bf); + + +void planner_init() { + // If you know all memory has been zeroed by a hard reset you don't need these next 2 lines + memset(&mr, 0, sizeof(mr)); // clear all values, pointers and status + memset(&mm, 0, sizeof(mm)); // clear all values, pointers and status + mp_init_buffers(); +} + + +/* + * Flush all moves in the planner and all arcs + * + * Does not affect the move currently running in mr. + * Does not affect mm or gm model positions + * This function is designed to be called during a hold to reset the planner + * This function should not generally be called; call cm_queue_flush() instead + */ +void mp_flush_planner() { + cm_abort_arc(); + mp_init_buffers(); + cm_set_motion_state(MOTION_STOP); +} + +/* + * mp_set_planner_position() - set planner position for a single axis + * mp_set_runtime_position() - set runtime position for a single axis + * mp_set_steps_to_runtime_position() - set encoder counts to the runtime position + * + * Since steps are in motor space you have to run the position vector through inverse + * kinematics to get the right numbers. This means that in a non-Cartesian robot changing + * any position can result in changes to multiple step values. So this operation is provided + * as a single function and always uses the new position vector as an input. + * + * Keeping track of position is complicated by the fact that moves exist in several reference + * frames. The scheme to keep this straight is: + * + * - mm.position - start and end position for planning + * - mr.position - current position of runtime segment + * - mr.target - target position of runtime segment + * - mr.endpoint - final target position of runtime segment + * + * Note that position is set immediately when called and may not be not an accurate representation + * of the tool position. The motors are still processing the action and the real tool position is + * still close to the starting point. + */ + +void mp_set_planner_position(uint8_t axis, const float position) {mm.position[axis] = position;} +void mp_set_runtime_position(uint8_t axis, const float position) {mr.position[axis] = position;} + +void mp_set_steps_to_runtime_position() { + float step_position[MOTORS]; + + ik_kinematics(mr.position, step_position); // convert lengths to steps in floating point + + for (uint8_t motor = 0; motor < MOTORS; motor++) { + mr.target_steps[motor] = step_position[motor]; + mr.position_steps[motor] = step_position[motor]; + mr.commanded_steps[motor] = step_position[motor]; + en_set_encoder_steps(motor, step_position[motor]); // write steps to encoder register + + // These must be zero: + mr.following_error[motor] = 0; + st_pre.mot[motor].corrected_steps = 0; + } +} + + +/************************************************************************************ + * mp_queue_command() - queue a synchronous Mcode, program control, or other command + * _exec_command() - callback to execute command + * + * How this works: + * - The command is called by the Gcode interpreter (cm_, e.g. an M code) + * - cm_ function calls mp_queue_command which puts it in the planning queue (bf buffer). + * This involves setting some parameters and registering a callback to the + * execution function in the canonical machine + * - the planning queue gets to the function and calls _exec_command() + * - ...which puts a pointer to the bf buffer in the prep stratuc (st_pre) + * - When the runtime gets to the end of the current activity (sending steps, counting a dwell) + * if executes mp_runtime_command... + * - ...which uses the callback function in the bf and the saved parameters in the vectors + * - To finish up mp_runtime_command() needs to free the bf buffer + * + * 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 much easier - therefore avoiding Q starvation + */ + +void mp_queue_command(void(*cm_exec)(float[], float[]), float *value, float *flag) { + mpBuf_t *bf; + + // Never supposed to fail as buffer availability was checked upstream in the controller + if ((bf = mp_get_write_buffer()) == 0) { + cm_hard_alarm(STAT_BUFFER_FULL_FATAL); + return; + } + + bf->move_type = MOVE_TYPE_COMMAND; + bf->bf_func = _exec_command; // callback to planner queue exec function + bf->cm_func = cm_exec; // callback to canonical machine exec function + + for (uint8_t axis = AXIS_X; axis < AXES; axis++) { + bf->value_vector[axis] = value[axis]; + bf->flag_vector[axis] = flag[axis]; + } + + mp_commit_write_buffer(MOVE_TYPE_COMMAND); // must be final operation before exit +} + + +static stat_t _exec_command(mpBuf_t *bf) { + st_prep_command(bf); + return STAT_OK; +} + + +stat_t mp_runtime_command(mpBuf_t *bf) { + bf->cm_func(bf->value_vector, bf->flag_vector); // 2 vectors used by callbacks + + // free buffer & perform cycle_end if planner is empty + if (mp_free_run_buffer()) cm_cycle_end(); + + return STAT_OK; +} + + +/************************************************************************* + * mp_dwell() - queue a dwell + * _exec_dwell() - dwell execution + * + * Dwells are performed by passing a dwell move to the stepper drivers. + * When the stepper driver sees a dwell it times the dwell on a separate + * timer than the stepper pulse timer. + */ +stat_t mp_dwell(float seconds) { + mpBuf_t *bf; + + if ((bf = mp_get_write_buffer()) == 0) // get write buffer or fail + return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // not ever supposed to fail + + bf->bf_func = _exec_dwell; // register callback to dwell start + bf->gm.move_time = seconds; // in seconds, not minutes + bf->move_state = MOVE_NEW; + mp_commit_write_buffer(MOVE_TYPE_DWELL); // must be final operation before exit + + return STAT_OK; +} + + +static stat_t _exec_dwell(mpBuf_t *bf) { + st_prep_dwell((uint32_t)(bf->gm.move_time * 1000000)); // convert seconds to uSec + if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & perform cycle_end if planner is empty + + return STAT_OK; +} + +/**** PLANNER BUFFERS ***************************************************** + * + * Planner buffers are used to queue and operate on Gcode blocks. Each buffer + * contains one Gcode block which may be a move, and M code, or other command + * that must be executed synchronously with movement. + * + * Buffers are in a circularly linked list managed by a WRITE pointer and a RUN pointer. + * New blocks are populated by (1) getting a write buffer, (2) populating the buffer, + * then (3) placing it in the queue (queue write buffer). If an exception occurs + * during population you can unget the write buffer before queuing it, which returns + * it to the pool of available buffers. + * + * The RUN buffer is the buffer currently executing. It may be retrieved once for + * simple commands, or multiple times for long-running commands like moves. When + * the command is complete the run buffer is returned to the pool by freeing it. + * + * Notes: + * The write buffer pointer only moves forward on _queue_write_buffer, and + * the read buffer pointer only moves forward on free_read calls. + * (test, get and unget have no effect) + * + * mp_get_planner_buffers_available() Returns # of available planner buffers + * + * mp_init_buffers() Initializes or resets buffers + * + * mp_get_write_buffer() Get pointer to next available write buffer + * Returns pointer or 0 if no buffer available. + * + * mp_unget_write_buffer() Free write buffer if you decide not to commit it. + * + * mp_commit_write_buffer() Commit the next write buffer to the queue + * Advances write pointer & changes buffer state + * WARNING: The calling routine must not use the write buffer + * once it has been queued as it may be processed and freed (wiped) + * before mp_queue_write_buffer() returns. + * + * mp_get_run_buffer() Get pointer to the next or current run buffer + * Returns a new run buffer if prev buf was ENDed + * Returns same buf if called again before ENDing + * Returns 0 if no buffer available + * The behavior supports continuations (iteration) + * + * mp_free_run_buffer() Release the run buffer & return to buffer pool. + * Returns true if queue is empty, false otherwise. + * This is useful for doing queue empty / end move functions. + * + * mp_get_prev_buffer(bf) Returns pointer to prev buffer in linked list + * mp_get_next_buffer(bf) Returns pointer to next buffer in linked list + * mp_get_first_buffer(bf) Returns pointer to first buffer, i.e. the running block + * mp_get_last_buffer(bf) Returns pointer to last buffer, i.e. last block (zero) + * mp_clear_buffer(bf) Zeroes the contents of the buffer + * mp_copy_buffer(bf,bp) Copies the contents of bp into bf - preserves links + */ + +uint8_t mp_get_planner_buffers_available() {return mb.buffers_available;} + + +void mp_init_buffers() { + mpBuf_t *pv; + + memset(&mb, 0, sizeof(mb)); // clear all values, pointers and status + + mb.w = &mb.bf[0]; // init write and read buffer pointers + mb.q = &mb.bf[0]; + mb.r = &mb.bf[0]; + pv = &mb.bf[PLANNER_BUFFER_POOL_SIZE - 1]; + + for (uint8_t i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) { // setup ring pointers + mb.bf[i].nx = &mb.bf[_bump(i)]; + mb.bf[i].pv = pv; + pv = &mb.bf[i]; + } + + mb.buffers_available = PLANNER_BUFFER_POOL_SIZE; +} + + +mpBuf_t *mp_get_write_buffer() { // get & clear a buffer + if (mb.w->buffer_state == MP_BUFFER_EMPTY) { + mpBuf_t *w = mb.w; + mpBuf_t *nx = mb.w->nx; // save linked list pointers + mpBuf_t *pv = mb.w->pv; + memset(mb.w, 0, sizeof(mpBuf_t)); // clear all values + w->nx = nx; // restore pointers + w->pv = pv; + w->buffer_state = MP_BUFFER_LOADING; + mb.buffers_available--; + mb.w = w->nx; + + return w; + } + + return 0; +} + + +void mp_unget_write_buffer() { + mb.w = mb.w->pv; // queued --> write + mb.w->buffer_state = MP_BUFFER_EMPTY; // not loading anymore + mb.buffers_available++; +} + + +/*** WARNING: The routine calling mp_commit_write_buffer() must not use the write buffer + once it has been queued. Action may start on the buffer immediately, + invalidating its contents ***/ +void mp_commit_write_buffer(const uint8_t move_type) { + mb.q->move_type = move_type; + mb.q->move_state = MOVE_NEW; + mb.q->buffer_state = MP_BUFFER_QUEUED; + mb.q = mb.q->nx; // advance the queued buffer pointer + st_request_exec_move(); // requests an exec if the runtime is not busy + // NB: BEWARE! the exec may result in the planner buffer being + // processed immediately and then freed - invalidating the contents +} + + +mpBuf_t *mp_get_run_buffer() { + // CASE: fresh buffer; becomes running if queued or pending + if ((mb.r->buffer_state == MP_BUFFER_QUEUED) || + (mb.r->buffer_state == MP_BUFFER_PENDING)) + mb.r->buffer_state = MP_BUFFER_RUNNING; + + // CASE: asking for the same run buffer for the Nth time + if (mb.r->buffer_state == MP_BUFFER_RUNNING) return mb.r; // return same buffer + + return 0; // CASE: no queued buffers. fail it. +} + + +uint8_t mp_free_run_buffer() { // EMPTY current run buf & adv to next + mp_clear_buffer(mb.r); // clear it out (& reset replannable) + mb.r = mb.r->nx; // advance to next run buffer + + if (mb.r->buffer_state == MP_BUFFER_QUEUED) // only if queued... + mb.r->buffer_state = MP_BUFFER_PENDING; // pend next buffer + + mb.buffers_available++; + + return mb.w == mb.r; // return true if the queue emptied +} + + +mpBuf_t *mp_get_first_buffer() { + return mp_get_run_buffer(); // returns buffer or 0 if nothing's running +} + + +mpBuf_t *mp_get_last_buffer() { + mpBuf_t *bf = mp_get_run_buffer(); + mpBuf_t *bp; + + for (bp = bf; bp && bp->nx != bf; bp = mp_get_next_buffer(bp)) + if (bp->nx->move_state == MOVE_OFF) break; + + return bp; +} + + +void mp_clear_buffer(mpBuf_t *bf) { + mpBuf_t *nx = bf->nx; // save pointers + mpBuf_t *pv = bf->pv; + memset(bf, 0, sizeof(mpBuf_t)); + bf->nx = nx; // restore pointers + bf->pv = pv; +} + + +void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp) { + mpBuf_t *nx = bf->nx; // save pointers + mpBuf_t *pv = bf->pv; + memcpy(bf, bp, sizeof(mpBuf_t)); + bf->nx = nx; // restore pointers + bf->pv = pv; +} diff --git a/src/plan/planner.h b/src/plan/planner.h new file mode 100644 index 0000000..8e37872 --- /dev/null +++ b/src/plan/planner.h @@ -0,0 +1,305 @@ +/* + * planner.h - cartesian trajectory planning and motion execution + * This file is part of the TinyG project + * + * Copyright (c) 2013 - 2015 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2015 Robert Giseburt + * + * 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 . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#ifndef PLANNER_H +#define PLANNER_H + +#include "canonical_machine.h" // used for GCodeState_t +#include "config.h" + +enum moveType { // bf->move_type values + MOVE_TYPE_0 = 0, // 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_TOOL, // T command + MOVE_TYPE_SPINDLE_SPEED, // S command + MOVE_TYPE_STOP, // program stop + MOVE_TYPE_END // program end +}; + +enum moveState { + MOVE_OFF = 0, // move inactive (MUST BE ZERO) + MOVE_NEW, // general value if you need an initialization + MOVE_RUN, // general run state (for non-acceleration moves) + MOVE_SKIP_BLOCK // mark a skipped block +}; + +enum moveSection { + SECTION_HEAD = 0, // acceleration + SECTION_BODY, // cruise + SECTION_TAIL // deceleration +}; +#define SECTIONS 3 + +enum sectionState { + SECTION_OFF = 0, // section inactive + SECTION_NEW, // uninitialized section + SECTION_1st_HALF, // first half of S curve + SECTION_2nd_HALF // second half of S curve or running a BODY (cruise) +}; + +// Most of these factors are the result of a lot of tweaking. Change with caution. +#define ARC_SEGMENT_LENGTH ((float)0.1) // Arc segment size (mm).(0.03) +#define MIN_ARC_RADIUS ((float)0.1) + +#define JERK_MULTIPLIER ((float)1000000) +#define JERK_MATCH_PRECISION ((float)1000) // precision jerk must match to be considered same + +#define NOM_SEGMENT_USEC ((float)5000) // nominal segment time +#define MIN_SEGMENT_USEC ((float)2500) // minimum segment time / minimum move time +#define MIN_ARC_SEGMENT_USEC ((float)10000) // minimum arc segment time + +#define NOM_SEGMENT_TIME (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) +#define MIN_SEGMENT_TIME (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) +#define MIN_ARC_SEGMENT_TIME (MIN_ARC_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) +#define MIN_TIME_MOVE MIN_SEGMENT_TIME // minimum time a move can be is one segment +#define MIN_BLOCK_TIME MIN_SEGMENT_TIME // factor for minimum size Gcode block to process + +#define MIN_SEGMENT_TIME_PLUS_MARGIN ((MIN_SEGMENT_USEC+1) / MICROSECONDS_PER_MINUTE) + +/* PLANNER_STARTUP_DELAY_SECONDS + * Used to introduce a short dwell before planning an idle machine. + * If you don't do this the first block will always plan to zero as it will + * start executing before the next block arrives from the serial port. + * This causes the machine to stutter once on startup. + */ +//#define PLANNER_STARTUP_DELAY_SECONDS ((float)0.05) // in seconds + +/* PLANNER_BUFFER_POOL_SIZE + * Should be at least the number of buffers requires to support optimal + * planning in the case of very short lines or arc segments. + * Suggest 12 min. Limit is 255 + */ +#define PLANNER_BUFFER_POOL_SIZE 32 +#define PLANNER_BUFFER_HEADROOM 4 // buffers to reserve in planner before processing new input line + +/* Some parameters for _generate_trapezoid() + * TRAPEZOID_ITERATION_MAX Max iterations for convergence in the HT asymmetric case. + * TRAPEZOID_ITERATION_ERROR_PERCENT Error percentage for iteration convergence. As percent - 0.01 = 1% + * TRAPEZOID_LENGTH_FIT_TOLERANCE Tolerance for "exact fit" for H and T cases + * TRAPEZOID_VELOCITY_TOLERANCE Adaptive velocity tolerance term + */ +#define TRAPEZOID_ITERATION_MAX 10 +#define TRAPEZOID_ITERATION_ERROR_PERCENT ((float)0.10) +#define TRAPEZOID_LENGTH_FIT_TOLERANCE ((float)0.0001) // allowable mm of error in planning phase +#define TRAPEZOID_VELOCITY_TOLERANCE (max(2, bf->entry_velocity / 100)) + + +typedef void (*cm_exec_t)(float[], float[]); // callback to canonical_machine execution function + + +// All the enums that equal zero must be zero. Don't change this +enum mpBufferState { // bf->buffer_state values + MP_BUFFER_EMPTY = 0, // struct is available for use (MUST BE 0) + MP_BUFFER_LOADING, // being written ("checked out") + MP_BUFFER_QUEUED, // in queue + MP_BUFFER_PENDING, // marked as the next buffer to run + MP_BUFFER_RUNNING // current running buffer +}; + + +typedef struct mpBuffer { // See Planning Velocity Notes for variable usage + struct mpBuffer *pv; // static pointer to previous buffer + struct mpBuffer *nx; // static pointer to next buffer + stat_t (*bf_func)(struct mpBuffer *bf); // callback to buffer exec function + cm_exec_t cm_func; // callback to canonical machine execution function + + float naiive_move_time; + + uint8_t buffer_state; // used to manage queuing/dequeuing + uint8_t move_type; // used to dispatch to run routine + uint8_t move_code; // byte that can be used by used exec functions + uint8_t move_state; // move state machine sequence + uint8_t replannable; // TRUE if move can be re-planned + + float unit[AXES]; // unit vector for axis scaling & planning + + float length; // total length of line or helix in mm + float head_length; + float body_length; + float tail_length; + // *** SEE NOTES ON THESE VARIABLES, in aline() *** + float entry_velocity; // entry velocity requested for the move + float cruise_velocity; // cruise velocity requested & achieved + float exit_velocity; // exit velocity requested for the move + + float entry_vmax; // max junction velocity at entry of this move + float cruise_vmax; // max cruise velocity requested for move + float exit_vmax; // max exit velocity possible (redundant) + float delta_vmax; // max velocity difference for this move + float braking_velocity; // current value for braking velocity + + uint8_t jerk_axis; // rate limiting axis used to compute jerk for the move + float jerk; // maximum linear jerk term for this move + float recip_jerk; // 1/Jm used for planning (computed and cached) + float cbrt_jerk; // cube root of Jm used for planning (computed and cached) + + GCodeState_t gm; // Gode model state - passed from model, used by planner and runtime + +} mpBuf_t; + + +typedef struct mpBufferPool { // ring buffer for sub-moves + uint8_t buffers_available; // running count of available buffers + mpBuf_t *w; // get_write_buffer pointer + mpBuf_t *q; // queue_write_buffer pointer + mpBuf_t *r; // get/end_run_buffer pointer + mpBuf_t bf[PLANNER_BUFFER_POOL_SIZE]; // buffer storage +} mpBufferPool_t; + + +typedef struct mpMoveMasterSingleton { // common variables for planning (move master) + float position[AXES]; // final move position for planning purposes + + float jerk; // jerk values cached from previous block + float recip_jerk; + float cbrt_jerk; +} mpMoveMasterSingleton_t; + + +typedef struct mpMoveRuntimeSingleton { // persistent runtime variables + uint8_t move_state; // state of the overall move + uint8_t section; // what section is the move in? + uint8_t section_state; // state within a move section + + float unit[AXES]; // unit vector for axis scaling & planning + float target[AXES]; // final target for bf (used to correct rounding errors) + float position[AXES]; // current move position + float position_c[AXES]; // for Kahan summation in _exec_aline_segment() + float waypoint[SECTIONS][AXES]; // head/body/tail endpoints for correction + + float target_steps[MOTORS]; // current MR target (absolute target as steps) + float position_steps[MOTORS]; // current MR position (target from previous segment) + float commanded_steps[MOTORS]; // will align with next encoder sample (target 2nd previous segment) + float encoder_steps[MOTORS]; // encoder position in steps - ideally the same as commanded_steps + float following_error[MOTORS]; // difference between encoder_steps and commanded steps + + float head_length; // copies of bf variables of same name + float body_length; + float tail_length; + + float entry_velocity; + float cruise_velocity; + float exit_velocity; + + float segments; // number of segments in line (also used by arc generation) + uint32_t segment_count; // count of running segments + float segment_velocity; // computed velocity for aline segment + float segment_time; // actual time increment per aline segment + float jerk; // max linear jerk + +#ifdef __JERK_EXEC // values used exclusively by computed jerk acceleration + float jerk_div2; // cached value for efficiency + float midpoint_velocity; // velocity at accel/decel midpoint + float midpoint_acceleration; + float accel_time; + float segment_accel_time; + float elapsed_accel_time; +#else // values used exclusively by forward differencing acceleration + float forward_diff_1; // forward difference level 1 + float forward_diff_2; // forward difference level 2 + float forward_diff_3; // forward difference level 3 + float forward_diff_4; // forward difference level 4 + float forward_diff_5; // forward difference level 5 +#ifdef __KAHAN + float forward_diff_1_c; // forward difference level 1 floating-point compensation + float forward_diff_2_c; // forward difference level 2 floating-point compensation + float forward_diff_3_c; // forward difference level 3 floating-point compensation + float forward_diff_4_c; // forward difference level 4 floating-point compensation + float forward_diff_5_c; // forward difference level 5 floating-point compensation +#endif +#endif + + GCodeState_t gm; // gcode model state currently executing +} mpMoveRuntimeSingleton_t; + + +// Reference global scope structures +extern mpBufferPool_t mb; // move buffer queue +extern mpMoveMasterSingleton_t mm; // context for line planning +extern mpMoveRuntimeSingleton_t mr; // context for line runtime + +void planner_init(); +void planner_init_assertions(); +stat_t planner_test_assertions(); + +void mp_flush_planner(); +void mp_set_planner_position(uint8_t axis, const float position); +void mp_set_runtime_position(uint8_t axis, const float position); +void mp_set_steps_to_runtime_position(); + +void mp_queue_command(void(*cm_exec_t)(float[], float[]), float *value, float *flag); +stat_t mp_runtime_command(mpBuf_t *bf); + +stat_t mp_dwell(const float seconds); +void mp_end_dwell(); + +stat_t mp_aline(GCodeState_t *gm_in); + +stat_t mp_plan_hold_callback(); +stat_t mp_end_hold(); +stat_t mp_feed_rate_override(uint8_t flag, float parameter); + +// planner buffer handlers +uint8_t mp_get_planner_buffers_available(); +void mp_init_buffers(); +mpBuf_t * mp_get_write_buffer(); +void mp_unget_write_buffer(); +void mp_commit_write_buffer(const uint8_t move_type); + +mpBuf_t *mp_get_run_buffer(); +uint8_t mp_free_run_buffer(); +mpBuf_t *mp_get_first_buffer(); +mpBuf_t *mp_get_last_buffer(); + +#define mp_get_prev_buffer(b) ((mpBuf_t *)(b->pv)) // use the macro instead +#define mp_get_next_buffer(b) ((mpBuf_t *)(b->nx)) + +void mp_clear_buffer(mpBuf_t *bf); +void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp); + +// line.c functions +float mp_get_runtime_velocity(); +float mp_get_runtime_work_position(uint8_t axis); +float mp_get_runtime_absolute_position(uint8_t axis); +void mp_set_runtime_work_offset(float offset[]); +void mp_zero_segment_velocity(); +uint8_t mp_get_runtime_busy(); +float* mp_get_planner_position_vector(); + +// zoid.c functions +void mp_calculate_trapezoid(mpBuf_t *bf); +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); + +// exec.c functions +stat_t mp_exec_move(); +stat_t mp_exec_aline(mpBuf_t *bf); + +#endif // PLANNER_H diff --git a/src/plan_zoid.c b/src/plan/zoid.c similarity index 99% rename from src/plan_zoid.c rename to src/plan/zoid.c index 7b289f5..8b2dbfd 100644 --- a/src/plan_zoid.c +++ b/src/plan/zoid.c @@ -1,5 +1,5 @@ /* - * plan_zoid.c - acceleration managed line planning and motion execution - trapezoid planner + * zoid.c - acceleration managed line planning and motion execution - trapezoid planner * This file is part of the TinyG project * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. @@ -26,12 +26,11 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" -#include "config.h" #include "planner.h" -#include "report.h" #include "util.h" +#include + /* * mp_calculate_trapezoid() - calculate trapezoid parameters * diff --git a/src/plan_arc.c b/src/plan_arc.c deleted file mode 100644 index beda066..0000000 --- a/src/plan_arc.c +++ /dev/null @@ -1,464 +0,0 @@ -/* - * plan_arc.c - arc planning and motion execution - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - * - * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* This module actually contains some parts that belong ion the canonical machine, - * and other parts that belong at the motion planner level, but the whole thing is - * treated as if it were part of the motion planner. - */ - -#include "tinyg.h" -#include "config.h" -#include "canonical_machine.h" -#include "plan_arc.h" -#include "planner.h" -#include "util.h" - -// Allocate arc planner singleton structure - -arc_t arc; - -// Local functions -static stat_t _compute_arc(); -static stat_t _compute_arc_offsets_from_radius(); -static void _estimate_arc_time(); -//static stat_t _test_arc_soft_limits(); - -/***************************************************************************** - * Canonical Machining arc functions (arc prep for planning and runtime) - * - * cm_arc_init() - initialize arcs - * cm_arc_feed() - canonical machine entry point for arc - * cm_arc_callback() - mail-loop callback for arc generation - * cm_abort_arc() - stop an arc in process - */ - -/* - * cm_arc_init() - initialize arc structures - */ -void cm_arc_init() -{ - arc.magic_start = MAGICNUM; - arc.magic_end = MAGICNUM; -} - -/* - * cm_arc_feed() - canonical machine entry point for arc - * - * Generates an arc by queuing line segments to the move buffer. The arc is - * approximated by generating a large number of tiny, linear arc_segments. - */ -stat_t cm_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 -{ - //////////////////////////////////////////////////// - // Set axis plane and trap arc specification errors - - // trap missing feed rate - if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) { - return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - } - - // set radius mode flag and do simple test(s) - bool radius_f = fp_NOT_ZERO(cm.gf.arc_radius); // set true if radius arc - if ((radius_f) && (cm.gn.arc_radius < MIN_ARC_RADIUS)) { // radius value must be + and > minimum radius - return STAT_ARC_RADIUS_OUT_OF_TOLERANCE; - } - - // setup some flags - bool target_x = fp_NOT_ZERO(flags[AXIS_X]); // set true if X axis has been specified - bool target_y = fp_NOT_ZERO(flags[AXIS_Y]); - bool target_z = fp_NOT_ZERO(flags[AXIS_Z]); - - bool offset_i = fp_NOT_ZERO(cm.gf.arc_offset[0]); // set true if offset I has been specified - bool offset_j = fp_NOT_ZERO(cm.gf.arc_offset[1]); // J - bool offset_k = fp_NOT_ZERO(cm.gf.arc_offset[2]); // K - - // Set the arc plane for the current G17/G18/G19 setting and test arc specification - // Plane axis 0 and 1 are the arc plane, the linear axis is normal to the arc plane. - if (cm.gm.select_plane == CANON_PLANE_XY) { // G17 - the vast majority of arcs are in the G17 (XY) plane - arc.plane_axis_0 = AXIS_X; - arc.plane_axis_1 = AXIS_Y; - arc.linear_axis = AXIS_Z; - if (radius_f) { - if (!(target_x || target_y)) { // must have at least one endpoint specified - return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; - } - } else { // center format arc tests - if (offset_k) { // it's OK to be missing either or both i and j, but error if k is present - return STAT_ARC_SPECIFICATION_ERROR; - } - } - - } else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18 - arc.plane_axis_0 = AXIS_X; - arc.plane_axis_1 = AXIS_Z; - arc.linear_axis = AXIS_Y; - if (radius_f) { - if (!(target_x || target_z)) - return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; - } else { - if (offset_j) - return STAT_ARC_SPECIFICATION_ERROR; - } - - } else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19 - arc.plane_axis_0 = AXIS_Y; - arc.plane_axis_1 = AXIS_Z; - arc.linear_axis = AXIS_X; - if (radius_f) { - if (!(target_y || target_z)) - return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; - } else { - if (offset_i) - return STAT_ARC_SPECIFICATION_ERROR; - } - } - - // set values in the Gcode model state & copy it (linenum was already captured) - cm_set_model_target(target, flags); - - // in radius mode it's an error for start == end - if(radius_f) { - if ((fp_EQ(cm.gmx.position[AXIS_X], cm.gm.target[AXIS_X])) && - (fp_EQ(cm.gmx.position[AXIS_Y], cm.gm.target[AXIS_Y])) && - (fp_EQ(cm.gmx.position[AXIS_Z], cm.gm.target[AXIS_Z]))) { - return STAT_ARC_ENDPOINT_IS_STARTING_POINT; - } - } - - // now get down to the rest of the work setting up the arc for execution - cm.gm.motion_mode = motion_mode; - cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to gm - memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // copy GCode context to arc singleton - some will be overwritten to run segments - copy_vector(arc.position, cm.gmx.position); // set initial arc position from gcode model - - arc.radius = _to_millimeters(radius); // set arc radius or zero - - arc.offset[0] = _to_millimeters(i); // copy offsets with conversion to canonical form (mm) - arc.offset[1] = _to_millimeters(j); - arc.offset[2] = _to_millimeters(k); - - arc.rotations = floor(fabs(cm.gn.parameter)); // P must be a positive integer - force it if not - - // determine if this is a full circle arc. Evaluates true if no target is set - arc.full_circle = (fp_ZERO(flags[arc.plane_axis_0]) & fp_ZERO(flags[arc.plane_axis_1])); - - // compute arc runtime values - ritorno(_compute_arc()); - - if (fp_ZERO(arc.length)) { - return STAT_MINIMUM_LENGTH_MOVE; // trap zero length arcs that _compute_arc can throw - } - - cm_cycle_start(); // if not already started - arc.run_state = MOVE_RUN; // enable arc to be run from the callback - cm_finalize_move(); - return STAT_OK; -} - -/* - * cm_arc_callback() - generate an arc - * - * cm_arc_callback() is called from the controller main loop. Each time it's called it - * queues as many arc segments (lines) as it can before it blocks, then returns. - * - * Parts of this routine were originally sourced from the grbl project. - */ - -stat_t cm_arc_callback() -{ - if (arc.run_state == MOVE_OFF) - return STAT_NOOP; - - if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) - return STAT_EAGAIN; - - arc.theta += arc.arc_segment_theta; - arc.gm.target[arc.plane_axis_0] = arc.center_0 + sin(arc.theta) * arc.radius; - arc.gm.target[arc.plane_axis_1] = arc.center_1 + cos(arc.theta) * arc.radius; - arc.gm.target[arc.linear_axis] += arc.arc_segment_linear_travel; - mp_aline(&arc.gm); // run the line - copy_vector(arc.position, arc.gm.target); // update arc current position - - if (--arc.arc_segment_count > 0) - return STAT_EAGAIN; - arc.run_state = MOVE_OFF; - return STAT_OK; -} - -/* - * cm_abort_arc() - stop arc movement without maintaining position - * - * OK to call if no arc is running - */ - -void cm_abort_arc() -{ - arc.run_state = MOVE_OFF; -} - -/* - * _compute_arc() - compute arc from I and J (arc center point) - * - * The theta calculation sets up an clockwise or counterclockwise arc from the current - * position to the target position around the center designated by the offset vector. - * All theta-values measured in radians of deviance from the positive y-axis. - * - * | <- theta == 0 - * * * * - * * * - * * * - * * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2) - * * / - * C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4)) - * - * Parts of this routine were originally sourced from the grbl project. - */ - -static stat_t _compute_arc() -{ - // Compute radius. A non-zero radius value indicates a radius arc - if (fp_NOT_ZERO(arc.radius)) { // indicates a radius arc - _compute_arc_offsets_from_radius(); - } else { // compute start radius - arc.radius = hypotf(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]); - } - - // Test arc specification for correctness according to: - // http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc - // "It is an error if: when the arc is projected on the selected plane, the distance from - // the current point to the center differs from the distance from the end point to the - // center by more than (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)." - - // Compute end radius from the center of circle (offsets) to target endpoint - float end_0 = arc.gm.target[arc.plane_axis_0] - arc.position[arc.plane_axis_0] - arc.offset[arc.plane_axis_0]; - float end_1 = arc.gm.target[arc.plane_axis_1] - arc.position[arc.plane_axis_1] - arc.offset[arc.plane_axis_1]; - float err = fabs(hypotf(end_0, end_1) - arc.radius); // end radius - start radius - if ( (err > ARC_RADIUS_ERROR_MAX) || - ((err < ARC_RADIUS_ERROR_MIN) && - (err > arc.radius * ARC_RADIUS_TOLERANCE)) ) { -// return STAT_ARC_HAS_IMPOSSIBLE_CENTER_POINT; - return STAT_ARC_SPECIFICATION_ERROR; - } - - // Calculate the theta (angle) of the current point (position) - // arc.theta is angular starting point for the arc (also needed later for calculating center point) - arc.theta = atan2(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]); - - // g18_correction is used to invert G18 XZ plane arcs for proper CW orientation - float g18_correction = (cm.gm.select_plane == CANON_PLANE_XZ) ? -1 : 1; - - if (arc.full_circle) { // if full circle you can skip the stuff in the else clause - arc.angular_travel = 0; // angular travel always starts as zero for full circles - if (fp_ZERO(arc.rotations)) { // handle the valid case of a full circle arc w/P=0 - arc.rotations = 1.0; - } - } else { // ... it's not a full circle - arc.theta_end = atan2(end_0, end_1); - - // Compute the angular travel - if (fp_EQ(arc.theta_end, arc.theta)) { - arc.angular_travel = 0; // very large radii arcs can have zero angular travel (thanks PartKam) - } else { - if (arc.theta_end < arc.theta) { // make the difference positive so we have clockwise travel - arc.theta_end += (2*M_PI * g18_correction); - } - arc.angular_travel = arc.theta_end - arc.theta; // compute positive angular travel - if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) { // reverse travel direction if it's CCW arc - arc.angular_travel -= (2*M_PI * g18_correction); - } - } - } - - // Add in travel for rotations - if (cm.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); - } - - // 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 the helix (or just a planar arc) - arc.linear_travel = arc.gm.target[arc.linear_axis] - arc.position[arc.linear_axis]; - arc.planar_travel = arc.angular_travel * arc.radius; - arc.length = hypotf(arc.planar_travel, arc.linear_travel); // NB: hypot is insensitive to +/- signs - _estimate_arc_time(); // get an estimate of execution time to inform arc_segment calculation - - // Find the minimum number of arc_segments that meets these constraints... - float arc_segments_for_chordal_accuracy = arc.length / sqrt(4*cm.chordal_tolerance * (2 * arc.radius - cm.chordal_tolerance)); - float arc_segments_for_minimum_distance = arc.length / cm.arc_segment_len; - float arc_segments_for_minimum_time = arc.arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC; - - arc.arc_segments = floor(min3(arc_segments_for_chordal_accuracy, - arc_segments_for_minimum_distance, - arc_segments_for_minimum_time)); - - arc.arc_segments = max(arc.arc_segments, 1); //...but is at least 1 arc_segment - arc.gm.move_time = arc.arc_time / arc.arc_segments; // gcode state struct gets arc_segment_time, not arc time - arc.arc_segment_count = (int32_t)arc.arc_segments; - arc.arc_segment_theta = arc.angular_travel / arc.arc_segments; - arc.arc_segment_linear_travel = arc.linear_travel / arc.arc_segments; - arc.center_0 = arc.position[arc.plane_axis_0] - sin(arc.theta) * arc.radius; - arc.center_1 = arc.position[arc.plane_axis_1] - cos(arc.theta) * arc.radius; - arc.gm.target[arc.linear_axis] = arc.position[arc.linear_axis]; // initialize the linear target - return STAT_OK; -} - -/* - * _compute_arc_offsets_from_radius() - compute arc center (offset) from radius. - * - * Needs to calculate the center of the circle that has the designated radius and - * passes through both the current position and the target position - * - * This method calculates the following set of equations where: - * ` [x,y] is the vector from current to target position, - * d == magnitude of that vector, - * h == hypotenuse of the triangle formed by the radius of the circle, - * the distance to the center of the travel vector. - * - * A vector perpendicular to the travel vector [-y,x] is scaled to the length - * of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] - * to form the new point [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the - * center of the arc. - * - * d^2 == x^2 + y^2 - * h^2 == r^2 - (d/2)^2 - * i == x/2 - y/d*h - * j == y/2 + x/d*h - * O <- [i,j] - * - | - * r - | - * - | - * - | h - * - | - * [0,0] -> C -----------------+--------------- T <- [x,y] - * | <------ d/2 ---->| - * - * C - Current position - * T - Target position - * O - center of circle that pass through both C and T - * d - distance from C to T - * r - designated radius - * h - distance from center of CT to O - * - * Expanding the equations: - * d -> sqrt(x^2 + y^2) - * h -> sqrt(4 * r^2 - x^2 - y^2)/2 - * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - * - * Which can be written: - * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - * - * Which we for size and speed reasons optimize to: - * h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) - * i = (x - (y * h_x2_div_d))/2 - * j = (y + (x * h_x2_div_d))/2 - * - * ----Computing clockwise vs counter-clockwise motion ---- - * - * The counter clockwise circle lies to the left of the target direction. - * When offset is positive the left hand circle will be generated - - * when it is negative the right hand circle is generated. - * - * T <-- Target position - * - * ^ - * Clockwise circles with | Clockwise circles with - * this center will have | this center will have - * > 180 deg of angular travel | < 180 deg of angular travel, - * \ | which is a good thing! - * \ | / - * center of arc when -> x <----- | -----> x <- center of arc when - * h_x2_div_d is positive | h_x2_div_d is negative - * | - * C <-- Current position - * - * - * Assumes arc singleton has been pre-loaded with target and position. - * Parts of this routine were originally sourced from the grbl project. - */ -static stat_t _compute_arc_offsets_from_radius() -{ - // Calculate the change in position along each selected axis - float x = cm.gm.target[arc.plane_axis_0] - cm.gmx.position[arc.plane_axis_0]; - float y = cm.gm.target[arc.plane_axis_1] - cm.gmx.position[arc.plane_axis_1]; - - // *** From Forrest Green - Other Machine Co, 3/27/14 - // If the distance between endpoints is greater than the arc diameter, disc - // will be negative indicating that the arc is offset into the complex plane - // beyond the reach of any real CNC. However, numerical errors can flip the - // sign of disc as it approaches zero (which happens as the arc angle approaches - // 180 degrees). To avoid mishandling these arcs we use the closest real - // solution (which will be 0 when disc <= 0). This risks obscuring g-code errors - // where the radius is actually too small (they will be treated as half circles), - // but ensures that all valid arcs end up reasonably close to their intended - // paths regardless of any numerical issues. - float disc = 4 * square(arc.radius) - (square(x) + square(y)); - - // h_x2_div_d == -(h * 2 / d) - 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) - if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d;} - - // Negative R is g-code-alese for "I want a circle with more than 180 degrees - // of travel" (go figure!), even though it is advised against ever generating - // such circles in a single line of g-code. By inverting the sign of - // h_x2_div_d the center of the circles is placed on the opposite side of - // the line of travel and thus we get the unadvisably long arcs as prescribed. - if (arc.radius < 0) { h_x2_div_d = -h_x2_div_d; } - - // Complete the operation by calculating the actual center of the arc - arc.offset[arc.plane_axis_0] = (x-(y*h_x2_div_d))/2; - arc.offset[arc.plane_axis_1] = (y+(x*h_x2_div_d))/2; - arc.offset[arc.linear_axis] = 0; - return STAT_OK; -} - -/* - * _estimate_arc_time () - * - * Returns a naiive estimate of arc execution time to inform segment calculation. - * The arc time is computed not to exceed the time taken in the slowest dimension - * in the arc plane or in linear travel. Maximum feed rates are compared in each - * dimension, but the comparison assumes that the arc will have at least one segment - * where the unit vector is 1 in that dimension. This is not true for any arbitrary arc, - * with the result that the time returned may be less than optimal. - */ -static void _estimate_arc_time () -{ - // Determine move time at requested feed rate - if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) { - arc.arc_time = cm.gm.feed_rate; // inverse feed rate has been normalized to minutes - cm.gm.feed_rate = 0; // reset feed rate so next block requires an explicit feed rate setting - cm.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE; - } else { - arc.arc_time = arc.length / cm.gm.feed_rate; - } - - // Downgrade the time if there is a rate-limiting axis - arc.arc_time = max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_0].feedrate_max); - arc.arc_time = max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_1].feedrate_max); - if (fabs(arc.linear_travel) > 0) { - arc.arc_time = max(arc.arc_time, fabs(arc.linear_travel/cm.a[arc.linear_axis].feedrate_max)); - } -} diff --git a/src/plan_arc.h b/src/plan_arc.h deleted file mode 100644 index 3376066..0000000 --- a/src/plan_arc.h +++ /dev/null @@ -1,77 +0,0 @@ -/* - * plan_arc.h - arc planning and motion execution - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - * - * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#ifndef PLAN_ARC_H_ONCE -#define PLAN_ARC_H_ONCE - -// Arc radius tests. See http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc -//#define ARC_RADIUS_ERROR_MAX ((float)0.5) // max allowable mm between start and end radius -#define ARC_RADIUS_ERROR_MAX ((float)1.0) // max allowable mm between start and end radius -#define ARC_RADIUS_ERROR_MIN ((float)0.005) // min mm where 1% rule applies -#define ARC_RADIUS_TOLERANCE ((float)0.001) // 0.1% radius variance test - -// See planner.h for MM_PER_ARC_SEGMENT and other arc setting #defines - -typedef struct arArcSingleton { // persistent planner and runtime variables - magic_t magic_start; - uint8_t run_state; // runtime state machine sequence - - float position[AXES]; // accumulating runtime position - float offset[3]; // IJK offsets - - float length; // length of line or helix in mm - float theta; // total angle specified by arc - float theta_end; - float radius; // Raw R value, or computed via offsets - float angular_travel; // travel along the arc - float linear_travel; // travel along linear axis of arc - float planar_travel; - uint8_t full_circle; // set true if full circle arcs specified - uint32_t rotations; // Number of full rotations for full circles (P value) - - uint8_t plane_axis_0; // arc plane axis 0 - e.g. X for G17 - uint8_t plane_axis_1; // arc plane axis 1 - e.g. Y for G17 - uint8_t linear_axis; // linear axis (normal to plane) - - float arc_time; // total running time for arc (derived) - float arc_segments; // number of segments in arc or blend - int32_t arc_segment_count; // count of running segments - float arc_segment_theta; // angular motion per segment - float arc_segment_linear_travel;// linear motion per segment - float center_0; // center of circle at plane axis 0 (e.g. X for G17) - float center_1; // center of circle at plane axis 1 (e.g. Y for G17) - - GCodeState_t gm; // Gcode state struct is passed for each arc segment. Usage: -// uint32_t linenum; // line number of the arc feed move - same for each segment -// float target[AXES]; // arc segment target -// float work_offset[AXES]; // offset from machine coord system for reporting (same for each segment) -// float move_time; // segment_time: constant time per aline segment - - magic_t magic_end; -} arc_t; -extern arc_t arc; - - -/* arc function prototypes */ // NOTE: See canonical_machine.h for cm_arc_feed() prototype - -void cm_arc_init(); -stat_t cm_arc_callback(); -void cm_abort_arc(); - -#endif // End of include guard: PLAN_ARC_H_ONCE diff --git a/src/plan_exec.c b/src/plan_exec.c deleted file mode 100644 index edf44bc..0000000 --- a/src/plan_exec.c +++ /dev/null @@ -1,731 +0,0 @@ -/* - * plan_exec.c - execution function for acceleration managed lines - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - * Copyright (c) 2012 - 2015 Rob Giseburt - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include "tinyg.h" -#include "config.h" -#include "planner.h" -#include "kinematics.h" -#include "stepper.h" -#include "encoder.h" -#include "report.h" -#include "util.h" - -// execute routines (NB: These are all called from the LO interrupt) -static stat_t _exec_aline_head(); -static stat_t _exec_aline_body(); -static stat_t _exec_aline_tail(); -static stat_t _exec_aline_segment(); - -#ifndef __JERK_EXEC -static void _init_forward_diffs(float Vi, float Vt); -#endif - -/************************************************************************* - * mp_exec_move() - execute runtime functions to prep move for steppers - * - * Dequeues the buffer queue and executes the move continuations. - * Manages run buffers and other details - */ - -stat_t mp_exec_move() -{ - mpBuf_t *bf; - - if ((bf = mp_get_run_buffer()) == 0) { // 0 means nothing's running - st_prep_null(); - return STAT_NOOP; - } - // Manage cycle and motion state transitions - if (bf->move_type == MOVE_TYPE_ALINE) { // cycle auto-start for lines only - if (cm.motion_state == MOTION_STOP) cm_set_motion_state(MOTION_RUN); - } - if (bf->bf_func == 0) - return cm_hard_alarm(STAT_INTERNAL_ERROR); // never supposed to get here - - return bf->bf_func(bf); // run the move callback in the planner buffer -} - -/*************************************************************************/ -/**** ALINE EXECUTION ROUTINES *******************************************/ -/************************************************************************* - * ---> Everything here fires from interrupts and must be interrupt safe - * - * _exec_aline() - acceleration line main routine - * _exec_aline_head() - helper for acceleration section - * _exec_aline_body() - helper for cruise section - * _exec_aline_tail() - helper for deceleration section - * _exec_aline_segment() - helper for running a segment - * - * Returns: - * STAT_OK move is done - * STAT_EAGAIN move is not finished - has more segments to run - * STAT_NOOP cause no operation from the steppers - do not load the move - * STAT_xxxxx fatal error. Ends the move and frees the bf buffer - * - * This routine is called from the (LO) interrupt level. The interrupt - * sequencing relies on the behaviors of the routines being exactly correct. - * Each call to _exec_aline() must execute and prep *one and only one* - * segment. If the segment is the not the last segment in the bf buffer the - * _aline() must return STAT_EAGAIN. If it's the last segment it must return - * STAT_OK. If it encounters a fatal error that would terminate the move it - * should return a valid error code. Failure to obey this will introduce - * subtle and very difficult to diagnose bugs (trust me on this). - * - * Note 1 Returning STAT_OK ends the move and frees the bf buffer. - * Returning STAT_OK at this point does NOT advance position meaning any - * position error will be compensated by the next move. - * - * Note 2 Solves a potential race condition where the current move ends but the - * new move has not started because the previous move is still being run - * by the steppers. Planning can overwrite the new move. - */ -/* OPERATION: - * Aline generates jerk-controlled S-curves as per Ed Red's course notes: - * http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf - * http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations - * - * A full trapezoid is divided into 5 periods Periods 1 and 2 are the - * first and second halves of the acceleration ramp (the concave and convex - * parts of the S curve in the "head"). Periods 3 and 4 are the first - * and second parts of the deceleration ramp (the tail). There is also - * a period for the constant-velocity plateau of the trapezoid (the body). - * There are various degraded trapezoids possible, including 2 section - * combinations (head and tail; head and body; body and tail), and single - * sections - any one of the three. - * - * The equations that govern the acceleration and deceleration ramps are: - * - * Period 1 V = Vi + Jm*(T^2)/2 - * Period 2 V = Vh + As*T - Jm*(T^2)/2 - * 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 all works out. move_time is the actual time of the - * move, accel_time is the time valaue needed to compute the velocity - which - * takes the initial velocity into account (move_time does not need to). - */ -/* --- State transitions - hierarchical state machine --- - * - * bf->move_state transitions: - * from _NEW to _RUN on first call (sub_state set to _OFF) - * from _RUN to _OFF on final call - * or just remains _OFF - * - * mr.move_state transitions on first call from _OFF to one of _HEAD, _BODY, _TAIL - * Within each section state may be - * _NEW - trigger initialization - * _RUN1 - run the first part - * _RUN2 - run the second part - * - * Note: For a direct math implementation see build 357.xx or earlier - * Builds 358 onward have only forward difference code - */ - -stat_t mp_exec_aline(mpBuf_t *bf) -{ - if (bf->move_state == MOVE_OFF) - return STAT_NOOP; - - // start a new move by setting up local context (singleton) - if (mr.move_state == MOVE_OFF) { - if (cm.hold_state == FEEDHOLD_HOLD) - return STAT_NOOP; // stops here if holding - - // initialization to process the new incoming bf buffer (Gcode block) - memcpy(&mr.gm, &(bf->gm), sizeof(GCodeState_t));// copy in the gcode model state - bf->replannable = false; - // too short lines have already been removed - if (fp_ZERO(bf->length)) { // ...looks for an actual zero here - mr.move_state = MOVE_OFF; // reset mr buffer - mr.section_state = SECTION_OFF; - bf->nx->replannable = false; // prevent overplanning (Note 2) - st_prep_null(); // call this to keep the loader happy - if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & end cycle if planner is empty - return STAT_NOOP; - } - bf->move_state = MOVE_RUN; - mr.move_state = MOVE_RUN; - mr.section = SECTION_HEAD; - mr.section_state = SECTION_NEW; - mr.jerk = bf->jerk; -#ifdef __JERK_EXEC - mr.jerk_div2 = bf->jerk/2; // only needed by __JERK_EXEC -#endif - mr.head_length = bf->head_length; - mr.body_length = bf->body_length; - mr.tail_length = bf->tail_length; - - mr.entry_velocity = bf->entry_velocity; - mr.cruise_velocity = bf->cruise_velocity; - mr.exit_velocity = bf->exit_velocity; - - copy_vector(mr.unit, bf->unit); - copy_vector(mr.target, bf->gm.target); // save the final target of the move - - // generate the waypoints for position correction at section ends - for (uint8_t axis=0; axismove_state Description - // ----------- -------------- ---------------------------------------- - // STAT_EAGAIN mr buffer has more segments to run - // STAT_OK MOVE_RUN mr and bf buffers are done - // STAT_OK MOVE_NEW mr done; bf must be run again (it's been reused) - - if (status == STAT_EAGAIN) { - sr_request_status_report(SR_TIMED_REQUEST); // continue reporting mr buffer - } else { - mr.move_state = MOVE_OFF; // reset mr buffer - mr.section_state = SECTION_OFF; - bf->nx->replannable = false; // prevent overplanning (Note 2) - if (bf->move_state == MOVE_RUN) { - if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & end cycle if planner is empty - } - } - return status; -} - -/* Forward difference math explained: - * - * 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 - * - * The Bezier curve takes the form: - * - * V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + P_4 * B_4(t) + P_5 * B_5(t) - * - * Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are the control points, and B_0(t) - * through B_5(t) are the Bernstein basis as follows: - * - * B_0(t) = (1-t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1 - * B_1(t) = 5(1-t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t - * B_2(t) = 10(1-t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2 - * B_3(t) = 10(1-t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3 - * B_4(t) = 5(1-t) * t^4 = -5t^5 + 5t^4 - * B_5(t) = t^5 = t^5 - * ^ ^ ^ ^ ^ ^ - * | | | | | | - * A B C D E F - * - * - * We use forward-differencing to calculate each position through the curve. - * This requires a formula of the form: - * - * V_f(t) = A*t^5 + B*t^4 + C*t^3 + D*t^2 + E*t + F - * - * Looking at the above B_0(t) through B_5(t) expanded forms, if we take the coefficients of t^5 - * through t of the Bezier form of V(t), we can determine that: - * - * A = -P_0 + 5*P_1 - 10*P_2 + 10*P_3 - 5*P_4 + P_5 - * B = 5*P_0 - 20*P_1 + 30*P_2 - 20*P_3 + 5*P_4 - * C = -10*P_0 + 30*P_1 - 30*P_2 + 10*P_3 - * D = 10*P_0 - 20*P_1 + 10*P_2 - * 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: - * - * A = - 6*P_i + 6*P_t - * B = 15*P_i - 15*P_t - * C = -10*P_i + 10*P_t - * D = 0 - * 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: - * - * V += F_5 - * F_5 += F_4 - * F_4 += F_3 - * F_3 += F_2 - * F_2 += F_1 - * - * See http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417 for an example of - * how to calculate F_0 - F_5 for a cubic bezier curve. Since this is a quintic bezier curve, we - * need to extend the formulas somewhat. I'll not go into the long-winded step-by-step here, - * but it gives the resulting formulas: - * - * a = A, b = B, c = C, d = D, e = E, f = F - * F_5(t+h)-F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 + (10ah^3 + 6bh^2 + 3ch)t^2 + - * (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 + bh^4 + ch^3 + dh^2 + eh - * - * a = 5ah - * b = 10ah^2 + 4bh - * c = 10ah^3 + 6bh^2 + 3ch - * d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh - * - * (After substitution, simplification, and rearranging): - * F_4(t+h)-F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 + (70ah^4 + 24bh^3 + 6ch^2)t + - * 30ah^5 + 14bh^4 + 6ch^3 + 2dh^2 - * - * a = (20ah^2) - * b = (60ah^3 + 12bh^2) - * c = (70ah^4 + 24bh^3 + 6ch^2) - * - * (After substitution, simplification, and rearranging): - * F_3(t+h)-F_3(t) = (60ah^3)t^2 + (180ah^4 + 24bh^3)t + 150ah^5 + 36bh^4 + 6ch^3 - * - * (You get the picture...) - * F_2(t+h)-F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4 - * F_1(t+h)-F_1(t) = 120ah^5 - * - * 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): - * - * F_5 = (121Ah^5)/16 + 5Bh^4 + (13Ch^3)/4 + 2Dh^2 + Eh - * F_4 = (165Ah^5)/2 + 29Bh^4 + 9Ch^3 + 2Dh^2 - * F_3 = 255Ah^5 + 48Bh^4 + 6Ch^3 - * F_2 = 300Ah^5 + 24Bh^4 - * F_1 = 120Ah^5 - * - * Note that with our current control points, D and E are actually 0. - */ -#ifndef __JERK_EXEC - -static void _init_forward_diffs(float Vi, float Vt) -{ - float A = -6.0*Vi + 6.0*Vt; - float B = 15.0*Vi - 15.0*Vt; - float C = -10.0*Vi + 10.0*Vt; - // D = 0 - // E = 0 - // F = Vi - - float h = 1/(mr.segments); - - float Ah_5 = A * h * h * h * h * h; - float Bh_4 = B * h * h * h * h; - float Ch_3 = C * h * h * h; - - mr.forward_diff_5 = (121.0/16.0)*Ah_5 + 5.0*Bh_4 + (13.0/4.0)*Ch_3; - mr.forward_diff_4 = (165.0/2.0)*Ah_5 + 29.0*Bh_4 + 9.0*Ch_3; - mr.forward_diff_3 = 255.0*Ah_5 + 48.0*Bh_4 + 6.0*Ch_3; - mr.forward_diff_2 = 300.0*Ah_5 + 24.0*Bh_4; - mr.forward_diff_1 = 120.0*Ah_5; - -#ifdef __KAHAN - mr.forward_diff_5_c = 0; - mr.forward_diff_4_c = 0; - mr.forward_diff_3_c = 0; - mr.forward_diff_2_c = 0; - mr.forward_diff_1_c = 0; -#endif - - // Calculate the initial velocity by calculating V(h/2) - float half_h = h/2.0; - float half_Ch_3 = C * half_h * half_h * half_h; - float half_Bh_4 = B * half_h * half_h * half_h * half_h; - float half_Ah_5 = C * half_h * half_h * half_h * half_h * half_h; - mr.segment_velocity = half_Ah_5 + half_Bh_4 + half_Ch_3 + Vi; -} -#endif - -/********************************************************************************************* - * _exec_aline_head() - */ -#ifdef __JERK_EXEC - -static stat_t _exec_aline_head() -{ - if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) - if (fp_ZERO(mr.head_length)) { - mr.section = SECTION_BODY; - return _exec_aline_body(); // skip ahead to the body generator - } - mr.midpoint_velocity = (mr.entry_velocity + mr.cruise_velocity) / 2; - mr.gm.move_time = mr.head_length / mr.midpoint_velocity; // time for entire accel region - mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC)); // # of segments in *each half* - mr.segment_time = mr.gm.move_time / (2 * mr.segments); - mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.entry_velocity) / mr.jerk); - mr.midpoint_acceleration = 2 * (mr.cruise_velocity - mr.entry_velocity) / mr.accel_time; - mr.segment_accel_time = mr.accel_time / (2 * mr.segments); // time to advance for each segment - mr.elapsed_accel_time = mr.segment_accel_time / 2; // elapsed time starting point (offset) - mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position - mr.section = SECTION_HEAD; - mr.section_state = SECTION_1st_HALF; - } - if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF (concave part of accel curve) - mr.segment_velocity = mr.entry_velocity + (square(mr.elapsed_accel_time) * mr.jerk_div2); - if (_exec_aline_segment() == STAT_OK) { // set up for second half - mr.segment_count = (uint32_t)mr.segments; - mr.section_state = SECTION_2nd_HALF; - mr.elapsed_accel_time = mr.segment_accel_time / 2; // start time from midpoint of segment - } - return STAT_EAGAIN; - } - if (mr.section_state == SECTION_2nd_HALF) { // SECOND HAF (convex part of accel curve) - mr.segment_velocity = mr.midpoint_velocity + - (mr.elapsed_accel_time * mr.midpoint_acceleration) - - (square(mr.elapsed_accel_time) * mr.jerk_div2); - if (_exec_aline_segment() == STAT_OK) { // OK means this section is done - if ((fp_ZERO(mr.body_length)) && (fp_ZERO(mr.tail_length))) - return STAT_OK; // ends the move - mr.section = SECTION_BODY; - mr.section_state = SECTION_NEW; - } - } - return STAT_EAGAIN; -} -#else // __ JERK_EXEC - -static stat_t _exec_aline_head() -{ - if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) - if (fp_ZERO(mr.head_length)) { - mr.section = SECTION_BODY; - return _exec_aline_body(); // skip ahead to the body generator - } - mr.gm.move_time = 2*mr.head_length / (mr.entry_velocity + mr.cruise_velocity);// time for entire accel region - mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC);// # of segments for the section - mr.segment_time = mr.gm.move_time / mr.segments; - _init_forward_diffs(mr.entry_velocity, mr.cruise_velocity); - mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position - mr.section = SECTION_HEAD; - mr.section_state = SECTION_1st_HALF; // Note: Set to SECTION_1st_HALF for one segment - } - // For forward differencing we should have one segment in SECTION_1st_HALF - // However, if it returns from that as STAT_OK, then there was only one segment in this section. - if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF (concave part of accel curve) - if (_exec_aline_segment() == STAT_OK) { // set up for second half - mr.section = SECTION_BODY; - mr.section_state = SECTION_NEW; - } else { - mr.section_state = SECTION_2nd_HALF; - } - return STAT_EAGAIN; - } - if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF (convex part of accel curve) -#ifndef __KAHAN - mr.segment_velocity += mr.forward_diff_5; -#else // Use Kahan summation algorithm to mitigate floating-point errors for the above - float y = mr.forward_diff_5 - mr.forward_diff_5_c; - float v = mr.segment_velocity + y; - mr.forward_diff_5_c = (v - mr.segment_velocity) - y; - mr.segment_velocity = v; -#endif - - if (_exec_aline_segment() == STAT_OK) { // set up for body - if ((fp_ZERO(mr.body_length)) && (fp_ZERO(mr.tail_length))) - return STAT_OK; // ends the move - mr.section = SECTION_BODY; - mr.section_state = SECTION_NEW; - } else { -#ifndef __KAHAN - mr.forward_diff_5 += mr.forward_diff_4; - mr.forward_diff_4 += mr.forward_diff_3; - mr.forward_diff_3 += mr.forward_diff_2; - mr.forward_diff_2 += mr.forward_diff_1; -#else - //mr.forward_diff_5 += mr.forward_diff_4; - y = mr.forward_diff_4 - mr.forward_diff_4_c; - v = mr.forward_diff_5 + y; - mr.forward_diff_4_c = (v - mr.forward_diff_5) - y; - mr.forward_diff_5 = v; - - //mr.forward_diff_4 += mr.forward_diff_3; - y = mr.forward_diff_3 - mr.forward_diff_3_c; - v = mr.forward_diff_4 + y; - mr.forward_diff_3_c = (v - mr.forward_diff_4) - y; - mr.forward_diff_4 = v; - - //mr.forward_diff_3 += mr.forward_diff_2; - y = mr.forward_diff_2 - mr.forward_diff_2_c; - v = mr.forward_diff_3 + y; - mr.forward_diff_2_c = (v - mr.forward_diff_3) - y; - mr.forward_diff_3 = v; - - //mr.forward_diff_2 += mr.forward_diff_1; - y = mr.forward_diff_1 - mr.forward_diff_1_c; - v = mr.forward_diff_2 + y; - mr.forward_diff_1_c = (v - mr.forward_diff_2) - y; - mr.forward_diff_2 = v; -#endif - } - } - return STAT_EAGAIN; -} -#endif // __ JERK_EXEC - -/********************************************************************************************* - * _exec_aline_body() - * - * The body is broken into little segments even though it is a straight line so that - * feedholds can happen in the middle of a line with a minimum of latency - */ -static stat_t _exec_aline_body() -{ - if (mr.section_state == SECTION_NEW) { - if (fp_ZERO(mr.body_length)) { - mr.section = SECTION_TAIL; - return _exec_aline_tail(); // skip ahead to tail periods - } - mr.gm.move_time = mr.body_length / mr.cruise_velocity; - mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC); - mr.segment_time = mr.gm.move_time / mr.segments; - mr.segment_velocity = mr.cruise_velocity; - mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position - mr.section = SECTION_BODY; - mr.section_state = SECTION_2nd_HALF; // uses PERIOD_2 so last segment detection works - } - if (mr.section_state == SECTION_2nd_HALF) { // straight part (period 3) - if (_exec_aline_segment() == STAT_OK) { // OK means this section is done - if (fp_ZERO(mr.tail_length)) - return STAT_OK; // ends the move - mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; - } - } - return STAT_EAGAIN; -} - -/********************************************************************************************* - * _exec_aline_tail() - */ - -#ifdef __JERK_EXEC - -static stat_t _exec_aline_tail() -{ - if (mr.section_state == SECTION_NEW) { // INITIALIZATION - if (fp_ZERO(mr.tail_length)) - return STAT_OK; // end the move - mr.midpoint_velocity = (mr.cruise_velocity + mr.exit_velocity) / 2; - mr.gm.move_time = mr.tail_length / mr.midpoint_velocity; - mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC));// # of segments in *each half* - mr.segment_time = mr.gm.move_time / (2 * mr.segments); // time to advance for each segment - mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.exit_velocity) / mr.jerk); - mr.midpoint_acceleration = 2 * (mr.cruise_velocity - mr.exit_velocity) / mr.accel_time; - mr.segment_accel_time = mr.accel_time / (2 * mr.segments); // time to advance for each segment - mr.elapsed_accel_time = mr.segment_accel_time / 2; //compute time from midpoint of segment - mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position - mr.section = SECTION_TAIL; - mr.section_state = SECTION_1st_HALF; - } - if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF - convex part (period 4) - mr.segment_velocity = mr.cruise_velocity - (square(mr.elapsed_accel_time) * mr.jerk_div2); - if (_exec_aline_segment() == STAT_OK) { // set up for second half - mr.segment_count = (uint32_t)mr.segments; - mr.section_state = SECTION_2nd_HALF; - mr.elapsed_accel_time = mr.segment_accel_time / 2; // start time from midpoint of segment - } - return STAT_EAGAIN; - } - if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF - concave part (period 5) - mr.segment_velocity = mr.midpoint_velocity - - (mr.elapsed_accel_time * mr.midpoint_acceleration) + - (square(mr.elapsed_accel_time) * mr.jerk_div2); - return _exec_aline_segment(); // ends the move or continues EAGAIN - } - return STAT_EAGAIN; // should never get here -} - -#else // __JERK_EXEC -- run forward differencing math - -static stat_t _exec_aline_tail() -{ - if (mr.section_state == SECTION_NEW) { // INITIALIZATION - if (fp_ZERO(mr.tail_length)) - return STAT_OK; // end the move - mr.gm.move_time = 2*mr.tail_length / (mr.cruise_velocity + mr.exit_velocity); // len/avg. velocity - mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC);// # of segments for the section - mr.segment_time = mr.gm.move_time / mr.segments; // time to advance for each segment - _init_forward_diffs(mr.cruise_velocity, mr.exit_velocity); - mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position - mr.section = SECTION_TAIL; - mr.section_state = SECTION_1st_HALF; - } - if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF - convex part (period 4) - if (_exec_aline_segment() == STAT_OK) { - // For forward differencing we should have one segment in SECTION_1st_HALF. - // However, if it returns from that as STAT_OK, then there was only one segment in this section. - // Show that we did complete section 2 ... effectively. - mr.section_state = SECTION_2nd_HALF; - return STAT_OK; - } else { - mr.section_state = SECTION_2nd_HALF; - } - return STAT_EAGAIN; - } - if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF - concave part (period 5) -#ifndef __KAHAN - mr.segment_velocity += mr.forward_diff_5; -#else // Use Kahan summation algorithm to mitigate floating-point errors for the above - float y = mr.forward_diff_5 - mr.forward_diff_5_c; - float v = mr.segment_velocity + y; - mr.forward_diff_5_c = (v - mr.segment_velocity) - y; - mr.segment_velocity = v; -#endif - - if (_exec_aline_segment() == STAT_OK) { // set up for body - return STAT_OK; - } else { -#ifndef __KAHAN - mr.forward_diff_5 += mr.forward_diff_4; - mr.forward_diff_4 += mr.forward_diff_3; - mr.forward_diff_3 += mr.forward_diff_2; - mr.forward_diff_2 += mr.forward_diff_1; -#else - //mr.forward_diff_5 += mr.forward_diff_4; - y = mr.forward_diff_4 - mr.forward_diff_4_c; - v = mr.forward_diff_5 + y; - mr.forward_diff_4_c = (v - mr.forward_diff_5) - y; - mr.forward_diff_5 = v; - - //mr.forward_diff_4 += mr.forward_diff_3; - y = mr.forward_diff_3 - mr.forward_diff_3_c; - v = mr.forward_diff_4 + y; - mr.forward_diff_3_c = (v - mr.forward_diff_4) - y; - mr.forward_diff_4 = v; - - //mr.forward_diff_3 += mr.forward_diff_2; - y = mr.forward_diff_2 - mr.forward_diff_2_c; - v = mr.forward_diff_3 + y; - mr.forward_diff_2_c = (v - mr.forward_diff_3) - y; - mr.forward_diff_3 = v; - - //mr.forward_diff_2 += mr.forward_diff_1; - y = mr.forward_diff_1 - mr.forward_diff_1_c; - v = mr.forward_diff_2 + y; - mr.forward_diff_1_c = (v - mr.forward_diff_2) - y; - mr.forward_diff_2 = v; -#endif - } - } - return STAT_EAGAIN; // should never get here -} -#endif // __JERK_EXEC - -/********************************************************************************************* - * _exec_aline_segment() - segment runner helper - * - * NOTES ON STEP ERROR CORRECTION: - * - * The commanded_steps are the target_steps delayed by one more segment. - * This lines them up in time with the encoder readings so a following error can be generated - * - * The following_error term is positive if the encoder reading is greater than (ahead of) - * the commanded steps, and negative (behind) if the encoder reading is less than the - * commanded steps. The following error is not affected by the direction of movement - - * it's purely a statement of relative position. Examples: - * - * Encoder Commanded Following Err - * 100 90 +10 encoder is 10 steps ahead of commanded steps - * -90 -100 +10 encoder is 10 steps ahead of commanded steps - * 90 100 -10 encoder is 10 steps behind commanded steps - * -100 -90 -10 encoder is 10 steps behind commanded steps - */ - -static stat_t _exec_aline_segment() -{ - uint8_t i; - float travel_steps[MOTORS]; - - // Set target position for the segment - // If the segment ends on a section 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 ((--mr.segment_count == 0) && (mr.section_state == SECTION_2nd_HALF) && - (cm.motion_state == MOTION_RUN) && (cm.cycle_state == CYCLE_MACHINING)) { - copy_vector(mr.gm.target, mr.waypoint[mr.section]); - } else { - float segment_length = mr.segment_velocity * mr.segment_time; - for (i=0; i. - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include "tinyg.h" -#include "config.h" -#include "controller.h" -#include "canonical_machine.h" -#include "planner.h" -#include "stepper.h" -#include "report.h" -#include "util.h" - -// aline planner routines / feedhold planning -//static void _calc_move_times(GCodeState_t *gms, const float position[]); -static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const float axis_square[]); -static void _plan_block_list(mpBuf_t *bf, uint8_t *mr_flag); -static float _get_junction_vmax(const float a_unit[], const float b_unit[]); -static void _reset_replannable_list(); - -/* Runtime-specific setters and getters - * - * mp_zero_segment_velocity() - correct velocity in last segment for reporting purposes - * mp_get_runtime_velocity() - returns current velocity (aggregate) - * mp_get_runtime_machine_position()- returns current axis position in machine coordinates - * mp_set_runtime_work_offset() - set offsets in the MR struct - * mp_get_runtime_work_position() - returns current axis position in work coordinates - * that were in effect at move planning time - */ - -void mp_zero_segment_velocity() { mr.segment_velocity = 0;} -float mp_get_runtime_velocity() { return mr.segment_velocity;} -float mp_get_runtime_absolute_position(uint8_t axis) { return mr.position[axis];} -void mp_set_runtime_work_offset(float offset[]) { copy_vector(mr.gm.work_offset, offset);} -float mp_get_runtime_work_position(uint8_t axis) { return mr.position[axis] - mr.gm.work_offset[axis];} - -/* - * mp_get_runtime_busy() - return TRUE if motion control busy (i.e. robot is moving) - * - * Use this function to sync to the queue. If you wait until it returns - * FALSE you know the queue is empty and the motors have stopped. - */ - -uint8_t mp_get_runtime_busy() -{ - if ((st_runtime_isbusy() == true) || (mr.move_state == MOVE_RUN)) return true; - return false; -} - -/**************************************************************************************** - * mp_aline() - plan a line with acceleration / deceleration - * - * This function uses constant jerk motion equations to plan acceleration and deceleration - * The jerk is the rate of change of acceleration; it's the 1st derivative of acceleration, - * and the 3rd derivative of position. Jerk is a measure of impact to the machine. - * Controlling jerk smooths transitions between moves and allows for faster feeds while - * controlling machine oscillations and other undesirable side-effects. - * - * Note All math is done in absolute coordinates using single precision floating point (float). - * - * Note: Returning a status that is not STAT_OK means the endpoint is NOT advanced. So lines - * that are too short to move will accumulate and get executed once the accumulated error - * exceeds the minimums. - */ -/* -#define axis_length bf->body_length -#define axis_velocity bf->cruise_velocity -#define axis_tail bf->tail_length -#define longest_tail bf->head_length -*/ -stat_t mp_aline(GCodeState_t *gm_in) -{ - mpBuf_t *bf; // current move pointer - float exact_stop = 0; // preset this value OFF - float junction_velocity; - uint8_t mr_flag = false; - - // compute some reusable terms - float axis_length[AXES]; - float axis_square[AXES]; - float length_square = 0; - - for (uint8_t axis=0; axistarget[axis] - mm.position[axis]; - axis_square[axis] = square(axis_length[axis]); - length_square += axis_square[axis]; - } - float length = sqrt(length_square); - - if (fp_ZERO(length)) { -// sr_request_status_report(); - return STAT_OK; - } - - // If _calc_move_times() says the move will take less than the minimum move time - // get a more accurate time estimate based on starting velocity and acceleration. - // The time of the move is determined by its initial velocity (Vi) and how much - // acceleration will be occur. For this we need to look at the exit velocity of - // the previous block. There are 3 possible cases: - // (1) There is no previous block. Vi = 0 - // (2) Previous block is optimally planned. Vi = previous block's exit_velocity - // (3) Previous block is not optimally planned. Vi <= previous block's entry_velocity + delta_velocity - - _calc_move_times(gm_in, axis_length, axis_square); // set move time and minimum time in the state - if (gm_in->move_time < MIN_BLOCK_TIME) { - float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; // max velocity change for this move - float entry_velocity = 0; // pre-set as if no previous block - if ((bf = mp_get_run_buffer()) != 0) { - if (bf->replannable == true) { // not optimally planned - entry_velocity = bf->entry_velocity + bf->delta_vmax; - } else { // optimally planned - entry_velocity = bf->exit_velocity; - } - } - float move_time = (2 * length) / (2*entry_velocity + delta_velocity);// compute execution time for this move - if (move_time < MIN_BLOCK_TIME) - return STAT_MINIMUM_TIME_MOVE; - } - - // get a cleared buffer and setup move variables - if ((bf = mp_get_write_buffer()) == 0) - return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // never supposed to fail - bf->bf_func = mp_exec_aline; // register the callback to the exec function - bf->length = length; - memcpy(&bf->gm, gm_in, sizeof(GCodeState_t)); // copy model state into planner buffer - - // Compute the unit vector and find the right jerk to use (combined operations) - // To determine the jerk value to use for the block we want to find the axis for which - // the jerk cannot be exceeded - the 'jerk-limit' axis. This is the axis for which - // the time to decelerate from the target velocity to zero would be the longest. - // - // We can determine the "longest" deceleration in terms of time or distance. - // - // The math for time-to-decelerate T from speed S to speed E with constant - // jerk J is: - // - // T = 2*sqrt((S-E)/J[n]) - // - // Since E is always zero in this calculation, we can simplify: - // T = 2*sqrt(S/J[n]) - // - // The math for distance-to-decelerate l from speed S to speed E with constant - // jerk J is: - // - // l = (S+E)*sqrt((S-E)/J) - // - // Since E is always zero in this calculation, we can simplify: - // l = S*sqrt(S/J) - // - // The final value we want is to know which one is *longest*, compared to the others, - // so we don't need the actual value. This means that the scale of the value doesn't - // matter, so for T we can remove the "2 *" and For L we can remove the "S*". - // This leaves both to be simply "sqrt(S/J)". Since we don't need the scale, - // it doesn't matter what speed we're coming from, so S can be replaced with 1. - // - // However, we *do* need to compensate for the fact that each axis contributes - // differently to the move, so we will scale each contribution C[n] by the - // proportion of the axis movement length D[n] to the total length of the move L. - // Using that, we construct the following, with these definitions: - // - // J[n] = max_jerk for axis n - // D[n] = distance traveled for this move for axis n - // L = total length of the move in all axes - // C[n] = "axis contribution" of axis n - // - // For each axis n: C[n] = sqrt(1/J[n]) * (D[n]/L) - // - // Keeping in mind that we only need a rank compared to the other axes, we can further - // optimize the calculations:: - // - // Square the expression to remove the square root: - // C[n]^2 = (1/J[n]) * (D[n]/L)^2 (We don't care the C is squared, we'll use it that way.) - // - // Re-arranged to optimize divides for pre-calculated values, we create a value - // M that we compute once: - // M = (1/(L^2)) - // Then we use it in the calculations for every axis: - // C[n] = (1/J[n]) * D[n]^2 * M - // - // Also note that we already have (1/J[n]) calculated for each axis, which simplifies it further. - // - // Finally, the selected jerk term needs to be scaled by the reciprocal of the absolute value - // of the jerk-limit axis's unit vector term. This way when the move is finally decomposed into - // its constituent axes for execution the jerk for that axis will be at it's maximum value. - - float C; // contribution term. C = T * a - float maxC = 0; - float recip_L2 = 1/length_square; - - for (uint8_t axis=0; axis 0) { // You cannot use the fp_XXX comparisons here! - bf->unit[axis] = axis_length[axis] / bf->length; // compute unit vector term (zeros are already zero) - C = axis_square[axis] * recip_L2 * cm.a[axis].recip_jerk; // squaring axis_length ensures it's positive - if (C > maxC) { - maxC = C; - bf->jerk_axis = axis; // also needed for junction vmax calculation - } - } - } - // set up and pre-compute the jerk terms needed for this round of planning - bf->jerk = cm.a[bf->jerk_axis].jerk_max * JERK_MULTIPLIER / fabs(bf->unit[bf->jerk_axis]); // scale the jerk - - if (fabs(bf->jerk - mm.jerk) > JERK_MATCH_PRECISION) { // specialized comparison for tolerance of delta - mm.jerk = bf->jerk; // used before this point next time around - mm.recip_jerk = 1/bf->jerk; // compute cached jerk terms used by planning - mm.cbrt_jerk = cbrt(bf->jerk); - } - bf->recip_jerk = mm.recip_jerk; - bf->cbrt_jerk = mm.cbrt_jerk; - - // finish up the current block variables - if (cm_get_path_control(MODEL) != PATH_EXACT_STOP) { // exact stop cases already zeroed - bf->replannable = true; - exact_stop = 8675309; // an arbitrarily large floating point number - } - bf->cruise_vmax = bf->length / bf->gm.move_time; // target velocity requested - junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit); - bf->entry_vmax = min3(bf->cruise_vmax, junction_velocity, exact_stop); - bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); - bf->exit_vmax = min3(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax), exact_stop); - bf->braking_velocity = bf->delta_vmax; - - // Note: these next lines must remain in exact order. Position must update before committing the buffer. - _plan_block_list(bf, &mr_flag); // replan block list - copy_vector(mm.position, bf->gm.target); // set the planner position - mp_commit_write_buffer(MOVE_TYPE_ALINE); // commit current block (must follow the position update) - return STAT_OK; -} - -/***** ALINE HELPERS ***** - * _calc_move_times() - * _plan_block_list() - * _get_junction_vmax() - * _reset_replannable_list() - */ - -/* - * _calc_move_times() - compute optimal and minimum move times into the gcode_state - * - * "Minimum time" is the fastest the move can be performed given the velocity constraints on each - * participating axis - regardless of the feed rate requested. The minimum time is the time limited - * by the rate-limiting axis. The minimum time is needed to compute the optimal time and is - * recorded for possible feed override computation.. - * - * "Optimal time" is either the time resulting from the requested feed rate or the minimum time if - * the requested feed rate is not achievable. Optimal times for traverses are always the minimum time. - * - * The gcode state must have targets set prior by having cm_set_target(). Axis modes are taken into - * account by this. - * - * The following times are compared and the longest is returned: - * - G93 inverse time (if G93 is active) - * - time for coordinated move at requested feed rate - * - time that the slowest axis would require for the move - * - * Sets the following variables in the gcode_state struct - * - move_time is set to optimal time - * - minimum_time is set to minimum time - */ -/* --- NIST RS274NGC_v3 Guidance --- - * - * The following is verbatim text from NIST RS274NGC_v3. As I interpret A for moves that - * combine both linear and rotational movement, the feed rate should apply to the XYZ - * movement, with the rotational axis (or axes) timed to start and end at the same time - * the linear move is performed. It is possible under this case for the rotational move - * to rate-limit the linear move. - * - * 2.1.2.5 Feed Rate - * - * The rate at which the controlled point or the axes move is nominally a steady rate - * which may be set by the user. In the Interpreter, the interpretation of the feed - * rate is as follows unless inverse time feed rate mode is being used in the - * RS274/NGC view (see Section 3.5.19). The canonical machining functions view of feed - * rate, as described in Section 4.3.5.1, has conditions under which the set feed rate - * is applied differently, but none of these is used in the Interpreter. - * - * A. For motion involving one or more of the X, Y, and Z axes (with or without - * simultaneous rotational axis motion), the feed rate means length units per - * minute along the programmed XYZ path, as if the rotational axes were not moving. - * - * B. For motion of one rotational axis with X, Y, and Z axes not moving, the - * feed rate means degrees per minute rotation of the rotational axis. - * - * C. For motion of two or three rotational axes with X, Y, and Z axes not moving, - * the rate is applied as follows. Let dA, dB, and dC be the angles in degrees - * through which the A, B, and C axes, respectively, must move. - * Let D = sqrt(dA^2 + dB^2 + dC^2). Conceptually, D is a measure of total - * angular motion, using the usual Euclidean metric. Let T be the amount of - * time required to move through D degrees at the current feed rate in degrees - * per minute. The rotational axes should be moved in coordinated linear motion - * so that the elapsed time from the start to the end of the motion is T plus - * any time required for acceleration or deceleration. - */ - -static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const float axis_square[]) - // gms = Gcode model state -{ - float inv_time=0; // inverse time if doing a feed in G93 mode - float xyz_time=0; // coordinated move linear part at requested feed rate - float abc_time=0; // coordinated move rotary part at requested feed rate - float max_time=0; // time required for the rate-limiting axis - float tmp_time=0; // used in computation - gms->minimum_time = 8675309; // arbitrarily large number - - // compute times for feed motion - if (gms->motion_mode != MOTION_MODE_STRAIGHT_TRAVERSE) { - if (gms->feed_rate_mode == INVERSE_TIME_MODE) { - inv_time = gms->feed_rate; // NB: feed rate was un-inverted to minutes by cm_set_feed_rate() - gms->feed_rate_mode = UNITS_PER_MINUTE_MODE; - } else { - // compute length of linear move in millimeters. Feed rate is provided as mm/min - xyz_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + axis_square[AXIS_Z]) / gms->feed_rate; - - // if no linear axes, compute length of multi-axis rotary move in degrees. Feed rate is provided as degrees/min - if (fp_ZERO(xyz_time)) { - abc_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + axis_square[AXIS_C]) / gms->feed_rate; - } - } - } - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - if (gms->motion_mode == MOTION_MODE_STRAIGHT_TRAVERSE) { - tmp_time = fabs(axis_length[axis]) / cm.a[axis].velocity_max; - } else { // MOTION_MODE_STRAIGHT_FEED - tmp_time = fabs(axis_length[axis]) / cm.a[axis].feedrate_max; - } - max_time = max(max_time, tmp_time); - - if (tmp_time > 0) { // collect minimum time if this axis is not zero - gms->minimum_time = min(gms->minimum_time, tmp_time); - } - } - gms->move_time = max4(inv_time, max_time, xyz_time, abc_time); -} - -/* _plan_block_list() - 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 "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 or there is only - * one block and it is already running. - * - * If blocks following the first block are already optimally planned (non replannable) - * the first block that is not optimally planned becomes the effective first block. - * - * _plan_block_list() plans all blocks between and including the (effective) first block - * and the bf. It sets entry, exit and cruise v's from vmax's then calls trapezoid generation. - * - * Variables that must be provided in the mpBuffers that will be processed: - * - * bf (function arg) - end of block list (last block in time) - * bf->replannable - start of block list set by last FALSE value [Note 1] - * bf->move_type - typically MOVE_TYPE_ALINE. Other move_types should be set to - * length=0, entry_vmax=0 and exit_vmax=0 and are treated - * as a momentary stop (plan to zero and from zero). - * - * bf->length - provides block length - * bf->entry_vmax - used during forward planning to set entry velocity - * bf->cruise_vmax - used during forward planning to set cruise velocity - * bf->exit_vmax - used during forward planning to set exit velocity - * bf->delta_vmax - used during forward planning to set exit velocity - * - * bf->recip_jerk - used during trapezoid generation - * bf->cbrt_jerk - used during trapezoid generation - * - * Variables that will be set during processing: - * - * bf->replannable - set if the block becomes optimally planned - * - * bf->braking_velocity - set during backward planning - * bf->entry_velocity - set during forward planning - * bf->cruise_velocity - set during forward planning - * bf->exit_velocity - set during forward planning - * - * bf->head_length - set during trapezoid generation - * bf->body_length - set during trapezoid generation - * bf->tail_length - set during trapezoid generation - * - * Variables that are ignored but here's what you would expect them to be: - * bf->move_state - NEW for all blocks but the earliest - * bf->target[] - block target position - * bf->unit[] - block unit vector - * bf->time - gets set later - * bf->jerk - source of the other jerk variables. Used in mr. - */ -/* Notes: - * [1] Whether or not a block is planned is controlled by the bf->replannable - * setting (set TRUE if it should be). Replan flags are checked during the - * backwards pass and prune the replan list to include only the the latest - * blocks that require planning - * - * In normal operation the first block (currently running block) is not - * replanned, but may be for feedholds and feed overrides. In these cases - * the prep routines modify the contents of the mr buffer and re-shuffle - * the block list, re-enlisting the current bf buffer with new parameters. - * These routines also set all blocks in the list to be replannable so the - * list can be recomputed regardless of exact stops and previous replanning - * optimizations. - * - * [2] The mr_flag is used to tell replan to account for mr buffer's exit velocity (Vx) - * mr's Vx is always found in the provided bf buffer. Used to replan feedholds - */ -static void _plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) -{ - mpBuf_t *bp = bf; - - // Backward planning pass. Find first block and update the braking velocities. - // At the end *bp points to the buffer before the first block. - while ((bp = mp_get_prev_buffer(bp)) != bf) { - if (bp->replannable == false) { break; } - bp->braking_velocity = min(bp->nx->entry_vmax, bp->nx->braking_velocity) + bp->delta_vmax; - } - - // forward planning pass - recomputes trapezoids in the list from the first block to the bf block. - while ((bp = mp_get_next_buffer(bp)) != bf) { - if ((bp->pv == bf) || (*mr_flag == true)) { - bp->entry_velocity = bp->entry_vmax; // first block in the list - *mr_flag = false; - } else { - bp->entry_velocity = bp->pv->exit_velocity; // other blocks in the list - } - bp->cruise_velocity = bp->cruise_vmax; - bp->exit_velocity = min4( bp->exit_vmax, - bp->nx->entry_vmax, - bp->nx->braking_velocity, - (bp->entry_velocity + bp->delta_vmax) ); - - mp_calculate_trapezoid(bp); - - // test for optimally planned trapezoids - only need to check various exit conditions - if ( ( (fp_EQ(bp->exit_velocity, bp->exit_vmax)) || - (fp_EQ(bp->exit_velocity, bp->nx->entry_vmax)) ) || - ( (bp->pv->replannable == false) && - (fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax))) ) ) { - bp->replannable = false; - } - } - // finish up the last block move - bp->entry_velocity = bp->pv->exit_velocity; - bp->cruise_velocity = bp->cruise_vmax; - bp->exit_velocity = 0; - mp_calculate_trapezoid(bp); -} - -/* - * _reset_replannable_list() - resets all blocks in the planning list to be replannable - */ -static void _reset_replannable_list() -{ - mpBuf_t *bf = mp_get_first_buffer(); - if (bf == 0) return; - mpBuf_t *bp = bf; - do { - bp->replannable = true; - } while (((bp = mp_get_next_buffer(bp)) != bf) && (bp->move_state != MOVE_OFF)); -} - -/* - * _get_junction_vmax() - Sonny's algorithm - simple - * - * Computes the maximum allowable junction speed by finding the velocity that will yield - * the centripetal acceleration in the corner_acceleration value. The value of delta sets - * the effective radius of curvature. Here's Sonny's (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), - * 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 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 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. - * - * (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: - * sin(theta/2) = R/(R+delta) Re-arranging in terms of circle radius (R) - * R = delta*sin(theta/2)/(1-sin(theta/2). - * - * Theta is the angle between line segments given by: - * 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 angle identity: - * 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." - * - * How to compute the radius using brute-force trig: - * float theta = acos(costheta); - * 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. - * - * 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: - * - * U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i]) - * Usum Length of sums Ux + Uy - * d Delta of sums (Dx*Ux+DY*UY)/Usum - */ - -static float _get_junction_vmax(const float a_unit[], const float b_unit[]) -{ - float costheta = - (a_unit[AXIS_X] * b_unit[AXIS_X]) - - (a_unit[AXIS_Y] * b_unit[AXIS_Y]) - - (a_unit[AXIS_Z] * b_unit[AXIS_Z]) - - (a_unit[AXIS_A] * b_unit[AXIS_A]) - - (a_unit[AXIS_B] * b_unit[AXIS_B]) - - (a_unit[AXIS_C] * b_unit[AXIS_C]); - - if (costheta < -0.99) { return 10000000; } // straight line cases - if (costheta > 0.99) { return 0; } // reversal cases - - // Fuse the junction deviations into a vector sum - float a_delta = square(a_unit[AXIS_X] * cm.a[AXIS_X].junction_dev); - a_delta += square(a_unit[AXIS_Y] * cm.a[AXIS_Y].junction_dev); - a_delta += square(a_unit[AXIS_Z] * cm.a[AXIS_Z].junction_dev); - a_delta += square(a_unit[AXIS_A] * cm.a[AXIS_A].junction_dev); - a_delta += square(a_unit[AXIS_B] * cm.a[AXIS_B].junction_dev); - a_delta += square(a_unit[AXIS_C] * cm.a[AXIS_C].junction_dev); - - float b_delta = square(b_unit[AXIS_X] * cm.a[AXIS_X].junction_dev); - b_delta += square(b_unit[AXIS_Y] * cm.a[AXIS_Y].junction_dev); - b_delta += square(b_unit[AXIS_Z] * cm.a[AXIS_Z].junction_dev); - b_delta += square(b_unit[AXIS_A] * cm.a[AXIS_A].junction_dev); - b_delta += square(b_unit[AXIS_B] * cm.a[AXIS_B].junction_dev); - b_delta += square(b_unit[AXIS_C] * cm.a[AXIS_C].junction_dev); - - float delta = (sqrt(a_delta) + sqrt(b_delta))/2; - float sintheta_over2 = sqrt((1 - costheta)/2); - float radius = delta * sintheta_over2 / (1-sintheta_over2); - float velocity = sqrt(radius * cm.junction_acceleration); -// printf ("v:%f\n", velocity); //+++++ - return velocity; -} - -/************************************************************************* - * feedholds - functions for performing holds - * - * mp_plan_hold_callback() - replan block list to execute hold - * mp_end_hold() - release the hold and restart block list - * - * Feedhold is executed as cm.hold_state transitions executed inside - * _exec_aline() and main loop callbacks to these functions: - * mp_plan_hold_callback() and mp_end_hold(). - */ -/* Holds work like this: - * - * - Hold is asserted by calling cm_feedhold() (usually invoked via a ! char) - * If hold_state is OFF and motion_state is RUNning it sets - * hold_state to SYNC and motion_state to HOLD. - * - * - Hold state == SYNC tells the aline exec routine to execute the next aline - * segment then set hold_state to PLAN. This gives the planner sufficient - * time to replan the block list for the hold before the next aline segment - * needs to be processed. - * - * - Hold state == PLAN tells the planner to replan the mr buffer, the current - * run buffer (bf), and any subsequent bf buffers as necessary to execute a - * hold. Hold planning replans the planner buffer queue down to zero and then - * back up from zero. Hold state is set to DECEL when planning is complete. - * - * - Hold state == DECEL persists until the aline execution runs to zero - * velocity, at which point hold state transitions to HOLD. - * - * - Hold state == HOLD persists until the cycle is restarted. A cycle start - * is an asynchronous event that sets the cycle_start_flag TRUE. It can - * occur any time after the hold is requested - either before or after - * motion stops. - * - * - mp_end_hold() is executed from cm_feedhold_sequencing_callback() once the - * hold state == HOLD and a cycle_start has been requested.This sets the hold - * state to OFF which enables _exec_aline() to continue processing. Move - * execution begins with the first buffer after the hold. - * - * Terms used: - * - mr is the runtime buffer. It was initially loaded from the bf buffer - * - bp+0 is the "companion" bf buffer to the mr buffer. - * - bp+1 is the bf buffer following bp+0. This runs through bp+N - * - bp (by itself) just refers to the current buffer being adjusted / replanned - * - * Details: Planning re-uses bp+0 as an "extra" buffer. Normally bp+0 is returned - * to the buffer pool as it is redundant once mr is loaded. Use the extra - * buffer to split the move in two where the hold decelerates to zero. Use - * one buffer to go to zero, the other to replan up from zero. All buffers past - * that point are unaffected other than that they need to be replanned for velocity. - * - * Note: There are multiple opportunities for more efficient organization of - * code in this module, but the code is so complicated I just left it - * organized for clarity and hoped for the best from compiler optimization. - */ - -static float _compute_next_segment_velocity() -{ - if (mr.section == SECTION_BODY) return mr.segment_velocity; -#ifdef __JERK_EXEC - return mr.segment_velocity; // an approximation -#else - return mr.segment_velocity + mr.forward_diff_5; -#endif -} - -stat_t mp_plan_hold_callback() -{ - if (cm.hold_state != FEEDHOLD_PLAN) - return STAT_NOOP; // not planning a feedhold - - mpBuf_t *bp; // working buffer pointer - if ((bp = mp_get_run_buffer()) == 0) - return STAT_NOOP; // Oops! nothing's running - - uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx - float mr_available_length; // available length left in mr buffer for deceleration - float braking_velocity; // velocity left to shed to brake to zero - float braking_length; // distance required to brake to zero from braking_velocity - - // examine and process mr buffer - mr_available_length = get_axis_vector_length(mr.target, mr.position); - - // compute next_segment velocity - braking_velocity = _compute_next_segment_velocity(); - braking_length = mp_get_target_length(braking_velocity, 0, bp); // bp is OK to use here - - // Hack to prevent Case 2 moves for perfect-fit decels. Happens in homing situations - // The real fix: The braking velocity cannot simply be the mr.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 ((braking_length > mr_available_length) && (fp_ZERO(bp->exit_velocity))) { - braking_length = mr_available_length; - } - - // Case 1: deceleration fits entirely into the length remaining in mr buffer - if (braking_length <= mr_available_length) { - // set mr to a tail to perform the deceleration - mr.exit_velocity = 0; - mr.tail_length = braking_length; - mr.cruise_velocity = braking_velocity; - mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; - - // re-use bp+0 to be the hold point and to run the remaining block length - bp->length = mr_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->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer - - _reset_replannable_list(); // make it replan all the blocks - _plan_block_list(mp_get_last_buffer(), &mr_flag); - cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit - return STAT_OK; - } - - // Case 2: deceleration exceeds length remaining in mr buffer - // First, replan mr to minimum (but non-zero) exit velocity - - mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; - mr.tail_length = mr_available_length; - mr.cruise_velocity = braking_velocity; - mr.exit_velocity = braking_velocity - mp_get_target_velocity(0, mr_available_length, bp); - - // Find the point where deceleration reaches zero. This could span multiple buffers. - braking_velocity = mr.exit_velocity; // adjust braking velocity downward - bp->move_state = MOVE_NEW; // tell _exec to re-use buffer - for (uint8_t i=0; inx); // copy bp+1 into bp+0 (and onward...) - if (bp->move_type != MOVE_TYPE_ALINE) { // skip any non-move buffers - bp = mp_get_next_buffer(bp); // point to next buffer - continue; - } - bp->entry_vmax = braking_velocity; // velocity we need to shed - braking_length = mp_get_target_length(braking_velocity, 0, bp); - - if (braking_length > bp->length) { // decel does not fit in bp buffer - bp->exit_vmax = braking_velocity - mp_get_target_velocity(0, bp->length, bp); - braking_velocity = bp->exit_vmax; // braking velocity for next buffer - bp = mp_get_next_buffer(bp); // point to next buffer - continue; - } - break; - } - // Deceleration now fits in the current bp buffer - // Plan the first buffer of the pair as the decel, the second as the accel - bp->length = braking_length; - bp->exit_vmax = 0; - - bp = mp_get_next_buffer(bp); // point to the acceleration buffer - bp->entry_vmax = 0; - bp->length -= braking_length; // the buffers were identical (and hence their lengths) - bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); - bp->exit_vmax = bp->delta_vmax; - - _reset_replannable_list(); // make it replan all the blocks - _plan_block_list(mp_get_last_buffer(), &mr_flag); - cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit - return STAT_OK; -} - -/* - * mp_end_hold() - end a feedhold - */ -stat_t mp_end_hold() -{ - if (cm.hold_state == FEEDHOLD_END_HOLD) { - cm.hold_state = FEEDHOLD_OFF; - mpBuf_t *bf; - if ((bf = mp_get_run_buffer()) == 0) { // 0 means nothing's running - cm_set_motion_state(MOTION_STOP); - return STAT_NOOP; - } - cm.motion_state = MOTION_RUN; - st_request_exec_move(); // restart the steppers - } - return STAT_OK; -} diff --git a/src/plan_line.h b/src/plan_line.h deleted file mode 100644 index 34e582d..0000000 --- a/src/plan_line.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * plan_line.h - acceleration managed line planning and motion execution - * Part of TinyG project - * - * Copyright (c) 2010 - 2013 Alden S. Hart Jr. - * Copyright (c) 2012 - 2013 Rob Giseburt - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#ifndef plan_line_h -#define plan_line_h - -// NOTE: function prototypes are in planner.h for ease of access by external files - -#endif diff --git a/src/planner.c b/src/planner.c deleted file mode 100644 index 07c02bb..0000000 --- a/src/planner.c +++ /dev/null @@ -1,441 +0,0 @@ -/* - * planner.c - Cartesian trajectory planning and motion execution - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - * Copyright (c) 2012 - 2015 Rob Giseburt - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* --- Planner Notes ---- - * - * The planner works below the canonical machine and above the motor mapping - * and stepper execution layers. A rudimentary multitasking capability is - * implemented for long-running commands such as lines, arcs, and dwells. - * These functions are coded as non-blocking continuations - which are simple - * state machines that are re-entered multiple times until a particular - * operation is complete. These functions have 2 parts - the initial call, - * which sets up the local context, and callbacks (continuations) that are - * called from the main loop (in controller.c). - * - * One important concept is isolation of the three layers of the data model - - * the Gcode model (gm), planner model (bf queue & mm), and runtime model (mr). - * These are designated as "model", "planner" and "runtime" in function names. - * - * The Gcode model is owned by the canonical machine and should only be accessed - * by cm_xxxx() functions. Data from the Gcode model is transferred to the planner - * by the mp_xxx() functions called by the canonical machine. - * - * The planner should only use data in the planner model. When a move (block) - * is ready for execution the planner data is transferred to the runtime model, - * which should also be isolated. - * - * Lower-level models should never use data from upper-level models as the data - * may have changed and lead to unpredictable results. - */ -#include "tinyg.h" -#include "config.h" -#include "canonical_machine.h" -#include "plan_arc.h" -#include "planner.h" -#include "kinematics.h" -#include "stepper.h" -#include "encoder.h" -#include "report.h" -#include "util.h" - -// Allocate planner structures -mpBufferPool_t mb; // move buffer queue -mpMoveMasterSingleton_t mm; // context for line planning -mpMoveRuntimeSingleton_t mr; // context for line runtime - -/* - * Local Scope Data and Functions - */ -#define _bump(a) ((a, e.g. an M code) - * - cm_ function calls mp_queue_command which puts it in the planning queue (bf buffer). - * This involves setting some parameters and registering a callback to the - * execution function in the canonical machine - * - the planning queue gets to the function and calls _exec_command() - * - ...which puts a pointer to the bf buffer in the prep stratuc (st_pre) - * - When the runtime gets to the end of the current activity (sending steps, counting a dwell) - * if executes mp_runtime_command... - * - ...which uses the callback function in the bf and the saved parameters in the vectors - * - To finish up mp_runtime_command() needs to free the bf buffer - * - * 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 much easier - therefore avoiding Q starvation - */ - -void mp_queue_command(void(*cm_exec)(float[], float[]), float *value, float *flag) -{ - mpBuf_t *bf; - - // Never supposed to fail as buffer availability was checked upstream in the controller - if ((bf = mp_get_write_buffer()) == 0) { - cm_hard_alarm(STAT_BUFFER_FULL_FATAL); - return; - } - - bf->move_type = MOVE_TYPE_COMMAND; - bf->bf_func = _exec_command; // callback to planner queue exec function - bf->cm_func = cm_exec; // callback to canonical machine exec function - - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - bf->value_vector[axis] = value[axis]; - bf->flag_vector[axis] = flag[axis]; - } - mp_commit_write_buffer(MOVE_TYPE_COMMAND); // must be final operation before exit -} - -static stat_t _exec_command(mpBuf_t *bf) -{ - st_prep_command(bf); - return STAT_OK; -} - -stat_t mp_runtime_command(mpBuf_t *bf) -{ - bf->cm_func(bf->value_vector, bf->flag_vector); // 2 vectors used by callbacks - if (mp_free_run_buffer()) - cm_cycle_end(); // free buffer & perform cycle_end if planner is empty - return STAT_OK; -} - -/************************************************************************* - * mp_dwell() - queue a dwell - * _exec_dwell() - dwell execution - * - * Dwells are performed by passing a dwell move to the stepper drivers. - * When the stepper driver sees a dwell it times the dwell on a separate - * timer than the stepper pulse timer. - */ -stat_t mp_dwell(float seconds) -{ - mpBuf_t *bf; - - if ((bf = mp_get_write_buffer()) == 0) // get write buffer or fail - return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // not ever supposed to fail - - bf->bf_func = _exec_dwell; // register callback to dwell start - bf->gm.move_time = seconds; // in seconds, not minutes - bf->move_state = MOVE_NEW; - mp_commit_write_buffer(MOVE_TYPE_DWELL); // must be final operation before exit - return STAT_OK; -} - -static stat_t _exec_dwell(mpBuf_t *bf) -{ - st_prep_dwell((uint32_t)(bf->gm.move_time * 1000000));// convert seconds to uSec - if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & perform cycle_end if planner is empty - return STAT_OK; -} - -/**** PLANNER BUFFERS ***************************************************** - * - * Planner buffers are used to queue and operate on Gcode blocks. Each buffer - * contains one Gcode block which may be a move, and M code, or other command - * that must be executed synchronously with movement. - * - * Buffers are in a circularly linked list managed by a WRITE pointer and a RUN pointer. - * New blocks are populated by (1) getting a write buffer, (2) populating the buffer, - * then (3) placing it in the queue (queue write buffer). If an exception occurs - * during population you can unget the write buffer before queuing it, which returns - * it to the pool of available buffers. - * - * The RUN buffer is the buffer currently executing. It may be retrieved once for - * simple commands, or multiple times for long-running commands like moves. When - * the command is complete the run buffer is returned to the pool by freeing it. - * - * Notes: - * The write buffer pointer only moves forward on _queue_write_buffer, and - * the read buffer pointer only moves forward on free_read calls. - * (test, get and unget have no effect) - * - * mp_get_planner_buffers_available() Returns # of available planner buffers - * - * mp_init_buffers() Initializes or resets buffers - * - * mp_get_write_buffer() Get pointer to next available write buffer - * Returns pointer or 0 if no buffer available. - * - * mp_unget_write_buffer() Free write buffer if you decide not to commit it. - * - * mp_commit_write_buffer() Commit the next write buffer to the queue - * Advances write pointer & changes buffer state - * WARNING: The calling routine must not use the write buffer - * once it has been queued as it may be processed and freed (wiped) - * before mp_queue_write_buffer() returns. - * - * mp_get_run_buffer() Get pointer to the next or current run buffer - * Returns a new run buffer if prev buf was ENDed - * Returns same buf if called again before ENDing - * Returns 0 if no buffer available - * The behavior supports continuations (iteration) - * - * mp_free_run_buffer() Release the run buffer & return to buffer pool. - * Returns true if queue is empty, false otherwise. - * This is useful for doing queue empty / end move functions. - * - * mp_get_prev_buffer(bf) Returns pointer to prev buffer in linked list - * mp_get_next_buffer(bf) Returns pointer to next buffer in linked list - * mp_get_first_buffer(bf) Returns pointer to first buffer, i.e. the running block - * mp_get_last_buffer(bf) Returns pointer to last buffer, i.e. last block (zero) - * mp_clear_buffer(bf) Zeroes the contents of the buffer - * mp_copy_buffer(bf,bp) Copies the contents of bp into bf - preserves links - */ - -uint8_t mp_get_planner_buffers_available() { return mb.buffers_available;} - -void mp_init_buffers() -{ - mpBuf_t *pv; - uint8_t i; - - memset(&mb, 0, sizeof(mb)); // clear all values, pointers and status - mb.magic_start = MAGICNUM; - mb.magic_end = MAGICNUM; - - mb.w = &mb.bf[0]; // init write and read buffer pointers - mb.q = &mb.bf[0]; - mb.r = &mb.bf[0]; - pv = &mb.bf[PLANNER_BUFFER_POOL_SIZE-1]; - for (i=0; i < PLANNER_BUFFER_POOL_SIZE; i++) { // setup ring pointers - mb.bf[i].nx = &mb.bf[_bump(i)]; - mb.bf[i].pv = pv; - pv = &mb.bf[i]; - } - mb.buffers_available = PLANNER_BUFFER_POOL_SIZE; -} - -mpBuf_t * mp_get_write_buffer() // get & clear a buffer -{ - if (mb.w->buffer_state == MP_BUFFER_EMPTY) { - mpBuf_t *w = mb.w; - mpBuf_t *nx = mb.w->nx; // save linked list pointers - mpBuf_t *pv = mb.w->pv; - memset(mb.w, 0, sizeof(mpBuf_t)); // clear all values - w->nx = nx; // restore pointers - w->pv = pv; - w->buffer_state = MP_BUFFER_LOADING; - mb.buffers_available--; - mb.w = w->nx; - return w; - } - rpt_exception(STAT_FAILED_TO_GET_PLANNER_BUFFER); - return 0; -} - -void mp_unget_write_buffer() -{ - mb.w = mb.w->pv; // queued --> write - mb.w->buffer_state = MP_BUFFER_EMPTY; // not loading anymore - mb.buffers_available++; -} - -/*** WARNING: The routine calling mp_commit_write_buffer() must not use the write buffer - once it has been queued. Action may start on the buffer immediately, - invalidating its contents ***/ - -void mp_commit_write_buffer(const uint8_t move_type) -{ - mb.q->move_type = move_type; - mb.q->move_state = MOVE_NEW; - mb.q->buffer_state = MP_BUFFER_QUEUED; - mb.q = mb.q->nx; // advance the queued buffer pointer - qr_request_queue_report(+1); // request a QR and add to the "added buffers" count - st_request_exec_move(); // requests an exec if the runtime is not busy - // NB: BEWARE! the exec may result in the planner buffer being - // processed immediately and then freed - invalidating the contents -} - -mpBuf_t * mp_get_run_buffer() -{ - // CASE: fresh buffer; becomes running if queued or pending - if ((mb.r->buffer_state == MP_BUFFER_QUEUED) || - (mb.r->buffer_state == MP_BUFFER_PENDING)) { - mb.r->buffer_state = MP_BUFFER_RUNNING; - } - // CASE: asking for the same run buffer for the Nth time - if (mb.r->buffer_state == MP_BUFFER_RUNNING) { // return same buffer - return mb.r; - } - return 0; // CASE: no queued buffers. fail it. -} - -uint8_t mp_free_run_buffer() // EMPTY current run buf & adv to next -{ - mp_clear_buffer(mb.r); // clear it out (& reset replannable) -// mb.r->buffer_state = MP_BUFFER_EMPTY; // redundant after the clear, above - mb.r = mb.r->nx; // advance to next run buffer - if (mb.r->buffer_state == MP_BUFFER_QUEUED) {// only if queued... - mb.r->buffer_state = MP_BUFFER_PENDING; // pend next buffer - } - mb.buffers_available++; - qr_request_queue_report(-1); // request a QR and add to the "removed buffers" count - return (mb.w == mb.r ? true : false); // return true if the queue emptied -} - -mpBuf_t * mp_get_first_buffer() -{ - return mp_get_run_buffer(); // returns buffer or 0 if nothing's running -} - -mpBuf_t * mp_get_last_buffer() -{ - mpBuf_t *bf = mp_get_run_buffer(); - mpBuf_t *bp = bf; - - if (bf == 0) return 0; - - do { - if ((bp->nx->move_state == MOVE_OFF) || (bp->nx == bf)) { - return bp; - } - } while ((bp = mp_get_next_buffer(bp)) != bf); - return bp; -} - -// Use the macro instead -//mpBuf_t * mp_get_prev_buffer(const mpBuf_t *bf) return bf->pv; -//mpBuf_t * mp_get_next_buffer(const mpBuf_t *bf) return bf->nx; - -void mp_clear_buffer(mpBuf_t *bf) -{ - mpBuf_t *nx = bf->nx; // save pointers - mpBuf_t *pv = bf->pv; - memset(bf, 0, sizeof(mpBuf_t)); - bf->nx = nx; // restore pointers - bf->pv = pv; -} - -void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp) -{ - mpBuf_t *nx = bf->nx; // save pointers - mpBuf_t *pv = bf->pv; - memcpy(bf, bp, sizeof(mpBuf_t)); - bf->nx = nx; // restore pointers - bf->pv = pv; -} diff --git a/src/planner.h b/src/planner.h deleted file mode 100644 index b6dbf85..0000000 --- a/src/planner.h +++ /dev/null @@ -1,322 +0,0 @@ -/* - * planner.h - cartesian trajectory planning and motion execution - * This file is part of the TinyG project - * - * Copyright (c) 2013 - 2015 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2015 Robert Giseburt - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#ifndef PLANNER_H_ONCE -#define PLANNER_H_ONCE - -#include "canonical_machine.h" // used for GCodeState_t - -enum moveType { // bf->move_type values - MOVE_TYPE_0 = 0, // 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_TOOL, // T command - MOVE_TYPE_SPINDLE_SPEED,// S command - MOVE_TYPE_STOP, // program stop - MOVE_TYPE_END // program end -}; - -enum moveState { - MOVE_OFF = 0, // move inactive (MUST BE ZERO) - MOVE_NEW, // general value if you need an initialization - MOVE_RUN, // general run state (for non-acceleration moves) - MOVE_SKIP_BLOCK // mark a skipped block -}; - -enum moveSection { - SECTION_HEAD = 0, // acceleration - SECTION_BODY, // cruise - SECTION_TAIL // deceleration -}; -#define SECTIONS 3 - -enum sectionState { - SECTION_OFF = 0, // section inactive - SECTION_NEW, // uninitialized section - SECTION_1st_HALF, // first half of S curve - SECTION_2nd_HALF // second half of S curve or running a BODY (cruise) -}; - -/*** Most of these factors are the result of a lot of tweaking. Change with caution.***/ - -#define ARC_SEGMENT_LENGTH ((float)0.1) // Arc segment size (mm).(0.03) -#define MIN_ARC_RADIUS ((float)0.1) - -#define JERK_MULTIPLIER ((float)1000000) -#define JERK_MATCH_PRECISION ((float)1000) // precision to which jerk must match to be considered effectively the same - -#define NOM_SEGMENT_USEC ((float)5000) // nominal segment time -#define MIN_SEGMENT_USEC ((float)2500) // minimum segment time / minimum move time -#define MIN_ARC_SEGMENT_USEC ((float)10000) // minimum arc segment time - -#define NOM_SEGMENT_TIME (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) -#define MIN_SEGMENT_TIME (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) -#define MIN_ARC_SEGMENT_TIME (MIN_ARC_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) -#define MIN_TIME_MOVE MIN_SEGMENT_TIME // minimum time a move can be is one segment -#define MIN_BLOCK_TIME MIN_SEGMENT_TIME // factor for minimum size Gcode block to process - -#define MIN_SEGMENT_TIME_PLUS_MARGIN ((MIN_SEGMENT_USEC+1) / MICROSECONDS_PER_MINUTE) - -/* PLANNER_STARTUP_DELAY_SECONDS - * Used to introduce a short dwell before planning an idle machine. - * If you don't do this the first block will always plan to zero as it will - * start executing before the next block arrives from the serial port. - * This causes the machine to stutter once on startup. - */ -//#define PLANNER_STARTUP_DELAY_SECONDS ((float)0.05) // in seconds - -/* PLANNER_BUFFER_POOL_SIZE - * Should be at least the number of buffers requires to support optimal - * planning in the case of very short lines or arc segments. - * Suggest 12 min. Limit is 255 - */ -#define PLANNER_BUFFER_POOL_SIZE 32 -#define PLANNER_BUFFER_HEADROOM 4 // buffers to reserve in planner before processing new input line - -/* Some parameters for _generate_trapezoid() - * TRAPEZOID_ITERATION_MAX Max iterations for convergence in the HT asymmetric case. - * TRAPEZOID_ITERATION_ERROR_PERCENT Error percentage for iteration convergence. As percent - 0.01 = 1% - * TRAPEZOID_LENGTH_FIT_TOLERANCE Tolerance for "exact fit" for H and T cases - * TRAPEZOID_VELOCITY_TOLERANCE Adaptive velocity tolerance term - */ -#define TRAPEZOID_ITERATION_MAX 10 -#define TRAPEZOID_ITERATION_ERROR_PERCENT ((float)0.10) -#define TRAPEZOID_LENGTH_FIT_TOLERANCE ((float)0.0001) // allowable mm of error in planning phase -#define TRAPEZOID_VELOCITY_TOLERANCE (max(2,bf->entry_velocity/100)) - -/* - * Macros and typedefs - */ - -typedef void (*cm_exec_t)(float[], float[]); // callback to canonical_machine execution function - -/* - * Planner structures - */ - -// All the enums that equal zero must be zero. Don't change this - -enum mpBufferState { // bf->buffer_state values - MP_BUFFER_EMPTY = 0, // struct is available for use (MUST BE 0) - MP_BUFFER_LOADING, // being written ("checked out") - MP_BUFFER_QUEUED, // in queue - MP_BUFFER_PENDING, // marked as the next buffer to run - MP_BUFFER_RUNNING // current running buffer -}; - -typedef struct mpBuffer { // See Planning Velocity Notes for variable usage - struct mpBuffer *pv; // static pointer to previous buffer - struct mpBuffer *nx; // static pointer to next buffer - stat_t (*bf_func)(struct mpBuffer *bf); // callback to buffer exec function - cm_exec_t cm_func; // callback to canonical machine execution function - - float naiive_move_time; - - uint8_t buffer_state; // used to manage queuing/dequeuing - uint8_t move_type; // used to dispatch to run routine - uint8_t move_code; // byte that can be used by used exec functions - uint8_t move_state; // move state machine sequence - uint8_t replannable; // TRUE if move can be re-planned - - float unit[AXES]; // unit vector for axis scaling & planning - - float length; // total length of line or helix in mm - float head_length; - float body_length; - float tail_length; - // *** SEE NOTES ON THESE VARIABLES, in aline() *** - float entry_velocity; // entry velocity requested for the move - float cruise_velocity; // cruise velocity requested & achieved - float exit_velocity; // exit velocity requested for the move - - float entry_vmax; // max junction velocity at entry of this move - float cruise_vmax; // max cruise velocity requested for move - float exit_vmax; // max exit velocity possible (redundant) - float delta_vmax; // max velocity difference for this move - float braking_velocity; // current value for braking velocity - - uint8_t jerk_axis; // rate limiting axis used to compute jerk for the move - float jerk; // maximum linear jerk term for this move - float recip_jerk; // 1/Jm used for planning (computed and cached) - float cbrt_jerk; // cube root of Jm used for planning (computed and cached) - - GCodeState_t gm; // Gode model state - passed from model, used by planner and runtime - -} mpBuf_t; - -typedef struct mpBufferPool { // ring buffer for sub-moves - magic_t magic_start; // magic number to test memory integrity - uint8_t buffers_available; // running count of available buffers - mpBuf_t *w; // get_write_buffer pointer - mpBuf_t *q; // queue_write_buffer pointer - mpBuf_t *r; // get/end_run_buffer pointer - mpBuf_t bf[PLANNER_BUFFER_POOL_SIZE];// buffer storage - magic_t magic_end; -} mpBufferPool_t; - -typedef struct mpMoveMasterSingleton { // common variables for planning (move master) - magic_t magic_start; // magic number to test memory integrity - float position[AXES]; // final move position for planning purposes - - float jerk; // jerk values cached from previous block - float recip_jerk; - float cbrt_jerk; - - magic_t magic_end; -} mpMoveMasterSingleton_t; - -typedef struct mpMoveRuntimeSingleton { // persistent runtime variables -// uint8_t (*run_move)(struct mpMoveRuntimeSingleton *m); // currently running move - left in for reference - magic_t magic_start; // magic number to test memory integrity - uint8_t move_state; // state of the overall move - uint8_t section; // what section is the move in? - uint8_t section_state; // state within a move section - - float unit[AXES]; // unit vector for axis scaling & planning - float target[AXES]; // final target for bf (used to correct rounding errors) - float position[AXES]; // current move position - float position_c[AXES]; // for Kahan summation in _exec_aline_segment() - float waypoint[SECTIONS][AXES]; // head/body/tail endpoints for correction - - float target_steps[MOTORS]; // current MR target (absolute target as steps) - float position_steps[MOTORS]; // current MR position (target from previous segment) - float commanded_steps[MOTORS]; // will align with next encoder sample (target from 2nd previous segment) - float encoder_steps[MOTORS]; // encoder position in steps - ideally the same as commanded_steps - float following_error[MOTORS]; // difference between encoder_steps and commanded steps - - float head_length; // copies of bf variables of same name - float body_length; - float tail_length; - - float entry_velocity; - float cruise_velocity; - float exit_velocity; - - float segments; // number of segments in line (also used by arc generation) - uint32_t segment_count; // count of running segments - float segment_velocity; // computed velocity for aline segment - float segment_time; // actual time increment per aline segment - float jerk; // max linear jerk - -#ifdef __JERK_EXEC // values used exclusively by computed jerk acceleration - float jerk_div2; // cached value for efficiency - float midpoint_velocity; // velocity at accel/decel midpoint - float midpoint_acceleration; // - float accel_time; // - float segment_accel_time; // - float elapsed_accel_time; // -#else // values used exclusively by forward differencing acceleration - float forward_diff_1; // forward difference level 1 - float forward_diff_2; // forward difference level 2 - float forward_diff_3; // forward difference level 3 - float forward_diff_4; // forward difference level 4 - float forward_diff_5; // forward difference level 5 -#ifdef __KAHAN - float forward_diff_1_c; // forward difference level 1 floating-point compensation - float forward_diff_2_c; // forward difference level 2 floating-point compensation - float forward_diff_3_c; // forward difference level 3 floating-point compensation - float forward_diff_4_c; // forward difference level 4 floating-point compensation - float forward_diff_5_c; // forward difference level 5 floating-point compensation -#endif -#endif - - GCodeState_t gm; // gcode model state currently executing - - magic_t magic_end; -} mpMoveRuntimeSingleton_t; - -// Reference global scope structures -extern mpBufferPool_t mb; // move buffer queue -extern mpMoveMasterSingleton_t mm; // context for line planning -extern mpMoveRuntimeSingleton_t mr; // context for line runtime - -/* - * Global Scope Functions - */ - -void planner_init(); -void planner_init_assertions(); -stat_t planner_test_assertions(); - -void mp_flush_planner(); -void mp_set_planner_position(uint8_t axis, const float position); -void mp_set_runtime_position(uint8_t axis, const float position); -void mp_set_steps_to_runtime_position(); - -void mp_queue_command(void(*cm_exec_t)(float[], float[]), float *value, float *flag); -stat_t mp_runtime_command(mpBuf_t *bf); - -stat_t mp_dwell(const float seconds); -void mp_end_dwell(); - -stat_t mp_aline(GCodeState_t *gm_in); - -stat_t mp_plan_hold_callback(); -stat_t mp_end_hold(); -stat_t mp_feed_rate_override(uint8_t flag, float parameter); - -// planner buffer handlers -uint8_t mp_get_planner_buffers_available(); -void mp_init_buffers(); -mpBuf_t * mp_get_write_buffer(); -void mp_unget_write_buffer(); -void mp_commit_write_buffer(const uint8_t move_type); - -mpBuf_t * mp_get_run_buffer(); -uint8_t mp_free_run_buffer(); -mpBuf_t * mp_get_first_buffer(); -mpBuf_t * mp_get_last_buffer(); - -//mpBuf_t * mp_get_prev_buffer(const mpBuf_t *bf); -//mpBuf_t * mp_get_next_buffer(const mpBuf_t *bf); -#define mp_get_prev_buffer(b) ((mpBuf_t *)(b->pv)) // use the macro instead -#define mp_get_next_buffer(b) ((mpBuf_t *)(b->nx)) - -void mp_clear_buffer(mpBuf_t *bf); -void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp); - -// plan_line.c functions -float mp_get_runtime_velocity(); -float mp_get_runtime_work_position(uint8_t axis); -float mp_get_runtime_absolute_position(uint8_t axis); -void mp_set_runtime_work_offset(float offset[]); -void mp_zero_segment_velocity(); -uint8_t mp_get_runtime_busy(); -float* mp_get_planner_position_vector(); - -// plan_zoid.c functions -void mp_calculate_trapezoid(mpBuf_t *bf); -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); - -// plan_exec.c functions -stat_t mp_exec_move(); -stat_t mp_exec_aline(mpBuf_t *bf); - -#endif // End of include Guard: PLANNER_H_ONCE diff --git a/src/pwm.c b/src/pwm.c index f907ab9..2eddd39 100644 --- a/src/pwm.c +++ b/src/pwm.c @@ -25,15 +25,13 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" // #1 -#include "config.h" // #2 -#include "hardware.h" -#include "text_parser.h" -#include "gpio.h" #include "pwm.h" -#include +#include "config.h" +#include "hardware.h" +#include "gpio.h" +#include pwmSingleton_t pwm; @@ -57,12 +55,12 @@ pwmSingleton_t pwm; */ #define PWM1_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc // starting clock select value #define PWM1_CTRLB (3 | TC0_CCBEN_bm) // single slope PWM enabled on channel B -#define PWM1_ISR_vect TCD1_CCB_vect // must match timer assignments in system.h +#define PWM1_ISR_vect TCD1_CCB_vect #define PWM1_INTCTRLB 0 // timer interrupt level (0=off, 1=lo, 2=med, 3=hi) #define PWM2_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc #define PWM2_CTRLB 3 // single slope PWM enabled, no output channel -#define PWM2_ISR_vect TCE1_CCB_vect // must match timer assignments in system.h +#define PWM2_ISR_vect TCE1_CCB_vect #define PWM2_INTCTRLB 0 // timer interrupt level (0=off, 1=lo, 2=med, 3=hi) @@ -72,7 +70,6 @@ pwmSingleton_t pwm; * Notes: * - Whatever level interrupts you use must be enabled in main() * - init assumes PWM1 output bit (D5) has been set to output previously (stepper.c) - * - See system.h for timer and port assignments */ void pwm_init() { gpio_set_bit_off(SPINDLE_PWM); @@ -90,6 +87,17 @@ void pwm_init() { pwm.p[PWM_2].ctrla = PWM2_CTRLA_CLKSEL; pwm.p[PWM_2].timer->CTRLB = PWM2_CTRLB; pwm.p[PWM_2].timer->INTCTRLB = PWM2_INTCTRLB; + + pwm.c[PWM_1].frequency = P1_PWM_FREQUENCY; + pwm.c[PWM_1].cw_speed_lo = P1_CW_SPEED_LO; + pwm.c[PWM_1].cw_speed_hi = P1_CW_SPEED_HI; + pwm.c[PWM_1].cw_phase_lo = P1_CW_PHASE_LO; + pwm.c[PWM_1].cw_phase_hi = P1_CW_PHASE_HI; + pwm.c[PWM_1].ccw_speed_lo = P1_CCW_SPEED_LO; + pwm.c[PWM_1].ccw_speed_hi = P1_CCW_SPEED_HI; + pwm.c[PWM_1].ccw_phase_lo = P1_CCW_PHASE_LO; + pwm.c[PWM_1].ccw_phase_hi = P1_CCW_PHASE_HI; + pwm.c[PWM_1].phase_off = P1_PWM_PHASE_OFF; } @@ -109,9 +117,9 @@ ISR(PWM2_ISR_vect) {} * Doesn't turn time on until duty cycle is set */ stat_t pwm_set_freq(uint8_t chan, float freq) { - if (chan > PWMS) { return STAT_NO_SUCH_DEVICE;} - if (freq > PWM_MAX_FREQ) { return STAT_INPUT_EXCEEDS_MAX_VALUE;} - if (freq < PWM_MIN_FREQ) { return STAT_INPUT_LESS_THAN_MIN_VALUE;} + if (chan > PWMS) return STAT_NO_SUCH_DEVICE; + if (freq > PWM_MAX_FREQ) return STAT_INPUT_EXCEEDS_MAX_VALUE; + if (freq < PWM_MIN_FREQ) return STAT_INPUT_LESS_THAN_MIN_VALUE; // Set the period and the prescaler float prescale = F_CPU / 65536 / freq; // optimal non-integer prescaler value @@ -163,28 +171,3 @@ stat_t pwm_set_duty(uint8_t chan, float duty) { return STAT_OK; } - - -#ifdef __TEXT_MODE -static const char fmt_p1frq[] PROGMEM = "[p1frq] pwm frequency %15.0f Hz\n"; -static const char fmt_p1csl[] PROGMEM = "[p1csl] pwm cw speed lo %15.0f RPM\n"; -static const char fmt_p1csh[] PROGMEM = "[p1csh] pwm cw speed hi %15.0f RPM\n"; -static const char fmt_p1cpl[] PROGMEM = "[p1cpl] pwm cw phase lo %15.3f [0..1]\n"; -static const char fmt_p1cph[] PROGMEM = "[p1cph] pwm cw phase hi %15.3f [0..1]\n"; -static const char fmt_p1wsl[] PROGMEM = "[p1wsl] pwm ccw speed lo%15.0f RPM\n"; -static const char fmt_p1wsh[] PROGMEM = "[p1wsh] pwm ccw speed hi%15.0f RPM\n"; -static const char fmt_p1wpl[] PROGMEM = "[p1wpl] pwm ccw phase lo%15.3f [0..1]\n"; -static const char fmt_p1wph[] PROGMEM = "[p1wph] pwm ccw phase hi%15.3f [0..1]\n"; -static const char fmt_p1pof[] PROGMEM = "[p1pof] pwm phase off %15.3f [0..1]\n"; - -void pwm_print_p1frq(nvObj_t *nv) {text_print_flt(nv, fmt_p1frq);} -void pwm_print_p1csl(nvObj_t *nv) {text_print_flt(nv, fmt_p1csl);} -void pwm_print_p1csh(nvObj_t *nv) {text_print_flt(nv, fmt_p1csh);} -void pwm_print_p1cpl(nvObj_t *nv) {text_print_flt(nv, fmt_p1cpl);} -void pwm_print_p1cph(nvObj_t *nv) {text_print_flt(nv, fmt_p1cph);} -void pwm_print_p1wsl(nvObj_t *nv) {text_print_flt(nv, fmt_p1wsl);} -void pwm_print_p1wsh(nvObj_t *nv) {text_print_flt(nv, fmt_p1wsh);} -void pwm_print_p1wpl(nvObj_t *nv) {text_print_flt(nv, fmt_p1wpl);} -void pwm_print_p1wph(nvObj_t *nv) {text_print_flt(nv, fmt_p1wph);} -void pwm_print_p1pof(nvObj_t *nv) {text_print_flt(nv, fmt_p1pof);} -#endif //__TEXT_MODE diff --git a/src/pwm.h b/src/pwm.h index 103aa15..2f5324a 100644 --- a/src/pwm.h +++ b/src/pwm.h @@ -25,8 +25,18 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#ifndef PWM_H_ONCE -#define PWM_H_ONCE +#ifndef PWM_H +#define PWM_H + +#include "config.h" +#include "status.h" + +#include + + +#define PWM_1 0 +#define PWM_2 1 + typedef struct pwmConfigChannel { float frequency; // base frequency for PWM driver, in Hz @@ -60,33 +70,4 @@ void pwm_init(); stat_t pwm_set_freq(uint8_t channel, float freq); stat_t pwm_set_duty(uint8_t channel, float duty); - -#ifdef __TEXT_MODE - -void pwm_print_p1frq(nvObj_t *nv); -void pwm_print_p1csl(nvObj_t *nv); -void pwm_print_p1csh(nvObj_t *nv); -void pwm_print_p1cpl(nvObj_t *nv); -void pwm_print_p1cph(nvObj_t *nv); -void pwm_print_p1wsl(nvObj_t *nv); -void pwm_print_p1wsh(nvObj_t *nv); -void pwm_print_p1wpl(nvObj_t *nv); -void pwm_print_p1wph(nvObj_t *nv); -void pwm_print_p1pof(nvObj_t *nv); - -#else - -#define pwm_print_p1frq tx_print_stub -#define pwm_print_p1csl tx_print_stub -#define pwm_print_p1csh tx_print_stub -#define pwm_print_p1cpl tx_print_stub -#define pwm_print_p1cph tx_print_stub -#define pwm_print_p1wsl tx_print_stub -#define pwm_print_p1wsh tx_print_stub -#define pwm_print_p1wpl tx_print_stub -#define pwm_print_p1wph tx_print_stub -#define pwm_print_p1pof tx_print_stub - -#endif // __TEXT_MODE - -#endif // PWM_H_ONCE +#endif // PWM_H diff --git a/src/report.c b/src/report.c index ecd50f7..42b0f23 100644 --- a/src/report.c +++ b/src/report.c @@ -1,601 +1,92 @@ -/* - * report.c - TinyG status report and other reporting functions. - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include "tinyg.h" -#include "config.h" -#include "report.h" -#include "controller.h" -#include "json_parser.h" -#include "text_parser.h" -#include "planner.h" -#include "settings.h" -#include "util.h" -#include "usart.h" - - -srSingleton_t sr; -qrSingleton_t qr; -rxSingleton_t rx; - - -/* - * Generate an exception message - always in JSON format - * - * Returns incoming status value - * - * WARNING: Do not call this function from MED or HI interrupts (LO is OK) - * or there is a potential for deadlock in the TX buffer. - */ -stat_t rpt_exception(uint8_t status) { - if (status != STAT_OK) { // makes it possible to call exception reports w/o checking status value - if (js.json_syntax == JSON_SYNTAX_RELAXED) - printf_P(PSTR("{er:{fb:%0.2f,st:%d,msg:\"%s\"}}\n"), - TINYG_FIRMWARE_BUILD, status, get_status_message(status)); - else printf_P(PSTR("{\"er\":{\"fb\":%0.2f,\"st\":%d,\"msg\":\"%s\"}}\n"), - TINYG_FIRMWARE_BUILD, status, get_status_message(status)); - } - - return status; -} - - -/// Send a bogus exception report for testing purposes (it's not real) -stat_t rpt_er(nvObj_t *nv) { - return rpt_exception(STAT_GENERIC_EXCEPTION_REPORT); // bogus exception report for testing -} - - - -// These messages are always in JSON format to allow UIs to sync -void _startup_helper(stat_t status, const char *msg) { -#ifndef __SUPPRESS_STARTUP_MESSAGES - js.json_footer_depth = JSON_FOOTER_DEPTH; - nv_reset_nv_list(); - - nv_add_object((const char_t *)"fv"); // firmware version - nv_add_object((const char_t *)"fb"); // firmware build - nv_add_object((const char_t *)"hp"); // hardware platform - nv_add_object((const char_t *)"hv"); // hardware version - nv_add_object((const char_t *)"id"); // hardware ID - nv_add_string((const char_t *)"msg", pstr2str(msg)); // startup message - - json_print_response(status); -#endif -} - - -/// Initializing configs from hard-coded profile -void rpt_print_initializing_message() { - _startup_helper(STAT_INITIALIZING, PSTR(INIT_MESSAGE)); -} - - -/// Loading configs from EEPROM -void rpt_print_loading_configs_message() { - _startup_helper(STAT_INITIALIZING, PSTR("Loading configs from EEPROM")); -} - - -/// System ready message -void rpt_print_system_ready_message() { - _startup_helper(STAT_OK, PSTR("SYSTEM READY")); - if (cfg.comm_mode == TEXT_MODE) - text_response(STAT_OK, (char_t *)""); // prompt -} - - -/***************************************************************************** - * Status Reports - * - * Status report behaviors - * - * Configuration: - * - * Status reports are configurable only from JSON. SRs are configured - * by sending a status report SET object, e.g: - * - * {"sr":{"line":true,"posx":true,"posy":true....."motm":true,"stat":true}} - * - * Status report formats: The following formats exist for status reports: - * - * - JSON format: Returns a JSON object as above, but with the values filled in. - * In JSON form all values are returned as numeric values or enumerations. - * E.g. "posx" is returned as 124.523 and "unit" is returned as 0 for - * inches (G20) and 1 for mm (G21). - * - * - CSV format: Returns a single line of comma separated token:value pairs. - * Values are returned as numeric values or English text. - * E.g. "posx" is still returned as 124.523 but "unit" is returned as - * "inch" for inches (G20) and "mm" for mm (G21). - * - * - Multi-line format: Returns a multi-line report where each value occupies - * one line. Each line contains explanatory English text. Enumerated values are - * returned as English text as per CSV form. - * - * Status report invocation: Status reports can be invoked in the following ways: - * - * - Ad-hoc request in JSON mode. Issue {"sr":""} (or equivalent). Returns a - * JSON format report (wrapped in a response header, of course). - * - * - Automatic status reports in JSON mode. Returns JSON format reports - * according to "si" setting. - * - * - Ad-hoc request in text mode. Triggered by sending ?. Returns status - * report in multi-line format. Additionally, a line starting with ? will put - * the system into text mode. - * - * - Automatic status reports in text mode return CSV format according to si setting - */ -static stat_t _populate_unfiltered_status_report(); -static uint8_t _populate_filtered_status_report(); - - -uint8_t _is_stat(nvObj_t *nv) { - char_t tok[TOKEN_LEN + 1]; - - GET_TOKEN_STRING(nv->value, tok); - - return strcmp(tok, "stat") == 0; -} - - -/// Call this function to completely re-initialize the status report -/// Sets SR list to hard-coded defaults and re-initializes SR values in NVM -void sr_init_status_report() { - nvObj_t *nv = nv_reset_nv_list(); // used for status report persistence locations - sr.status_report_requested = false; - char_t sr_defaults[NV_STATUS_REPORT_LEN][TOKEN_LEN+1] = {STATUS_REPORT_DEFAULTS}; // see settings.h - nv->index = nv_get_index((const char_t *)"", (const char_t *)"se00"); // set first SR persistence index - sr.stat_index = 0; - - for (uint8_t i = 0; i < NV_STATUS_REPORT_LEN; i++) { - if (sr_defaults[i][0] == 0) break; // quit on first blank array entry - - sr.status_report_value[i] = -1234567; // pre-load values with an unlikely number - nv->value = nv_get_index((const char_t *)"", sr_defaults[i]); // load the index for the SR element - - if (nv->value == NO_MATCH) { - rpt_exception(STAT_BAD_STATUS_REPORT_SETTING); // trap mis-configured profile settings - return; - } - - if (_is_stat(nv)) sr.stat_index = nv->value; // identify index for 'stat' if status is in the report - - nv_set(nv); - nv_persist(nv); // conditionally persist - automatic by nv_persist() - nv->index++; // increment SR NVM index - } -} - - -/* - * Interpret an SR setup string and return current report - * - * Note: By the time this function is called any unrecognized tokens have been detected and - * rejected by the JSON or text parser. In other words, it should never get to here if - * there is an unrecognized token in the SR string. - */ -stat_t sr_set_status_report(nvObj_t *nv) { - uint8_t elements = 0; - index_t status_report_list[NV_STATUS_REPORT_LEN]; - memset(status_report_list, 0, sizeof(status_report_list)); - index_t sr_start = nv_get_index((const char_t *)"", (const char_t *)"se00"); // set first SR persistence index - - for (uint8_t i = 0; i < NV_STATUS_REPORT_LEN; i++) { - if (((nv = nv->nx) == 0) || (nv->valuetype == TYPE_EMPTY)) break; - - if ((nv->valuetype == TYPE_BOOL) && (fp_TRUE(nv->value))) { - status_report_list[i] = nv->index; - nv->value = nv->index; // persist the index as the value - nv->index = sr_start + i; // index of the SR persistence location - nv_persist(nv); - elements++; - - } else return STAT_UNRECOGNIZED_NAME; - } - - if (elements == 0) - return STAT_INVALID_OR_MALFORMED_COMMAND; - - memcpy(sr.status_report_list, status_report_list, sizeof(status_report_list)); - - return _populate_unfiltered_status_report(); // return current values -} - - -/* - * Request a status report to run after minimum interval - * - * Status reports can be request from a number of sources including: - * - direct request from command line in the form of ? or {"sr:""} - * - timed requests during machining cycle - * - filtered request after each Gcode block - * - * Status reports are generally returned with minimal delay (from the controller callback), - * but will not be provided more frequently than the status report interval - */ -stat_t sr_request_status_report(uint8_t request_type) { - if (request_type == SR_IMMEDIATE_REQUEST) - sr.status_report_systick = SysTickTimer_getValue(); - - if ((request_type == SR_TIMED_REQUEST) && (sr.status_report_requested == false)) - sr.status_report_systick = SysTickTimer_getValue() + sr.status_report_interval; - - sr.status_report_requested = true; - - return STAT_OK; -} - - -/// Main loop callback to send a report if one is ready -stat_t sr_status_report_callback() { // called by controller dispatcher -#ifdef __SUPPRESS_STATUS_REPORTS - return STAT_NOOP; -#endif - - if (sr.status_report_verbosity == SR_OFF) return STAT_NOOP; - if (sr.status_report_requested == false) return STAT_NOOP; - if (SysTickTimer_getValue() < sr.status_report_systick) return STAT_NOOP; - - sr.status_report_requested = false; // disable reports until requested again - - if (sr.status_report_verbosity == SR_VERBOSE) - _populate_unfiltered_status_report(); - else if (_populate_filtered_status_report() == false) return STAT_OK; // no new data - - nv_print_list(STAT_OK, TEXT_INLINE_PAIRS, JSON_OBJECT_FORMAT); - - return STAT_OK; -} - - -/// Generate a text mode status report in multiline format -stat_t sr_run_text_status_report() { - _populate_unfiltered_status_report(); - nv_print_list(STAT_OK, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); - return STAT_OK; -} +/******************************************************************************\ + This file is part of the TinyG firmware. -/// Populate nvObj body with status values -/// Designed to be run as a response; i.e. have a "r" header and a footer. -static stat_t _populate_unfiltered_status_report() { - const char_t sr_str[] = "sr"; - char_t tmp[TOKEN_LEN + 1]; - nvObj_t *nv = nv_reset_nv_list(); // sets *nv to the start of the body + Copyright (c) 2016, Buildbotics LLC + All rights reserved. - nv->valuetype = TYPE_PARENT; // setup the parent object (no length checking required) - strcpy(nv->token, sr_str); - nv->index = nv_get_index((const char_t *)"", sr_str);// set the index - may be needed by calling function - nv = nv->nx; // no need to check for 0 as list has just been reset + The C! library is free software: you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation, either version 2.1 of + the License, or (at your option) any later version. - for (uint8_t i = 0; i < NV_STATUS_REPORT_LEN; i++) { - if ((nv->index = sr.status_report_list[i]) == 0) break; - nv_get_nvObj(nv); + The C! library 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. - strcpy(tmp, nv->group); // flatten out groups - WARNING - you cannot use strncpy here... - strcat(tmp, nv->token); - strcpy(nv->token, tmp); //...or here. + You should have received a copy of the GNU Lesser General Public + License along with the C! library. If not, see + . - if (!(nv = nv->nx)) - return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // should never be 0 unless SR length exceeds available buffer array - } + In addition, BSD licensing may be granted on a case by case basis + by written permission from at least one of the copyright holders. + You may request written permission by emailing the authors. - return STAT_OK; -} - - -/* - * Populate nvObj body with status values - * - * Designed to be displayed as a JSON object; i;e; no footer or header - * Returns 'true' if the report has new data, 'false' if there is nothing to report. - * - * NOTE: Unlike sr_populate_unfiltered_status_report(), this function does NOT set - * the SR index, which is a relatively expensive operation. In current use this - * doesn't matter, but if the caller assumes its set it may lead to a side-effect (bug) - * - * NOTE: Room for improvement - look up the SR index initially and cache it, use the - * cached value for all remaining reports. - */ -static uint8_t _populate_filtered_status_report() { - const char_t sr_str[] = "sr"; - uint8_t has_data = false; - char_t tmp[TOKEN_LEN + 1]; - nvObj_t *nv = nv_reset_nv_list(); // sets nv to the start of the body - - nv->valuetype = TYPE_PARENT; // setup the parent object (no need to length check the copy) - strcpy(nv->token, sr_str); - nv = nv->nx; // no need to check for 0 as list has just been reset - - for (uint8_t i = 0; i < NV_STATUS_REPORT_LEN; i++) { - if ((nv->index = sr.status_report_list[i]) == 0) break; - - nv_get_nvObj(nv); - // do not report values that have not changed... - // ...except for stat=3 (STOP), which is an exception - if (fp_EQ(nv->value, sr.status_report_value[i])) { - nv->valuetype = TYPE_EMPTY; - continue; - - } else { - // report anything that has changed - strcpy(tmp, nv->group); // flatten out groups - WARNING - you cannot use strncpy here... - strcat(tmp, nv->token); - strcpy(nv->token, tmp); //...or here. - sr.status_report_value[i] = nv->value; - if (!(nv = nv->nx)) return false; // should never be 0 unless SR length exceeds available buffer array - has_data = true; - } - } - - return has_data; -} - - -/// Run status report -stat_t sr_get(nvObj_t *nv) {return _populate_unfiltered_status_report();} - - -/// Set status report elements -stat_t sr_set(nvObj_t *nv) {return sr_set_status_report(nv);} - - -/// Set status report interval -stat_t sr_set_si(nvObj_t *nv) { - if (nv->value < STATUS_REPORT_MIN_MS) { nv->value = STATUS_REPORT_MIN_MS;} - sr.status_report_interval = (uint32_t)nv->value; - return STAT_OK; -} - - -#ifdef __TEXT_MODE - -static const char fmt_si[] PROGMEM = "[si] status interval%14.0f ms\n"; -static const char fmt_sv[] PROGMEM = "[sv] status report verbosity%6d [0=off,1=filtered,2=verbose]\n"; - -void sr_print_sr(nvObj_t *nv) {_populate_unfiltered_status_report();} -void sr_print_si(nvObj_t *nv) {text_print_flt(nv, fmt_si);} -void sr_print_sv(nvObj_t *nv) {text_print_ui8(nv, fmt_sv);} - -#endif // __TEXT_MODE - - -/***************************************************************************** - * Queue Reports - * - * Queue reports can report three values: - * - qr queue depth - # of buffers availabel in planner queue - * - qi buffers added to planner queue since las report - * - qo buffers removed from planner queue since last report - * - * A QR_SINGLE report returns qr only. A QR_TRIPLE returns all 3 values - * - * There are 2 ways to get queue reports: - * - * 1. Enable single or triple queue reports using the QV variable. This will - * return a queue report every time the buffer depth changes - * - * 2. Add qr, qi and qo (or some combination) to the status report. This will - * return queue report data when status reports are generated. - */ - - -/// Initialize or clear queue report values -void qr_init_queue_report() { - qr.queue_report_requested = false; - qr.buffers_added = 0; - qr.buffers_removed = 0; - qr.init_tick = SysTickTimer_getValue(); -} - - -/* - * Request a queue report - * - * Requests a queue report and also records the buffers added and removed - * since the last init (usually re-initted when a report is generated). - */ -void qr_request_queue_report(int8_t buffers) { - // get buffer depth and added/removed count - qr.buffers_available = mp_get_planner_buffers_available(); - if (buffers > 0) qr.buffers_added += buffers; - else qr.buffers_removed -= buffers; - - // time-throttle requests while generating arcs - qr.motion_mode = cm_get_motion_mode(ACTIVE_MODEL); - if ((qr.motion_mode == MOTION_MODE_CW_ARC) || (qr.motion_mode == MOTION_MODE_CCW_ARC)) { - uint32_t tick = SysTickTimer_getValue(); - - if (tick - qr.init_tick < MIN_ARC_QR_INTERVAL) { - qr.queue_report_requested = false; - return; - } - } - - // either return or request a report - if (qr.queue_report_verbosity != QR_OFF) - qr.queue_report_requested = true; -} - - -/// generate a queue report if one has been requested -stat_t qr_queue_report_callback() { // called by controller dispatcher -#ifdef __SUPPRESS_QUEUE_REPORTS - return STAT_NOOP; -#endif + For information regarding this software email: + Joseph Coffland + joseph@buildbotics.com - if (qr.queue_report_verbosity == QR_OFF) return STAT_NOOP; - if (qr.queue_report_requested == false) return STAT_NOOP; +\******************************************************************************/ - qr.queue_report_requested = false; - - if (cfg.comm_mode == TEXT_MODE) { - if (qr.queue_report_verbosity == QR_SINGLE) - fprintf(stderr, "qr:%d\n", qr.buffers_available); - else fprintf(stderr, "qr:%d, qi:%d, qo:%d\n", qr.buffers_available,qr.buffers_added,qr.buffers_removed); - - } else if (js.json_syntax == JSON_SYNTAX_RELAXED) { - if (qr.queue_report_verbosity == QR_SINGLE) - fprintf(stderr, "{qr:%d}\n", qr.buffers_available); - else fprintf(stderr, "{qr:%d,qi:%d,qo:%d}\n", qr.buffers_available, qr.buffers_added,qr.buffers_removed); - - } else { - if (qr.queue_report_verbosity == QR_SINGLE) - fprintf(stderr, "{\"qr\":%d}\n", qr.buffers_available); - else fprintf(stderr, "{\"qr\":%d,\"qi\":%d,\"qo\":%d}\n", qr.buffers_available, qr.buffers_added,qr.buffers_removed); - } - - qr_init_queue_report(); +#include "report.h" +#include "config.h" +#include "canonical_machine.h" +#include "usart.h" +#include "rtc.h" +#include "vars.h" - return STAT_OK; -} +#include "plan/planner.h" +#include -/// Request an update on serial buffer space available -void rx_request_rx_report() { - rx.rx_report_requested = true; - rx.space_available = usart_rx_space(); -} +#include +#include -/// Send rx report if one has been requested -stat_t rx_report_callback() { - if (!rx.rx_report_requested) return STAT_NOOP; +static bool report_requested = false; +static bool report_full = false; +static uint32_t last_report = 0; - rx.rx_report_requested = false; +static float velocity; +static float positions[AXES]; - fprintf(stderr, "{\"rx\":%d}\n", rx.space_available); - return STAT_OK; -} +void report_init() { + velocity = 0; -/// Run a queue report (as data) -stat_t qr_get(nvObj_t *nv) { - nv->value = (float)mp_get_planner_buffers_available(); // ensure that manually requested QR count is always up to date - nv->valuetype = TYPE_INTEGER; - return STAT_OK; + for (int axis = 0; axis < AXES; axis++) + positions[axis] = 0; } -/// Run a queue report - buffers in -stat_t qi_get(nvObj_t *nv) { - nv->value = (float)qr.buffers_added; - nv->valuetype = TYPE_INTEGER; - qr.buffers_added = 0; // reset it - return STAT_OK; +void report_request() { + report_requested = true; } -/// Run a queue report - buffers out -stat_t qo_get(nvObj_t *nv) { - nv->value = (float)qr.buffers_removed; - nv->valuetype = TYPE_INTEGER; - qr.buffers_removed = 0; // reset it - return STAT_OK; +void report_request_full() { + report_requested = report_full = true; } -// Job ID reports -stat_t job_populate_job_report() { - const char_t job_str[] = "job"; - char_t tmp[TOKEN_LEN+1]; - nvObj_t *nv = nv_reset_nv_list(); // sets *nv to the start of the body - - nv->valuetype = TYPE_PARENT; // setup the parent object - strcpy(nv->token, job_str); - - nv = nv->nx; // no need to check for 0 as list has just been reset - - index_t job_start = nv_get_index((const char_t *)"",(const char_t *)"job1"); // set first job persistence index +stat_t report_callback() { + uint32_t now = rtc_get_time(); + if (now - last_report < 100) return STAT_OK; + last_report = now; - for (uint8_t i = 0; i < 4; i++) { - nv->index = job_start + i; - nv_get_nvObj(nv); - - strcpy(tmp, nv->group); // concatenate groups and tokens - do NOT use strncpy() - strcat(tmp, nv->token); - strcpy(nv->token, tmp); - - if ((nv = nv->nx) == 0) return STAT_OK; // should never be 0 unless SR length exceeds available buffer array - } + if (report_requested && usart_tx_empty()) vars_report(report_full); + report_requested = report_full = false; return STAT_OK; } -stat_t job_set_job_report(nvObj_t *nv) { - index_t job_start = nv_get_index((const char_t *)"",(const char_t *)"job1"); // set first job persistence index - - for (uint8_t i=0; i<4; i++) { - if (((nv = nv->nx) == 0) || (nv->valuetype == TYPE_EMPTY)) break; - - if (nv->valuetype == TYPE_INTEGER) { - cs.job_id[i] = nv->value; - nv->index = job_start + i; // index of the SR persistence location - nv_persist(nv); - - } else return STAT_UNSUPPORTED_TYPE; - } - - job_populate_job_report(); // return current values - - return STAT_OK; +float get_pos(int index) { + return cm_get_absolute_position(0, index); } -uint8_t job_report_callback() { - if (cfg.comm_mode == TEXT_MODE) ; // no-op, job_ids are client app state - - else if (js.json_syntax == JSON_SYNTAX_RELAXED) - fprintf(stderr, "{job:[%lu,%lu,%lu,%lu]}\n", - cs.job_id[0], cs.job_id[1], cs.job_id[2], cs.job_id[3]); - - else fprintf(stderr, "{\"job\":[%lu,%lu,%lu,%lu]}\n", - cs.job_id[0], cs.job_id[1], cs.job_id[2], cs.job_id[3]); - - return STAT_OK; +float get_vel() { + return mp_get_runtime_velocity(); } - -stat_t job_get(nvObj_t *nv) {return job_populate_job_report();} -stat_t job_set(nvObj_t *nv) {return job_set_job_report(nv);} -void job_print_job(nvObj_t *nv) {job_populate_job_report();} - - -#ifdef __TEXT_MODE - -static const char fmt_qr[] PROGMEM = "qr:%d\n"; -static const char fmt_qi[] PROGMEM = "qi:%d\n"; -static const char fmt_qo[] PROGMEM = "qo:%d\n"; -static const char fmt_qv[] PROGMEM = "[qv] queue report verbosity%7d [0=off,1=single,2=triple]\n"; - -void qr_print_qr(nvObj_t *nv) {text_print_int(nv, fmt_qr);} -void qr_print_qi(nvObj_t *nv) {text_print_int(nv, fmt_qi);} -void qr_print_qo(nvObj_t *nv) {text_print_int(nv, fmt_qo);} -void qr_print_qv(nvObj_t *nv) {text_print_ui8(nv, fmt_qv);} - -#endif // __TEXT_MODE diff --git a/src/report.h b/src/report.h index 1163a76..46302a4 100644 --- a/src/report.h +++ b/src/report.h @@ -1,149 +1,42 @@ -/* - * report.h - TinyG status report and other reporting functions - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2014 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ +/******************************************************************************\ -#ifndef REPORT_H_ONCE -#define REPORT_H_ONCE + This file is part of the TinyG firmware. -// Notes: -// - The NV_STATUS_REPORT_LEN define is in config.h -// - The status report defaults can be found in settings.h + Copyright (c) 2016, Buildbotics LLC + All rights reserved. -#define MIN_ARC_QR_INTERVAL 200 // minimum interval between QRs during arc generation (in system ticks) + The C! library is free software: you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation, either version 2.1 of + the License, or (at your option) any later version. + The C! library 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. -enum srVerbosity { // status report enable and verbosity - SR_OFF = 0, // no reports - SR_FILTERED, // reports only values that have changed from the last report - SR_VERBOSE // reports all values specified -}; + You should have received a copy of the GNU Lesser General Public + License along with the C! library. If not, see + . + In addition, BSD licensing may be granted on a case by case basis + by written permission from at least one of the copyright holders. + You may request written permission by emailing the authors. -enum cmStatusReportRequest { - SR_TIMED_REQUEST = 0, // request a status report at next timer interval - SR_IMMEDIATE_REQUEST // request a status report ASAP -}; + For information regarding this software email: + Joseph Coffland + joseph@buildbotics.com +\******************************************************************************/ -enum qrVerbosity { // planner queue enable and verbosity - QR_OFF = 0, // no response is provided - QR_SINGLE, // queue depth reported - QR_TRIPLE // queue depth reported for buffers, buffers added, buffered removed -}; +#ifndef REPORT_H +#define REPORT_H +#include "status.h" -typedef struct srSingleton { - // Public - uint8_t status_report_verbosity; - uint32_t status_report_interval; // in milliseconds +void report_init(); +void report_request(); +void report_request_full(); +stat_t report_callback(); - // Private - uint8_t status_report_requested; // flag that SR has been requested - uint32_t status_report_systick; // SysTick value for next status report - index_t stat_index; // table index value for stat - determined during initialization - index_t status_report_list[NV_STATUS_REPORT_LEN]; // status report elements to report - float status_report_value[NV_STATUS_REPORT_LEN]; // previous values for filtered reporting -} srSingleton_t; - - -typedef struct qrSingleton { // data for queue reports - // Public - uint8_t queue_report_verbosity; // queue reports enabled and verbosity level - - // Private - uint8_t queue_report_requested; // set to true to request a report - uint8_t buffers_available; // stored buffer depth passed to by callback - uint8_t prev_available; // buffers available at last count - uint16_t buffers_added; // buffers added since last count - uint16_t buffers_removed; // buffers removed since last report - uint8_t motion_mode; // used to detect arc movement - uint32_t init_tick; // time when values were last initialized or cleared -} qrSingleton_t; - - -typedef struct rxSingleton { - uint8_t rx_report_requested; - uint16_t space_available; // space available in rx buffer at time of request -} rxSingleton_t; - - -extern srSingleton_t sr; -extern qrSingleton_t qr; -extern rxSingleton_t rx; - - -void rpt_print_message(char *msg); -stat_t rpt_exception(uint8_t status); - -stat_t rpt_er(nvObj_t *nv); -void rpt_print_loading_configs_message(); -void rpt_print_initializing_message(); -void rpt_print_system_ready_message(); - -void sr_init_status_report(); -stat_t sr_set_status_report(nvObj_t *nv); -stat_t sr_request_status_report(uint8_t request_type); -stat_t sr_status_report_callback(); -stat_t sr_run_text_status_report(); - -stat_t sr_get(nvObj_t *nv); -stat_t sr_set(nvObj_t *nv); -stat_t sr_set_si(nvObj_t *nv); - -void qr_init_queue_report(); -void qr_request_queue_report(int8_t buffers); -stat_t qr_queue_report_callback(); - -void rx_request_rx_report(); -stat_t rx_report_callback(); - -stat_t qr_get(nvObj_t *nv); -stat_t qi_get(nvObj_t *nv); -stat_t qo_get(nvObj_t *nv); - -#ifdef __TEXT_MODE - -void sr_print_sr(nvObj_t *nv); -void sr_print_si(nvObj_t *nv); -void sr_print_sv(nvObj_t *nv); -void qr_print_qv(nvObj_t *nv); -void qr_print_qr(nvObj_t *nv); -void qr_print_qi(nvObj_t *nv); -void qr_print_qo(nvObj_t *nv); - -#else - -#define sr_print_sr tx_print_stub -#define sr_print_si tx_print_stub -#define sr_print_sv tx_print_stub -#define qr_print_qv tx_print_stub -#define qr_print_qr tx_print_stub -#define qr_print_qi tx_print_stub -#define qr_print_qo tx_print_stub - -#endif // __TEXT_MODE - -#endif // REPORT_H_ONCE +#endif // REPORT_H diff --git a/src/rtc.c b/src/rtc.c new file mode 100644 index 0000000..e061812 --- /dev/null +++ b/src/rtc.c @@ -0,0 +1,81 @@ +/* + * rtc.h - real-time counter/clock + * Part of TinyG project + * + * Copyright (c) 2010 - 2013 Alden S. Hart Jr. + * + * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "rtc.h" + +#include "switch.h" + +#include +#include + + +rtClock_t rtc; // allocate clock control struct + +/* + * Initialize and start the clock + * + * This routine follows the code in app note AVR1314. + */ +void rtc_init() { + OSC.CTRL |= OSC_RC32KEN_bm; // enable internal 32kHz. + while (!(OSC.STATUS & OSC_RC32KRDY_bm)); // 32kHz osc stabilize + while (RTC.STATUS & RTC_SYNCBUSY_bm); // wait RTC not busy + + CLK.RTCCTRL = CLK_RTCSRC_RCOSC_gc | CLK_RTCEN_bm; // 32kHz clock as RTC src + while (RTC.STATUS & RTC_SYNCBUSY_bm); // wait RTC not busy + + // the following must be in this order or it doesn't work + RTC.PER = RTC_MILLISECONDS - 1; // overflow period ~10ms + RTC.CNT = 0; + RTC.COMP = RTC_MILLISECONDS - 1; + RTC.CTRL = RTC_PRESCALER_DIV1_gc; // no prescale (1x) + RTC.INTCTRL = RTC_COMPINTLVL; // interrupt on compare + rtc.rtc_ticks = 0; // reset tick counter + rtc.sys_ticks = 0; // reset tick counter +} + + +/* + * rtc ISR + * + * It is the responsibility of callback code to ensure atomicity and volatiles + * are observed correctly as the callback will be run at the interrupt level. + * + * Here's the code in case the main loop (non-interrupt) function needs to + * create a critical region for variables set or used by the callback: + * + * #include "gpio.h" + * #include "rtc.h" + * + * RTC.INTCTRL = RTC_OVFINTLVL_OFF_gc; // disable interrupt + * blah blah blah critical region + * RTC.INTCTRL = RTC_OVFINTLVL_LO_gc; // enable interrupt + */ +ISR(RTC_COMP_vect) { + rtc.sys_ticks = ++rtc.rtc_ticks * 10; // advance both tick counters + + // callbacks to whatever you need to happen on each RTC tick go here: + switch_rtc_callback(); // switch debouncing +} + + +/// This is a hack to get around some compatibility problems +uint32_t rtc_get_time() { + return rtc.sys_ticks; +} diff --git a/src/xmega/xmega_rtc.h b/src/rtc.h similarity index 79% rename from src/xmega/xmega_rtc.h rename to src/rtc.h index c0dea79..7957caf 100644 --- a/src/xmega/xmega_rtc.h +++ b/src/rtc.h @@ -1,5 +1,5 @@ /* - * xmega_rtc.h - general purpose real-time clock + * rtc.h - general purpose real-time clock * Part of TinyG project * * Copyright (c) 2010 - 2013 Alden S. Hart Jr. @@ -17,8 +17,10 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#ifndef XMEGA_RTC_H_ONCE -#define XMEGA_RTC_H_ONCE +#ifndef RTC_H +#define RTC_H + +#include #define RTC_MILLISECONDS 10 // interrupt on every 10 RTC ticks (~10 ms) @@ -29,13 +31,13 @@ // Note: sys_ticks is in ms but is only accurate to 10 ms as it's derived from rtc_ticks typedef struct rtClock { - uint32_t rtc_ticks; // RTC tick counter, 10 uSec each - uint32_t sys_ticks; // system tick counter, 1 ms each - uint16_t magic_end; // magic number is read directly + uint32_t rtc_ticks; // RTC tick counter, 10 uSec each + uint32_t sys_ticks; // system tick counter, 1 ms each } rtClock_t; extern rtClock_t rtc; void rtc_init(); // initialize and start general timer +uint32_t rtc_get_time(); -#endif // End of include guard: XMEGA_RTC_H_ONCE +#endif // RTC_H diff --git a/src/settings.h b/src/settings.h deleted file mode 100644 index 4866a28..0000000 --- a/src/settings.h +++ /dev/null @@ -1,137 +0,0 @@ -/* - * settings.h - default runtime settings - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* The values in this file are the default settings that are loaded into a virgin EEPROM, - * and can be changed using the config commands. After initial load the EEPROM values - * (or changed values) are used. - * - * System and hardware settings that you shouldn't need to change are in hardware.h - * Application settings that also shouldn't need to be changed are in tinyg.h - */ - -#ifndef SETTINGS_H_ONCE -#define SETTINGS_H_ONCE - -/**** GENERAL SETTINGS ******************************************************/ - -// **** PLEASE NOTE **** Any of these may be overridden in machine profiles -// Do not assume these are the effective settings. Check the machine profile - -// Machine configuration settings -#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing -#define SOFT_LIMIT_ENABLE 0 // 0 = off, 1 = on -#define SWITCH_TYPE SW_TYPE_NORMALLY_OPEN // one of: SW_TYPE_NORMALLY_OPEN, SW_TYPE_NORMALLY_CLOSED - -#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // one of: MOTOR_DISABLED (0) - // MOTOR_ALWAYS_POWERED (1) - // MOTOR_POWERED_IN_CYCLE (2) - // MOTOR_POWERED_ONLY_WHEN_MOVING (3) - -#define MOTOR_IDLE_TIMEOUT 2.00 // seconds to maintain motor at full power before idling - -// Communications and reporting settings -#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE -#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE -#define NETWORK_MODE NETWORK_STANDALONE - -#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -#define JSON_SYNTAX_MODE JSON_SYNTAX_STRICT // one of JSON_SYNTAX_RELAXED, JSON_SYNTAX_STRICT -#define JSON_FOOTER_DEPTH 0 // 0 = new style, 1 = old style - -#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE= -#define STATUS_REPORT_MIN_MS 100 // milliseconds - enforces a viable minimum -#define STATUS_REPORT_INTERVAL_MS 500 // milliseconds - set $SV=0 to disable -#define STATUS_REPORT_DEFAULTS "posx","posy","posz","posa","feed","vel","unit","coor","dist","frmo","stat","mst1","mfl1" - -#define QUEUE_REPORT_VERBOSITY QR_OFF // one of: QR_OFF, QR_SINGLE, QR_TRIPLE - -// Gcode startup defaults -#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES -#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ -#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58 or G59 -#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS -#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE - -// Comm mode and echo levels -#define COM_EXPAND_CR true -#define COM_ENABLE_ECHO true -#define COM_ENABLE_FLOW_CONTROL FLOW_CONTROL_XON // FLOW_CONTROL_OFF, FLOW_CONTROL_XON, FLOW_CONTROL_RTS - -// Debug settings -#ifdef __DEBUG_SETTINGS -#undef QUEUE_REPORT_VERBOSITY -#define QUEUE_REPORT_VERBOSITY QR_SINGLE // one of: QR_OFF, QR_SINGLE, QR_TRIPLE - -#undef JSON_VERBOSITY -#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE - -#undef STATUS_REPORT_DEFAULTS -#define STATUS_REPORT_DEFAULTS "posx","posy","posz","posa","feed","vel","unit","coor","dist","frmo","stat" - -#undef STATUS_REPORT_VERBOSITY -#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE -#endif // __DEBUG_SETTINGS - -// Machine profile -#include "settings/settings_default.h" // Default settings for release - -/*** Handle optional modules that may not be in every machine ***/ - -// If PWM_1 is not defined fill it with default values -#ifndef P1_PWM_FREQUENCY -#define P1_PWM_FREQUENCY 100 // in Hz -#define P1_CW_SPEED_LO 1000 // in RPM (arbitrary units) -#define P1_CW_SPEED_HI 2000 -#define P1_CW_PHASE_LO 0.125 // phase [0..1] -#define P1_CW_PHASE_HI 0.2 -#define P1_CCW_SPEED_LO 1000 -#define P1_CCW_SPEED_HI 2000 -#define P1_CCW_PHASE_LO 0.125 -#define P1_CCW_PHASE_HI 0.2 -#define P1_PWM_PHASE_OFF 0.1 -#endif // P1_PWM_FREQUENCY - - -/*** User-Defined Data Defaults ***/ - -#define USER_DATA_A0 0 -#define USER_DATA_A1 0 -#define USER_DATA_A2 0 -#define USER_DATA_A3 0 -#define USER_DATA_B0 0 -#define USER_DATA_B1 0 -#define USER_DATA_B2 0 -#define USER_DATA_B3 0 -#define USER_DATA_C0 0 -#define USER_DATA_C1 0 -#define USER_DATA_C2 0 -#define USER_DATA_C3 0 -#define USER_DATA_D0 0 -#define USER_DATA_D1 0 -#define USER_DATA_D2 0 -#define USER_DATA_D3 0 - -#endif // End of include guard: SETTINGS_H_ONCE diff --git a/src/settings/settings_Ultimaker.h b/src/settings/settings_Ultimaker.h deleted file mode 100644 index 7604c73..0000000 --- a/src/settings/settings_Ultimaker.h +++ /dev/null @@ -1,247 +0,0 @@ -/* - * settings_Ultimaker.h - Ultimaker motion demo - * This file is part of the the TinyG project - * - * Copyright (c) 2010 - 2014 Alden S. Hart, Jr. - * Copyright (c) 2010 - 2014 Robert Giseburt - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. - * - * System and hardware settings that you shouldn't need to change - * are in hardware.h Application settings that also shouldn't need - * to be changed are in tinyg.h - */ - -/***********************************************************************/ -/**** Ultimaker profile ************************************************/ -/***********************************************************************/ - -// ***> NOTE: The init message must be a single line with no CRs or LFs -#define INIT_MESSAGE "Initializing configs to Ultimaker profile" - -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 400000 // centripetal acceleration around corners - -#ifndef PI -#define PI 3.14159628 -#endif - -// *** settings.h overrides *** - -#undef SWITCH_TYPE -#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED - -// *** motor settings *** - -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 40.64 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 1 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWERED_IN_CYCLE // 1pm standard -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 40.64 -//#define M2_MICROSTEPS 8 -#define M2_MICROSTEPS 4 // correction for v9d boards -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 3.00 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 1 -#define M3_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M4_MICROSTEPS 8 -#define M4_POLARITY 0 -#define M4_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M5_MOTOR_MAP AXIS_B -#define M5_STEP_ANGLE 1.8 -#define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M5_MICROSTEPS 8 -#define M5_POLARITY 0 -#define M5_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M6_MOTOR_MAP AXIS_C -#define M6_STEP_ANGLE 1.8 -#define M6_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M6_MICROSTEPS 8 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL - -// *** axis settings *** - -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 20000 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing -#define X_TRAVEL_MAX 212 // xtm travel between switches or crashes -#define X_JERK_MAX 40000 // xjm yes, that's "100 billion" mm/(min^3) -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_HOMING_LIMIT, SW_MODE_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_LIMIT // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_HOMING_LIMIT, SW_MODE_LIMIT -#define X_SEARCH_VELOCITY 3000 // xsv move in negative direction -#define X_LATCH_VELOCITY 200 // xlv mm/min -#define X_LATCH_BACKOFF 10 // xlb mm -#define X_ZERO_BACKOFF 3 // xzb mm -#define X_JERK_HOMING X_JERK_MAX // xjh - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 20000 -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 190 -#define Y_JERK_MAX 40000 -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_LIMIT -#define Y_SEARCH_VELOCITY 3000 -#define Y_LATCH_VELOCITY 200 -#define Y_LATCH_BACKOFF 10 -#define Y_ZERO_BACKOFF 3 -#define Y_JERK_HOMING Y_JERK_MAX - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 1000 -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN 0 -#define Z_TRAVEL_MAX 220 -#define Z_JERK_MAX 50 // 50,000,000 -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY 500 -#define Z_LATCH_VELOCITY 200 -#define Z_LATCH_BACKOFF 3 -#define Z_ZERO_BACKOFF 0.04 -#define Z_JERK_HOMING Z_JERK_MAX - -#define A_AXIS_MODE AXIS_RADIUS -#define A_RADIUS 0.609 -#define A_VELOCITY_MAX 200000.000 -#define A_FEEDRATE_MAX 50000.000 -#define A_TRAVEL_MIN 0 -#define A_TRAVEL_MAX 10 -#define A_JERK_MAX 25000 -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_SWITCH_MODE_MIN SW_MODE_DISABLED -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 2000 -#define A_LATCH_VELOCITY 2000 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_RADIUS -#define B_RADIUS 0.609 -#define B_VELOCITY_MAX 200000 -#define B_FEEDRATE_MAX 50000 -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX 25000 -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_SWITCH_MODE_MIN SW_MODE_DISABLED -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 2000 -#define B_LATCH_VELOCITY 2000 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 -#define B_JERK_HOMING B_JERK_MAX - -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX 3600 -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN 0 -#define C_TRAVEL_MAX -1 -//#define C_JERK_MAX 20000000 -#define C_JERK_MAX 20 -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS 1 -#define C_SWITCH_MODE_MIN SW_MODE_DISABLED -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 10 -#define C_ZERO_BACKOFF 2 -#define C_JERK_HOMING C_JERK_MAX - -// *** DEFAULT COORDINATE SYSTEM OFFSETS *** - -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set to middle of table -#define G55_Y_OFFSET (Y_TRAVEL_MAX/2) -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET 0 - -#define G56_X_OFFSET 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 - - diff --git a/src/settings/settings_cnc3040.h b/src/settings/settings_cnc3040.h deleted file mode 100644 index fb72234..0000000 --- a/src/settings/settings_cnc3040.h +++ /dev/null @@ -1,240 +0,0 @@ -/* - * settings_cnc3020_generic.h - generic CNC3020 Chinese engraving machine - * This file is part of the TinyG project - * - * Copyright (c) 2014 - 2015 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. - * - * System and hardware settings that you shouldn't need to change - * are in hardware.h Application settings that also shouldn't need - * to be changed are in tinyg.h - */ -/***********************************************************************/ -/**** Default profile for screw driven machines ************************/ -/***********************************************************************/ -// NOTE: Non-machine specific systems settings have been moved to settings.h -// These may be overridden using undefs - -// ***> NOTE: The init message must be a single line with no CRs or LFs -#define INIT_MESSAGE "Initializing configs to CNC3020 generic settings" - -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners - -// **** settings.h overrides **** - -#undef SWITCH_TYPE -#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED // to accommodate teh eStop switch / Amin - -// *** motor settings *** - -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 4.02 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 0 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 4.02 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 1 -#define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 4.02 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 1 -#define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M4_MICROSTEPS 8 -#define M4_POLARITY 0 -#define M4_POWER_MODE MOTOR_POWER_MODE -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M5_MOTOR_MAP AXIS_B -#define M5_STEP_ANGLE 1.8 -#define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M5_MICROSTEPS 8 -#define M5_POLARITY 0 -#define M5_POWER_MODE MOTOR_POWER_MODE -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M6_MOTOR_MAP AXIS_C -#define M6_STEP_ANGLE 1.8 -#define M6_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M6_MICROSTEPS 8 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWER_MODE -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL - -// *** axis settings *** - -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 5000 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX 2500 // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing -#define X_TRAVEL_MAX 285 // xtm maximum travel - used by soft limits and homing -#define X_JERK_MAX 100 // xjm -#define X_JERK_HOMING X_JERK_MAX // xjh -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY 500 // xsv move in negative direction -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 5 // xlb mm -#define X_ZERO_BACKOFF 1 // xzb mm - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 2000 -#define Y_FEEDRATE_MAX 2000 -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 390 -#define Y_JERK_MAX 100 -#define Y_JERK_HOMING Y_JERK_MAX -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY 500 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 5 -#define Y_ZERO_BACKOFF 1 - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 5000 -#define Z_FEEDRATE_MAX 2500 -#define Z_TRAVEL_MIN 0 -#define Z_TRAVEL_MAX 55 -#define Z_JERK_MAX 100 -#define Z_JERK_HOMING Z_JERK_MAX -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY 4007 -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 5 -#define Z_ZERO_BACKOFF 1 - -// Rotary values are chosen to make the motor react the same as X for testing -#define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit -#define A_TRAVEL_MAX -1 -#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define A_SWITCH_MODE_MIN SW_MODE_LIMIT -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_DISABLED // DISABLED -#define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define B_SWITCH_MODE_MIN SW_MODE_HOMING -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 600 -#define B_LATCH_VELOCITY 100 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 -#define B_JERK_HOMING B_JERK_MAX - -#define C_AXIS_MODE AXIS_DISABLED // DISABLED -#define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN -1 -#define C_TRAVEL_MAX -1 -#define C_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define C_SWITCH_MODE_MIN SW_MODE_HOMING -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 -#define C_JERK_HOMING C_JERK_MAX - -// *** DEFAULT COORDINATE SYSTEM OFFSETS *** - -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set to middle of table -#define G55_Y_OFFSET (Y_TRAVEL_MAX/2) -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET 0 - -#define G56_X_OFFSET 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 diff --git a/src/settings/settings_default.h b/src/settings/settings_default.h deleted file mode 100644 index 6c9b44d..0000000 --- a/src/settings/settings_default.h +++ /dev/null @@ -1,238 +0,0 @@ -/* - * settings_default.h - default machine profile - * This file is part of the TinyG project - * - * Copyright (c) 2012 - 2015 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. - * - * System and hardware settings that you shouldn't need to change - * are in hardware.h Application settings that also shouldn't need - * to be changed are in tinyg.h - */ -/***********************************************************************/ -/**** Default profile for screw driven machines ************************/ -/***********************************************************************/ -// NOTE: Non-machine specific systems settings have been moved to settings.h -// These may be overridden using undefs - -// ***> NOTE: The init message must be a single line with no CRs or LFs -#define INIT_MESSAGE "Initializing configs to default settings" - -#define JERK_MAX 20 // yes, that's "20,000,000" mm/(min^3) -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners - - -// Motor settings -#define MOTOR_MICROSTEPS 8 - -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 1.25 // 1tr -#define M1_MICROSTEPS MOTOR_MICROSTEPS // 1mi -#define M1_POLARITY 0 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 1.25 -#define M2_MICROSTEPS MOTOR_MICROSTEPS -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 1.25 -#define M3_MICROSTEPS MOTOR_MICROSTEPS -#define M3_POLARITY 0 -#define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M4_MICROSTEPS MOTOR_MICROSTEPS -#define M4_POLARITY 0 -#define M4_POWER_MODE MOTOR_POWER_MODE -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M5_MOTOR_MAP AXIS_B -#define M5_STEP_ANGLE 1.8 -#define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M5_MICROSTEPS MOTOR_MICROSTEPS -#define M5_POLARITY 0 -#define M5_POWER_MODE MOTOR_POWER_MODE -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M6_MOTOR_MAP AXIS_C -#define M6_STEP_ANGLE 1.8 -#define M6_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M6_MICROSTEPS MOTOR_MICROSTEPS -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWER_MODE -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL - -// *** axis settings *** - -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 800 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits -#define X_TRAVEL_MAX 150 // xtm travel between switches or crashes -#define X_JERK_MAX JERK_MAX // xjm -#define X_JERK_HOMING (X_JERK_MAX * 2) // xjh -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY 500 // xsv move in negative direction -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 5 // xlb mm -#define X_ZERO_BACKOFF 1 // xzb mm - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 800 -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 150 -#define Y_JERK_MAX JERK_MAX -#define Y_JERK_HOMING (Y_JERK_MAX * 2) -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY 500 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 5 -#define Y_ZERO_BACKOFF 1 - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 800 -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN 0 -#define Z_TRAVEL_MAX 75 -#define Z_JERK_MAX JERK_MAX -#define Z_JERK_HOMING (Z_JERK_MAX * 2) -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY 400 -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 5 -#define Z_ZERO_BACKOFF 1 - -// A values are chosen to make the A motor react the same as X for testing -#define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MIN -1 -#define A_TRAVEL_MAX -1 // same number means infinite -#define A_JERK_MAX (X_JERK_MAX * (360 / M1_TRAVEL_PER_REV)) -#define A_JERK_HOMING (A_JERK_MAX * 2) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 - -#define B_AXIS_MODE AXIS_DISABLED -#define B_VELOCITY_MAX 3600 -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX JERK_MAX -#define B_JERK_HOMING (B_JERK_MAX * 2) -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS 1 -#define B_SWITCH_MODE_MIN SW_MODE_HOMING -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 600 -#define B_LATCH_VELOCITY 100 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 - -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX 3600 -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN -1 -#define C_TRAVEL_MAX -1 -#define C_JERK_MAX JERK_MAX -#define C_JERK_HOMING (C_JERK_MAX * 2) -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS 1 -#define C_SWITCH_MODE_MIN SW_MODE_HOMING -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 - -// *** DEFAULT COORDINATE SYSTEM OFFSETS *** - -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET (X_TRAVEL_MAX / 2) // set to middle of table -#define G55_Y_OFFSET (Y_TRAVEL_MAX / 2) -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET 0 - -#define G56_X_OFFSET 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 diff --git a/src/settings/settings_openpnp.h b/src/settings/settings_openpnp.h deleted file mode 100644 index c312cc6..0000000 --- a/src/settings/settings_openpnp.h +++ /dev/null @@ -1,224 +0,0 @@ -/* - * settings_openpnp.h - OpenPNP settings - * Part of TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. - * - * System and hardware settings that you shouldn't need to change - * are in system.h Application settings that also shouldn't need - * to be changed are in tinyg.h - */ - -/***********************************************************************/ -/**** Shapoko 375mm profile ********************************************/ -/***********************************************************************/ - -// ***> NOTE: The init message must be a single line with no CRs or LFs -#define INIT_MESSAGE "Initializing configs to OpenPnP experimental profile" - -#define JUNCTION_DEVIATION 0.01 // default value, in mm - smaller is faster -#define JUNCTION_ACCELERATION 2000000 // 2 million - centripetal acceleration around corners - -// *** settings.h overrides *** - -#undef COMM_MODE -#define COMM_MODE TEXT_MODE - -#undef JSON_VERBOSITY -#define JSON_VERBOSITY JV_LINENUM - -#undef COM_ENABLE_XON -#define COM_ENABLE_XON true - -// *** motor settings *** - -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 25.4 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 0 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWERED_IN_CYCLE // 1pm - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 36.54 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWERED_IN_CYCLE - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 1.25 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 0 -#define M3_POWER_MODE MOTOR_POWERED_IN_CYCLE - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 180 // degrees per motor rev - 1:2 gearing -#define M4_MICROSTEPS 8 -#define M4_POLARITY 0 -#define M4_POWER_MODE MOTOR_POWERED_IN_CYCLE - -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL - -// *** axis settings *** - -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 15000 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MAX 220 // xtm travel between switches or crashes -#define X_TRAVEL_MIN 0 // xtn monimum travel for soft limits -#define X_JERK_MAX 2500 // xjm 2.5 billion mm/(min^3) -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY 3000 // xsv minus means move to minimum switch -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 20 // xlb mm -#define X_ZERO_BACKOFF 3 // xzb mm -#define X_JERK_HOMING 10000 // xjh - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 16000 -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MAX 220 -#define Y_TRAVEL_MIN 0 -#define Y_JERK_MAX 5000 // 5,000,000,000 -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY 3000 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 20 -#define Y_ZERO_BACKOFF 3 -#define Y_JERK_HOMING 10000 // xjh - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 800 -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MAX 100 -#define Z_TRAVEL_MIN 0 -#define Z_JERK_MAX 50 // 50,000,000 -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY Z_VELOCITY_MAX -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 20 -#define Z_ZERO_BACKOFF 10 -#define Z_JERK_HOMING 1000 // xjh - -#define A_AXIS_MODE AXIS_STANDARD -#define A_VELOCITY_MAX 60000 -#define A_FEEDRATE_MAX 48000 -#define A_TRAVEL_MAX 400 // degrees -#define A_TRAVEL_MIN -1 // -1 means infinite, no limit -#define A_JERK_MAX 24000 // yes, 24 billion -#define A_JUNCTION_DEVIATION 0.1 -#define A_RADIUS 1.0 -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 6000 -#define A_LATCH_VELOCITY 1000 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_DISABLED -#define B_VELOCITY_MAX 3600 -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MAX -1 -#define B_TRAVEL_MIN -1 -#define B_JERK_MAX 20 -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS 1 - -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX 3600 -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MAX -1 -#define C_TRAVEL_MIN -1 -#define C_JERK_MAX 20 -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS 1 - - -// *** DEFAULT COORDINATE SYSTEM OFFSETS *** -// Our convention is: -// - leave G54 in machine coordinates to act as a persistent absolute coordinate system -// - set G55 to be a zero in the middle of the table -// - no action for the others - -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set g55 to middle of table -#define G55_Y_OFFSET (Y_TRAVEL_MAX/2) -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET 0 - -#define G56_X_OFFSET (X_TRAVEL_MAX/2) // special settings for running braid tests -#define G56_Y_OFFSET 20 -#define G56_Z_OFFSET -10 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 - - diff --git a/src/settings/settings_othermill.h b/src/settings/settings_othermill.h deleted file mode 100644 index a6f5c39..0000000 --- a/src/settings/settings_othermill.h +++ /dev/null @@ -1,294 +0,0 @@ -/* - * settings_othermill.h - Other Machine Company Mini Milling Machine - * This file is part of the TinyG project - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. - * - * System and hardware settings that you shouldn't need to change - * are in hardware.h Application settings that also shouldn't need - * to be changed are in tinyg.h - */ - -/***********************************************************************/ -/**** Otherlab OtherMill profile ***************************************/ -/***********************************************************************/ - -// ***> NOTE: The init message must be a single line with no CRs or LFs -#define INIT_MESSAGE "Initializing configs to OMC OtherMill settings" - -#define JERK_MAX 500 // 500 million mm/(min^3) -#define JERK_HOMING 1000 // 1000 million mm/(min^3) // Jerk during homing needs to stop *fast* -#define JUNCTION_DEVIATION 0.01 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners -#define LATCH_VELOCITY 25 // reeeeally slow for accuracy - -// WARNING: Older Othermill machines use a 15deg can stack for their Z axis. -// new machines use a stepper which has the same config as the other axis. -#define HAS_CANSTACK_Z_AXIS 0 - -// *** settings.h overrides *** -// Note: there are some commented test values below - -#undef MOTOR_POWER_MODE -#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE - -#undef STATUS_REPORT_DEFAULTS -#define STATUS_REPORT_DEFAULTS "mpox","mpoy","mpoz","mpoa","ofsx","ofsy","ofsz","ofsa","unit","stat","coor","momo","dist","home","hold","macs","cycs","mots","plan","prbe" - -#undef SWITCH_TYPE -#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED - -#undef COMM_MODE -#define COMM_MODE JSON_MODE - -#undef JSON_VERBOSITY -#define JSON_VERBOSITY JV_CONFIGS // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE - -#undef JSON_FOOTER_DEPTH -#define JSON_FOOTER_DEPTH 0 // 0 = new style, 1 = old style - -#undef JSON_SYNTAX_MODE -#define JSON_SYNTAX_MODE JSON_SYNTAX_STRICT - -#undef QUEUE_REPORT_VERBOSITY -#define QUEUE_REPORT_VERBOSITY QR_SINGLE - -#undef STATUS_REPORT_VERBOSITY -#define STATUS_REPORT_VERBOSITY SR_FILTERED - -#undef COM_ENABLE_FLOW_CONTROL -#define COM_ENABLE_FLOW_CONTROL FLOW_CONTROL_XON - -#undef GCODE_DEFAULT_COORD_SYSTEM -#undef GCODE_DEFAULT_UNITS -#undef GCODE_DEFAULT_PLANE -#undef GCODE_DEFAULT_COORD_SYSTEM -#undef GCODE_DEFAULT_PATH_CONTROL -#undef GCODE_DEFAULT_DISTANCE_MODE - -#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES -#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ -#define GCODE_DEFAULT_COORD_SYSTEM G55 // G54, G55, G56, G57, G58 or G59 -#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS -#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE - -// *** motor settings *** - -#define M4_MOTOR_MAP AXIS_X // 1ma -#define M4_STEP_ANGLE 1.8 // 1sa -#define M4_TRAVEL_PER_REV 5.08 // 1tr -#define M4_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M4_POLARITY 0 // 1po 0=normal, 1=reversed -#define M4_POWER_MODE MOTOR_POWER_MODE // 1pm TRUE=low power idle enabled -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL // v9 only - -#define M3_MOTOR_MAP AXIS_Y -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 5.08 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 1 -#define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M2_MOTOR_MAP AXIS_Z -#if HAS_CANSTACK_Z_AXIS -#define M2_STEP_ANGLE 15 -#define M2_TRAVEL_PER_REV 1.27254 -#else -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 5.08 -#endif -#define M2_MICROSTEPS 8 -#define M2_POLARITY 1 -#define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M1_MOTOR_MAP AXIS_A -#define M1_STEP_ANGLE 1.8 -#define M1_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M1_MICROSTEPS 8 -#define M1_POLARITY 1 -#define M1_POWER_MODE MOTOR_POWER_MODE -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL - -// *** axis settings *** - -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 1500 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits -#define X_TRAVEL_MAX 138 // xtr travel between switches or crashes -#define X_JERK_MAX JERK_MAX // xjm -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY (X_FEEDRATE_MAX/3) // xsv -#define X_LATCH_VELOCITY LATCH_VELOCITY // xlv mm/min -#define X_LATCH_BACKOFF 5 // xlb mm -#define X_ZERO_BACKOFF 0 // xzb mm -#define X_JERK_HOMING JERK_HOMING // xjh - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX X_VELOCITY_MAX -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 115 -#define Y_JERK_MAX JERK_MAX -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY (Y_FEEDRATE_MAX/3) -#define Y_LATCH_VELOCITY LATCH_VELOCITY -#define Y_LATCH_BACKOFF 5 -#define Y_ZERO_BACKOFF 3 -#define Y_JERK_HOMING JERK_HOMING - -#define Z_AXIS_MODE AXIS_STANDARD -#if HAS_CANSTACK_Z_AXIS -#define Z_VELOCITY_MAX 1000 -#else -#define Z_VELOCITY_MAX X_VELOCITY_MAX -#endif -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN -70 -#define Z_TRAVEL_MAX 0 -#define Z_JERK_MAX JERK_MAX // 200 million -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY (Z_FEEDRATE_MAX/3) -#define Z_LATCH_VELOCITY LATCH_VELOCITY -#define Z_LATCH_BACKOFF 5 -#define Z_ZERO_BACKOFF 0 -#define Z_JERK_HOMING JERK_HOMING - -// Rotary values are chosen to make the motor react the same as X for testing -#define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit -#define A_TRAVEL_MAX -1 -#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_DISABLED // DISABLED -#define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define B_SWITCH_MODE_MIN SW_MODE_HOMING -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 600 -#define B_LATCH_VELOCITY 100 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 -#define B_JERK_HOMING B_JERK_MAX - -#define C_AXIS_MODE AXIS_DISABLED // DISABLED -#define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN -1 -#define C_TRAVEL_MAX -1 -#define C_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define C_SWITCH_MODE_MIN SW_MODE_HOMING -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 -#define C_JERK_HOMING C_JERK_MAX - -// *** PWM SPINDLE CONTROL *** - -#define P1_PWM_FREQUENCY 100 // in Hz -#define P1_CW_SPEED_LO 7900 // in RPM (arbitrary units) -#define P1_CW_SPEED_HI 12800 -#define P1_CW_PHASE_LO 0.13 // phase [0..1] -#define P1_CW_PHASE_HI 0.17 -#define P1_CCW_SPEED_LO 0 -#define P1_CCW_SPEED_HI 0 -#define P1_CCW_PHASE_LO 0.1 -#define P1_CCW_PHASE_HI 0.1 -#define P1_PWM_PHASE_OFF 0.1 - -// *** DEFAULT COORDINATE SYSTEM OFFSETS *** - -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET 0 // but the again, so is everyting else (at least for start) -#define G55_Y_OFFSET 0 -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET -20 // this is where we currently store the tool offset - -#define G56_X_OFFSET 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 diff --git a/src/settings/settings_probotixV90.h b/src/settings/settings_probotixV90.h deleted file mode 100644 index 7a415c6..0000000 --- a/src/settings/settings_probotixV90.h +++ /dev/null @@ -1,237 +0,0 @@ -/* - * settings_probotix.h - Probotix Fireball V90 machine profile - * This file is part the TinyG project - * - * Copyright (c) 2011 - 2014 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. - * - * System and hardware settings that you shouldn't need to change - * are in hardware.h Application settings that also shouldn't need - * to be changed are in tinyg.h - */ - -/***********************************************************************/ -/**** Probotix Fireball V90 profile ************************************/ -/***********************************************************************/ - -// ***> NOTE: The init message must be a single line with no CRs or LFs -#define INIT_MESSAGE "Initializing configs to Probotix Fireball V90 profile" - -#define JERK_MAX 100 // yes, that's "100,000,000" mm/(min^3) -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 200000 // centripetal acceleration around corners - -// **** settings.h overrides **** - -// *** motor settings *** - -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 5.08 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 0 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1pl - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 5.08 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 2.1166666 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 0 -#define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M4_MICROSTEPS 8 -#define M4_POLARITY 0 -#define M4_POWER_MODE MOTOR_POWER_MODE -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M5_MOTOR_MAP AXIS_B -#define M5_STEP_ANGLE 1.8 -#define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M5_MICROSTEPS 8 -#define M5_POLARITY 0 -#define M5_POWER_MODE MOTOR_POWER_MODE -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M6_MOTOR_MAP AXIS_C -#define M6_STEP_ANGLE 1.8 -#define M6_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M6_MICROSTEPS 8 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWER_MODE -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL - -// *** axis settings *** - -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 2400 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing -#define X_TRAVEL_MAX 400 // xtm maximum travel - used by soft limits and homing -#define X_JERK_MAX JERK_MAX // xjm -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY 1000 // xsv move in negative direction -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 10 // xlb mm -#define X_ZERO_BACKOFF 2 // xzb mm -#define X_JERK_HOMING X_JERK_MAX // xjh - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 2400 -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 175 -#define Y_JERK_MAX JERK_MAX -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_LIMIT -#define Y_SEARCH_VELOCITY 1000 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 10 -#define Y_ZERO_BACKOFF 2 -#define Y_JERK_HOMING Y_JERK_MAX - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 1200 -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN 0 -#define Z_TRAVEL_MAX 75 -#define Z_JERK_MAX JERK_MAX -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY 600 -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 10 -#define Z_ZERO_BACKOFF 2 -#define Z_JERK_HOMING Z_JERK_MAX - -// Rotary values are chosen to make the motor react the same as X for testing -#define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit -#define A_TRAVEL_MAX -1 -#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_RADIUS -#define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define B_SWITCH_MODE_MIN SW_MODE_HOMING -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 600 -#define B_LATCH_VELOCITY 100 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 -#define B_JERK_HOMING B_JERK_MAX - -#define C_AXIS_MODE AXIS_RADIUS -#define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN -1 -#define C_TRAVEL_MAX -1 -#define C_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define C_SWITCH_MODE_MIN SW_MODE_HOMING -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 -#define C_JERK_HOMING C_JERK_MAX - -// *** DEFAULT COORDINATE SYSTEM OFFSETS *** - -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET (X_TRAVEL_MAX/2) -#define G55_Y_OFFSET (Y_TRAVEL_MAX/2) -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET 0 - -#define G56_X_OFFSET 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 diff --git a/src/settings/settings_shapeoko2.h b/src/settings/settings_shapeoko2.h deleted file mode 100644 index a3c6c7d..0000000 --- a/src/settings/settings_shapeoko2.h +++ /dev/null @@ -1,283 +0,0 @@ -/* - * settings_shapeoko375.h - Shapeoko2 500mm table - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2014 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. - * - * System and hardware settings that you shouldn't need to change - * are in system.h Application settings that also shouldn't need - * to be changed are in tinyg.h - */ - -/***********************************************************************/ -/**** Shapeoko2 500mm profile ********************************************/ -/***********************************************************************/ - -// ***> NOTE: The init message must be a single line with no CRs or LFs -#define INIT_MESSAGE "Initializing configs to Shapeoko2 500mm profile" - -#define JUNCTION_DEVIATION 0.01 // default value, in mm - smaller is faster -#define JUNCTION_ACCELERATION 2000000 // 2 million - centripetal acceleration around corners - -// *** settings.h overrides *** - -#undef COMM_MODE -#define COMM_MODE JSON_MODE - -#undef JSON_VERBOSITY -#define JSON_VERBOSITY JV_VERBOSE - -#undef SWITCH_TYPE -#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED // one of: SW_TYPE_NORMALLY_OPEN, SW_TYPE_NORMALLY_CLOSED - -// *** motor settings *** - -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 40.00 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 1 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE 2 // 1pm TRUE=low power idle enabled - -#define M2_MOTOR_MAP AXIS_Y // Y1 - left side of machine -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 40.00 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 1 -#define M2_POWER_MODE 2 - -#define M3_MOTOR_MAP AXIS_Y // Y2 - right sif of machine -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 40.00 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 0 -#define M3_POWER_MODE 2 - -#define M4_MOTOR_MAP AXIS_Z -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 2.1166 -#define M4_MICROSTEPS 8 -#define M4_POLARITY 1 -#define M4_POWER_MODE 2 - -#define M5_MOTOR_MAP AXIS_DISABLED -#define M5_STEP_ANGLE 1.8 -#define M5_TRAVEL_PER_REV 360 // degrees per motor rev -#define M5_MICROSTEPS 8 -#define M5_POLARITY 0 -#define M5_POWER_MODE MOTOR_POWER_MODE - -#define M6_MOTOR_MAP AXIS_DISABLED -#define M6_STEP_ANGLE 1.8 -#define M6_TRAVEL_PER_REV 360 -#define M6_MICROSTEPS 8 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWER_MODE - -// *** axis settings *** - -// These are relative conservative values for a well-tuned Shapeoko2 or similar XY belt / Z screw machine - -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 16000 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel -#define X_TRAVEL_MAX 290 // xtm maximum travel (travel between switches or crashes) -#define X_JERK_MAX 5000 // xjm yes, that's "5 billion" mm/(min^3) -#define X_JERK_HOMING 10000 // xjh -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY 3000 // xsv minus means move to minimum switch -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 10 // xlb mm -#define X_ZERO_BACKOFF 2 // xzb mm - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 16000 -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 320 -#define Y_JERK_MAX 5000 -#define Y_JERK_HOMING 10000 // xjh -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY 3000 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 10 -#define Y_ZERO_BACKOFF 2 - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 1000 -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MAX 0 -#define Z_TRAVEL_MIN -120 // this is approximate as Z depth depends on tooling - // value must be large enough to guarantee return to Zmax during homing -#define Z_JERK_MAX 50 // 50,000,000 -#define Z_JERK_HOMING 1000 -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY Z_VELOCITY_MAX -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 10 -#define Z_ZERO_BACKOFF 3 - -/*************************************************************************************** - * A Axis rotary values are chosen to make the motor react the same as X for testing - * - * To calculate the speeds here, in Wolfram Alpha-speak: - * - * c=2*pi*r, r=0.609, d=c/360, s=((S*60)/d), S=40 for s - * c=2*pi*r, r=5.30516, d=c/360, s=((S*60)/d), S=40 for s - * - * Change r to A_RADIUS, and S to the desired speed, in mm/s or mm/s/s/s. - * - * It will return s= as the value you want to enter. - * - * If the value is over 1 million, the code will divide it by 1 million, - * so you have to pre-multiply it by 1000000.0. (The value is in millions, btw.) - * - * Note that you need these to be floating point values, so always have a .0 at the end! - * - ***************************************************************************************/ - -#define A_AXIS_MODE AXIS_RADIUS -#define A_RADIUS 5.30516 // -#define A_VELOCITY_MAX 25920.0 // ~40 mm/s, 2,400 mm/min -#define A_FEEDRATE_MAX 25920.0/2.0 // ~20 mm/s, 1,200 mm/min -#define A_TRAVEL_MIN -1 // identical mean no homing will occur -#define A_TRAVEL_MAX -1 -#define A_JERK_MAX 324000 // 1,000 million mm/min^3 - // * a million IF it's over a million - // c=2*pi*r, r=5.30516476972984, d=c/360, s=((1000*60)/d) -#define A_JERK_HOMING A_JERK_MAX -#define A_JUNCTION_DEVIATION 0.1 -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 2000 -#define A_LATCH_VELOCITY 2000 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 - -/* -#define A_AXIS_MODE AXIS_STANDARD -#define A_VELOCITY_MAX 60000 -#define A_FEEDRATE_MAX 48000 -#define A_JERK_MAX 24000 // yes, 24 billion -#define A_JERK_HOMING A_JERK_MAX -#define A_RADIUS 1.0 -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 6000 -#define A_LATCH_VELOCITY 1000 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -*/ - -#define B_AXIS_MODE AXIS_DISABLED -#define B_VELOCITY_MAX 3600 -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MAX -1 -#define B_TRAVEL_MIN -1 -#define B_JERK_MAX 20 -#define B_JERK_HOMING B_JERK_MAX -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS 1 -#define B_SWITCH_MODE_MIN SW_MODE_HOMING -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 6000 -#define B_LATCH_VELOCITY 1000 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 - -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX 3600 -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MAX -1 -#define C_TRAVEL_MIN -1 -#define C_JERK_MAX 20 -#define C_JERK_HOMING C_JERK_MAX -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS 1 -#define C_SWITCH_MODE_MIN SW_MODE_HOMING -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 6000 -#define C_LATCH_VELOCITY 1000 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 - -// *** DEFAULT COORDINATE SYSTEM OFFSETS *** -// Our convention is: -// - leave G54 in machine coordinates to act as a persistent absolute coordinate system -// - set G55 to be a zero in the middle of the table -// - no action for the others - -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set g55 to middle of table -#define G55_Y_OFFSET (Y_TRAVEL_MAX/2) -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET 0 - -#define G56_X_OFFSET 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 - - diff --git a/src/settings/settings_test.h b/src/settings/settings_test.h deleted file mode 100644 index 749b3b9..0000000 --- a/src/settings/settings_test.h +++ /dev/null @@ -1,309 +0,0 @@ -/* - * settings_test.h - settings for testing - subject to wild change - * This file is part of the TinyG project - * - * Copyright (c) 2014 Alden S. Hart Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. - * - * System and hardware settings that you shouldn't need to change - * are in hardware.h Application settings that also shouldn't need - * to be changed are in tinyg.h - */ - -/***********************************************************************/ -/**** Otherlab OtherMill profile ***************************************/ -/***********************************************************************/ - -// ***> NOTE: The init message must be a single line with no CRs or LFs -#define INIT_MESSAGE "Initializing configs to TEST settings" - -#define JERK_MAX 500 // 500 million mm/(min^3) -#define JERK_HOMING 1000 // 1000 million mm/(min^3) // Jerk during homing needs to stop *fast* -#define JUNCTION_DEVIATION 0.01 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners -#define LATCH_VELOCITY 25 // reeeeally slow for accuracy - -// WARNING: Older Othermill machines use a 15deg can stack for their Z axis. -// new machines use a stepper which has the same config as the other axis. -//#define HAS_CANSTACK_Z_AXIS 0 -#define HAS_CANSTACK_Z_AXIS 1 // Earlier machines - -// *** settings.h overrides *** -// Note: there are some commented test values below - - -#undef JSON_VERBOSITY -//#define JSON_VERBOSITY JV_SILENT // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -//#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -#define JSON_VERBOSITY JV_VERBOSE // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE - - -/* -#undef JSON_SYNTAX_MODE -#define JSON_SYNTAX_MODE JSON_SYNTAX_RELAXED // one of JSON_SYNTAX_RELAXED, JSON_SYNTAX_STRICT -#define JSON_SYNTAX_MODE JSON_SYNTAX_STRICT // one of JSON_SYNTAX_RELAXED, JSON_SYNTAX_STRICT -*/ - - -#undef STATUS_REPORT_VERBOSITY -//#define STATUS_REPORT_VERBOSITY SR_OFF // one of: SR_OFF, SR_FILTERED, SR_VERBOSE -//#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE -#define STATUS_REPORT_VERBOSITY SR_VERBOSE // one of: SR_OFF, SR_FILTERED, SR_VERBOSE - - -#undef STATUS_REPORT_DEFAULTS -//#define STATUS_REPORT_DEFAULTS "mpox","mpoy","mpoz","mpoa","ofsx","ofsy","ofsz","ofsa","unit","stat","coor","momo","dist","home","hold","macs","cycs","mots","plan","feed" -//#define STATUS_REPORT_DEFAULTS "line","mpox","mpoy","mpoz","mpoa","ofsx","ofsy","ofsz","ofsa","stat","_cs1","_es1","_fe0","_fe1","_fe2","_fe3" -//#define STATUS_REPORT_DEFAULTS "line","mpox","mpoy","mpoz","stat","_ts2","_ps2","_cs2","_es2","_fe2" -#define STATUS_REPORT_DEFAULTS "line","mpox","mpoy","mpoz","_cs3","_es3","_fe3","_xs3","_cs2","_es2","_fe2","_xs2","stat" -//#define STATUS_REPORT_DEFAULTS "line","posx","posy","posz","posa","feed","vel","unit","coor","dist","frmo","momo","stat","_cs1","_es1","_xs1","_fe1" - -#undef SWITCH_TYPE -#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED - -#undef COMM_MODE -#define COMM_MODE JSON_MODE - -#undef JSON_VERBOSITY -//#define JSON_VERBOSITY JV_CONFIGS // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -#define JSON_VERBOSITY JV_VERBOSE // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE - -#undef JSON_FOOTER_DEPTH -#define JSON_FOOTER_DEPTH 0 // 0 = new style, 1 = old style - -#undef JSON_SYNTAX_MODE -#define JSON_SYNTAX_MODE JSON_SYNTAX_STRICT - -//#undef QUEUE_REPORT_VERBOSITY -//#define QUEUE_REPORT_VERBOSITY QR_SINGLE - -#undef STATUS_REPORT_VERBOSITY -//#define STATUS_REPORT_VERBOSITY SR_FILTERED -#define STATUS_REPORT_VERBOSITY SR_VERBOSE - -#undef COM_ENABLE_FLOW_CONTROL -#define COM_ENABLE_FLOW_CONTROL FLOW_CONTROL_XON - -#undef GCODE_DEFAULT_COORD_SYSTEM -#undef GCODE_DEFAULT_UNITS -#undef GCODE_DEFAULT_PLANE -#undef GCODE_DEFAULT_COORD_SYSTEM -#undef GCODE_DEFAULT_PATH_CONTROL -#undef GCODE_DEFAULT_DISTANCE_MODE - -#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES -#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ -#define GCODE_DEFAULT_COORD_SYSTEM G55 // G54, G55, G56, G57, G58 or G59 -#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS -#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE - -// *** motor settings *** - -#define M4_MOTOR_MAP AXIS_X // 1ma -#define M4_STEP_ANGLE 1.8 // 1sa -#define M4_TRAVEL_PER_REV 5.08 // 1tr -#define M4_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M4_POLARITY 0 // 1po 0=normal, 1=reversed -#define M4_POWER_MODE MOTOR_ALWAYS_POWERED// 1pm TRUE=low power idle enabled -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Y -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 5.08 // 1tr -#define M3_MICROSTEPS 8 -#define M3_POLARITY 1 -#define M3_POWER_MODE MOTOR_ALWAYS_POWERED -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M2_MOTOR_MAP AXIS_Z -#if HAS_CANSTACK_Z_AXIS -#define M2_STEP_ANGLE 15 -#define M2_TRAVEL_PER_REV 1.27254 -#else -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 5.08 -#endif -#define M2_MICROSTEPS 8 -#define M2_POLARITY 1 -#define M2_POWER_MODE MOTOR_ALWAYS_POWERED -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M1_MOTOR_MAP AXIS_A -#define M1_STEP_ANGLE 1.8 -#define M1_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M1_MICROSTEPS 8 -#define M1_POLARITY 1 -#define M1_POWER_MODE MOTOR_ALWAYS_POWERED -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL - -// *** axis settings *** - -//#define X_AXIS_MODE AXIS_DISABLED // DIAGNOSTIC TEST ONLY!!! -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 1500 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits -#define X_TRAVEL_MAX 138 // xtr travel between switches or crashes -#define X_JERK_MAX JERK_MAX // xjm -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY (X_FEEDRATE_MAX/3) // xsv -#define X_LATCH_VELOCITY LATCH_VELOCITY // xlv mm/min -#define X_LATCH_BACKOFF 5 // xlb mm -#define X_ZERO_BACKOFF 0 // xzb mm -#define X_JERK_HOMING JERK_HOMING // xjh - -//#define Y_AXIS_MODE AXIS_DISABLED // DIAGNOSTIC TEST ONLY!!! -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX X_VELOCITY_MAX -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 115 -#define Y_JERK_MAX JERK_MAX -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY (Y_FEEDRATE_MAX/3) -#define Y_LATCH_VELOCITY LATCH_VELOCITY -#define Y_LATCH_BACKOFF 5 -#define Y_ZERO_BACKOFF 0 -#define Y_JERK_HOMING JERK_HOMING - -#define Z_AXIS_MODE AXIS_STANDARD -#if HAS_CANSTACK_Z_AXIS -#define Z_VELOCITY_MAX 1000 -#else -#define Z_VELOCITY_MAX X_VELOCITY_MAX -#endif -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN -75 -#define Z_TRAVEL_MAX 0 -#define Z_JERK_MAX JERK_MAX // 200 million -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY (Z_FEEDRATE_MAX/3) -#define Z_LATCH_VELOCITY LATCH_VELOCITY -#define Z_LATCH_BACKOFF 5 -#define Z_ZERO_BACKOFF 0 -#define Z_JERK_HOMING JERK_HOMING - -// A values are chosen to make the A motor react the same as X for testing -#define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MAX 0 // max=0 min=0 means infinite, no limit -#define A_TRAVEL_MIN 0 -#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_DISABLED -#define B_VELOCITY_MAX 3600 -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MAX 0 -#define B_TRAVEL_MIN 0 -#define B_JERK_MAX JERK_MAX -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS 1 - -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX 3600 -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MAX 0 -#define C_TRAVEL_MIN 0 -#define C_JERK_MAX JERK_MAX -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS 1 - - -// *** PWM SPINDLE CONTROL *** - -#define P1_PWM_FREQUENCY 100 // in Hz -#define P1_CW_SPEED_LO 1000 // in RPM (arbitrary units) -#define P1_CW_SPEED_HI 2000 -#define P1_CW_PHASE_LO 0.125 // phase [0..1] -#define P1_CW_PHASE_HI 0.2 -#define P1_CCW_SPEED_LO 1000 -#define P1_CCW_SPEED_HI 2000 -#define P1_CCW_PHASE_LO 0.125 -#define P1_CCW_PHASE_HI 0.2 -#define P1_PWM_PHASE_OFF 0.1 - -// *** DEFAULT COORDINATE SYSTEM OFFSETS *** - -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET 0 // but the again, so is everyting else (at least for start) -#define G55_Y_OFFSET 0 -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET -20 // this is where we currently store the tool offset - -#define G56_X_OFFSET 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 diff --git a/src/settings/settings_zen7x12.h b/src/settings/settings_zen7x12.h deleted file mode 100644 index 95bbd79..0000000 --- a/src/settings/settings_zen7x12.h +++ /dev/null @@ -1,245 +0,0 @@ -/* - * settings_zen7x12.h - Zen Toolworks 7x12 machine profile - * This file is part of the TinyG project - * - * Copyright (c) 2011 - 2014 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. - */ - -/***********************************************************************/ -/**** Zen Toolworks 7x12 profile ***************************************/ -/***********************************************************************/ - -// ***> NOTE: The init message must be a single line with no CRs or LFs -#define INIT_MESSAGE "Initializing configs to Zen Toolworks 7x12 profile" - -#define JERK_MAX_LINEAR 500 // 500,000,000 mm/(min^3) -#define JERK_MAX_ROTARY 10000 // 10 billion mm/(min^3) -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners - -// *** settings.h overrides *** - -#undef COMM_MODE -#define COMM_MODE JSON_MODE - -#undef JSON_VERBOSITY -#define JSON_VERBOSITY JV_VERBOSE - -#undef SWITCH_TYPE -#define SWITCH_TYPE SW_TYPE_NORMALLY_OPEN - -// *** motor settings *** - -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 1.25 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 1 // REVERSE// 1po 0=normal, 1=reverse -#define M1_POWER_MODE MOTOR_POWERED_IN_CYCLE // 1pm standard -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 1.25 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 1.25 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 1 // REVERSE -#define M3_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M4_MICROSTEPS 8 -#define M4_POLARITY 0 -#define M4_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M5_MOTOR_MAP AXIS_B -#define M5_STEP_ANGLE 1.8 -#define M5_TRAVEL_PER_REV 360 -#define M5_MICROSTEPS 8 -#define M5_POLARITY 0 -#define M5_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M6_MOTOR_MAP AXIS_C -#define M6_STEP_ANGLE 1.8 -#define M6_TRAVEL_PER_REV 360 -#define M6_MICROSTEPS 8 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL - -// *** axis settings *** - -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 600 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing -#define X_TRAVEL_MAX 475 // xtm maximum travel - used by soft limits and homing -#define X_JERK_MAX JERK_MAX_LINEAR // xjm -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_LIMIT // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -//#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY 500 // xsv move in negative direction -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 2 // xlb mm -#define X_ZERO_BACKOFF 1 // xzb mm -#define X_JERK_HOMING X_JERK_MAX // xjh - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 600 -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 200 -#define Y_JERK_MAX JERK_MAX_LINEAR -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_LIMIT -//#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY 500 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 2 -#define Y_ZERO_BACKOFF 1 -#define Y_JERK_HOMING Y_JERK_MAX - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 500 -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN 0 -#define Z_TRAVEL_MAX 75 -#define Z_JERK_MAX JERK_MAX_LINEAR -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY 400 -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 2 -#define Z_ZERO_BACKOFF 1 -#define Z_JERK_HOMING Z_JERK_MAX - -// Rotary values are chosen to make the motor react the same as X for testing -#define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit -#define A_TRAVEL_MAX -1 -#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_RADIUS -#define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define B_SWITCH_MODE_MIN SW_MODE_HOMING -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 600 -#define B_LATCH_VELOCITY 100 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 -#define B_JERK_HOMING B_JERK_MAX - -#define C_AXIS_MODE AXIS_RADIUS -#define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN -1 -#define C_TRAVEL_MAX -1 -#define C_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define C_SWITCH_MODE_MIN SW_MODE_HOMING -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 -#define C_JERK_HOMING C_JERK_MAX - -// *** DEFAULT COORDINATE SYSTEM OFFSETS *** - -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set to middle of table -#define G55_Y_OFFSET (Y_TRAVEL_MAX/2) -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET 0 - -#define G56_X_OFFSET 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 diff --git a/src/spindle.c b/src/spindle.c index f687802..8f1f203 100644 --- a/src/spindle.c +++ b/src/spindle.c @@ -25,14 +25,16 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" // #1 -#include "config.h" // #2 #include "spindle.h" + +#include "config.h" #include "gpio.h" -#include "planner.h" #include "hardware.h" #include "pwm.h" +#include "plan/planner.h" + + static void _exec_spindle_control(float *value, float *flag); static void _exec_spindle_speed(float *value, float *flag); @@ -41,11 +43,11 @@ static void _exec_spindle_speed(float *value, float *flag); */ void cm_spindle_init() { - if( pwm.c[PWM_1].frequency < 0 ) - pwm.c[PWM_1].frequency = 0; + if( pwm.c[PWM_1].frequency < 0 ) + pwm.c[PWM_1].frequency = 0; - pwm_set_freq(PWM_1, pwm.c[PWM_1].frequency); - pwm_set_duty(PWM_1, pwm.c[PWM_1].phase_off); + pwm_set_freq(PWM_1, pwm.c[PWM_1].frequency); + pwm_set_duty(PWM_1, pwm.c[PWM_1].phase_off); } /* @@ -53,30 +55,30 @@ void cm_spindle_init() */ float cm_get_spindle_pwm( uint8_t spindle_mode ) { - float speed_lo=0, speed_hi=0, phase_lo=0, phase_hi=0; - if (spindle_mode == SPINDLE_CW ) { - speed_lo = pwm.c[PWM_1].cw_speed_lo; - speed_hi = pwm.c[PWM_1].cw_speed_hi; - phase_lo = pwm.c[PWM_1].cw_phase_lo; - phase_hi = pwm.c[PWM_1].cw_phase_hi; - } else if (spindle_mode == SPINDLE_CCW ) { - speed_lo = pwm.c[PWM_1].ccw_speed_lo; - speed_hi = pwm.c[PWM_1].ccw_speed_hi; - phase_lo = pwm.c[PWM_1].ccw_phase_lo; - phase_hi = pwm.c[PWM_1].ccw_phase_hi; - } - - if (spindle_mode==SPINDLE_CW || spindle_mode==SPINDLE_CCW ) { - // clamp spindle speed to lo/hi range - if( cm.gm.spindle_speed < speed_lo ) cm.gm.spindle_speed = speed_lo; - if( cm.gm.spindle_speed > speed_hi ) cm.gm.spindle_speed = speed_hi; - - // normalize speed to [0..1] - float speed = (cm.gm.spindle_speed - speed_lo) / (speed_hi - speed_lo); - return speed * (phase_hi - phase_lo) + phase_lo; - } else { - return pwm.c[PWM_1].phase_off; - } + float speed_lo=0, speed_hi=0, phase_lo=0, phase_hi=0; + if (spindle_mode == SPINDLE_CW ) { + speed_lo = pwm.c[PWM_1].cw_speed_lo; + speed_hi = pwm.c[PWM_1].cw_speed_hi; + phase_lo = pwm.c[PWM_1].cw_phase_lo; + phase_hi = pwm.c[PWM_1].cw_phase_hi; + } else if (spindle_mode == SPINDLE_CCW ) { + speed_lo = pwm.c[PWM_1].ccw_speed_lo; + speed_hi = pwm.c[PWM_1].ccw_speed_hi; + phase_lo = pwm.c[PWM_1].ccw_phase_lo; + phase_hi = pwm.c[PWM_1].ccw_phase_hi; + } + + if (spindle_mode==SPINDLE_CW || spindle_mode==SPINDLE_CCW ) { + // clamp spindle speed to lo/hi range + if( cm.gm.spindle_speed < speed_lo ) cm.gm.spindle_speed = speed_lo; + if( cm.gm.spindle_speed > speed_hi ) cm.gm.spindle_speed = speed_hi; + + // normalize speed to [0..1] + float speed = (cm.gm.spindle_speed - speed_lo) / (speed_hi - speed_lo); + return speed * (phase_hi - phase_lo) + phase_lo; + } else { + return pwm.c[PWM_1].phase_off; + } } /* @@ -86,29 +88,29 @@ float cm_get_spindle_pwm( uint8_t spindle_mode ) stat_t cm_spindle_control(uint8_t spindle_mode) { - float value[AXES] = { (float)spindle_mode, 0,0,0,0,0 }; - mp_queue_command(_exec_spindle_control, value, value); - return STAT_OK; + float value[AXES] = { (float)spindle_mode, 0,0,0,0,0 }; + mp_queue_command(_exec_spindle_control, value, value); + return STAT_OK; } //static void _exec_spindle_control(uint8_t spindle_mode, float f, float *vector, float *flag) static void _exec_spindle_control(float *value, float *flag) { - uint8_t spindle_mode = (uint8_t)value[0]; - cm_set_spindle_mode(MODEL, spindle_mode); - - if (spindle_mode == SPINDLE_CW) { - gpio_set_bit_on(SPINDLE_BIT); - gpio_set_bit_off(SPINDLE_DIR); - } else if (spindle_mode == SPINDLE_CCW) { - gpio_set_bit_on(SPINDLE_BIT); - gpio_set_bit_on(SPINDLE_DIR); - } else { - gpio_set_bit_off(SPINDLE_BIT); // failsafe: any error causes stop - } - - // PWM spindle control - pwm_set_duty(PWM_1, cm_get_spindle_pwm(spindle_mode) ); + uint8_t spindle_mode = (uint8_t)value[0]; + cm_set_spindle_mode(MODEL, spindle_mode); + + if (spindle_mode == SPINDLE_CW) { + gpio_set_bit_on(SPINDLE_BIT); + gpio_set_bit_off(SPINDLE_DIR); + } else if (spindle_mode == SPINDLE_CCW) { + gpio_set_bit_on(SPINDLE_BIT); + gpio_set_bit_on(SPINDLE_DIR); + } else { + gpio_set_bit_off(SPINDLE_BIT); // failsafe: any error causes stop + } + + // PWM spindle control + pwm_set_duty(PWM_1, cm_get_spindle_pwm(spindle_mode) ); } /* @@ -118,21 +120,18 @@ static void _exec_spindle_control(float *value, float *flag) */ stat_t cm_set_spindle_speed(float speed) { -// if (speed > cfg.max_spindle speed) -// return STAT_MAX_SPINDLE_SPEED_EXCEEDED; - - float value[AXES] = { speed, 0,0,0,0,0 }; - mp_queue_command(_exec_spindle_speed, value, value); - return STAT_OK; + float value[AXES] = { speed, 0,0,0,0,0 }; + mp_queue_command(_exec_spindle_speed, value, value); + return STAT_OK; } void cm_exec_spindle_speed(float speed) { - cm_set_spindle_speed(speed); + cm_set_spindle_speed(speed); } static void _exec_spindle_speed(float *value, float *flag) { - cm_set_spindle_speed_parameter(MODEL, value[0]); - pwm_set_duty(PWM_1, cm_get_spindle_pwm(cm.gm.spindle_mode) ); // update spindle speed if we're running + cm_set_spindle_speed_parameter(MODEL, value[0]); + pwm_set_duty(PWM_1, cm_get_spindle_pwm(cm.gm.spindle_mode) ); // update spindle speed if we're running } diff --git a/src/spindle.h b/src/spindle.h index 431a73d..19b6283 100644 --- a/src/spindle.h +++ b/src/spindle.h @@ -25,15 +25,17 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#ifndef SPINDLE_H_ONCE -#define SPINDLE_H_ONCE +#ifndef SPINDLE_H +#define SPINDLE_H + +#include "status.h" void cm_spindle_init(); -stat_t cm_set_spindle_speed(float speed); // S parameter +stat_t cm_set_spindle_speed(float speed); // S parameter void cm_exec_spindle_speed(float speed); // callback for above stat_t cm_spindle_control(uint8_t spindle_mode); // M3, M4, M5 integrated spindle control -void cm_exec_spindle_control(uint8_t spindle_mode); // callback for above +void cm_exec_spindle_control(uint8_t spindle_mode); // callback for above -#endif // End of include guard: SPINDLE_H_ONCE +#endif // SPINDLE_H diff --git a/src/status.c b/src/status.c index 985032e..ad71a1a 100644 --- a/src/status.c +++ b/src/status.c @@ -18,324 +18,25 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" +#include "status.h" -#include +#include +#include -/**** Status Messages *************************************************************** - * See tinyg.h for status codes. These strings must align with the status codes in tinyg.h - * The number of elements in the indexing array must match the # of strings - */ - -stat_t status_code; // allocate a variable for the ritorno macro -char global_string_buf[MESSAGE_LEN]; // allocate a string for global message use - - -static const char stat_00[] PROGMEM = "OK"; -static const char stat_01[] PROGMEM = "Error"; -static const char stat_02[] PROGMEM = "Eagain"; -static const char stat_03[] PROGMEM = "Noop"; -static const char stat_04[] PROGMEM = "Complete"; -static const char stat_05[] PROGMEM = "Terminated"; -static const char stat_06[] PROGMEM = "Hard reset"; -static const char stat_07[] PROGMEM = "End of line"; -static const char stat_08[] PROGMEM = "End of file"; -static const char stat_09[] PROGMEM = "File not open"; - -static const char stat_10[] PROGMEM = "Max file size exceeded"; -static const char stat_11[] PROGMEM = "No such device"; -static const char stat_12[] PROGMEM = "Buffer empty"; -static const char stat_13[] PROGMEM = "Buffer full"; -static const char stat_14[] PROGMEM = "Buffer full - fatal"; -static const char stat_15[] PROGMEM = "Initializing"; -static const char stat_16[] PROGMEM = "Entering boot loader"; -static const char stat_17[] PROGMEM = "Function is stubbed"; -static const char stat_18[] PROGMEM = "18"; -static const char stat_19[] PROGMEM = "19"; - -static const char stat_20[] PROGMEM = "Internal error"; -static const char stat_21[] PROGMEM = "Internal range error"; -static const char stat_22[] PROGMEM = "Floating point error"; -static const char stat_23[] PROGMEM = "Divide by zero"; -static const char stat_24[] PROGMEM = "Invalid Address"; -static const char stat_25[] PROGMEM = "Read-only address"; -static const char stat_26[] PROGMEM = "Initialization failure"; -static const char stat_27[] PROGMEM = "System alarm - shutting down"; -static const char stat_28[] PROGMEM = "Failed to get planner buffer"; -static const char stat_29[] PROGMEM = "Generic exception report"; - -static const char stat_30[] PROGMEM = "Move time is infinite"; -static const char stat_31[] PROGMEM = "Move time is NAN"; -static const char stat_32[] PROGMEM = "Float is infinite"; -static const char stat_33[] PROGMEM = "Float is NAN"; -static const char stat_34[] PROGMEM = "Persistence error"; -static const char stat_35[] PROGMEM = "Bad status report setting"; -static const char stat_36[] PROGMEM = "36"; -static const char stat_37[] PROGMEM = "37"; -static const char stat_38[] PROGMEM = "38"; -static const char stat_39[] PROGMEM = "39"; - -static const char stat_40[] PROGMEM = "40"; -static const char stat_41[] PROGMEM = "41"; -static const char stat_42[] PROGMEM = "42"; -static const char stat_43[] PROGMEM = "43"; -static const char stat_44[] PROGMEM = "44"; -static const char stat_45[] PROGMEM = "45"; -static const char stat_46[] PROGMEM = "46"; -static const char stat_47[] PROGMEM = "47"; -static const char stat_48[] PROGMEM = "48"; -static const char stat_49[] PROGMEM = "49"; -static const char stat_50[] PROGMEM = "50"; -static const char stat_51[] PROGMEM = "51"; -static const char stat_52[] PROGMEM = "52"; -static const char stat_53[] PROGMEM = "53"; -static const char stat_54[] PROGMEM = "54"; -static const char stat_55[] PROGMEM = "55"; -static const char stat_56[] PROGMEM = "56"; -static const char stat_57[] PROGMEM = "57"; -static const char stat_58[] PROGMEM = "58"; -static const char stat_59[] PROGMEM = "59"; -static const char stat_60[] PROGMEM = "60"; -static const char stat_61[] PROGMEM = "61"; -static const char stat_62[] PROGMEM = "62"; -static const char stat_63[] PROGMEM = "63"; -static const char stat_64[] PROGMEM = "64"; -static const char stat_65[] PROGMEM = "65"; -static const char stat_66[] PROGMEM = "66"; -static const char stat_67[] PROGMEM = "67"; -static const char stat_68[] PROGMEM = "68"; -static const char stat_69[] PROGMEM = "69"; -static const char stat_70[] PROGMEM = "70"; -static const char stat_71[] PROGMEM = "71"; -static const char stat_72[] PROGMEM = "72"; -static const char stat_73[] PROGMEM = "73"; -static const char stat_74[] PROGMEM = "74"; -static const char stat_75[] PROGMEM = "75"; -static const char stat_76[] PROGMEM = "76"; -static const char stat_77[] PROGMEM = "77"; -static const char stat_78[] PROGMEM = "78"; -static const char stat_79[] PROGMEM = "79"; -static const char stat_80[] PROGMEM = "80"; -static const char stat_81[] PROGMEM = "81"; -static const char stat_82[] PROGMEM = "82"; -static const char stat_83[] PROGMEM = "83"; -static const char stat_84[] PROGMEM = "84"; -static const char stat_85[] PROGMEM = "85"; -static const char stat_86[] PROGMEM = "86"; -static const char stat_87[] PROGMEM = "87"; -static const char stat_88[] PROGMEM = "88"; -static const char stat_89[] PROGMEM = "89"; - -static const char stat_90[] PROGMEM = "Config sub-system assertion failure"; -static const char stat_91[] PROGMEM = "IO sub-system assertion failure"; -static const char stat_92[] PROGMEM = "Encoder assertion failure"; -static const char stat_93[] PROGMEM = "Stepper assertion failure"; -static const char stat_94[] PROGMEM = "Planner assertion failure"; -static const char stat_95[] PROGMEM = "Canonical machine assertion failure"; -static const char stat_96[] PROGMEM = "Controller assertion failure"; -static const char stat_97[] PROGMEM = "Stack overflow detected"; -static const char stat_98[] PROGMEM = "Memory fault detected"; -static const char stat_99[] PROGMEM = "Generic assertion failure"; - -static const char stat_100[] PROGMEM = "Unrecognized command or config name"; -static const char stat_101[] PROGMEM = "Invalid or malformed command"; -static const char stat_102[] PROGMEM = "Bad number format"; -static const char stat_103[] PROGMEM = "Unsupported number or JSON type"; -static const char stat_104[] PROGMEM = "Parameter is read-only"; -static const char stat_105[] PROGMEM = "Parameter cannot be read"; -static const char stat_106[] PROGMEM = "Command not accepted at this time"; -static const char stat_107[] PROGMEM = "Input exceeds max length"; -static const char stat_108[] PROGMEM = "Input less than minimum value"; -static const char stat_109[] PROGMEM = "Input exceeds maximum value"; - -static const char stat_110[] PROGMEM = "Input value range error"; -static const char stat_111[] PROGMEM = "JSON syntax error"; -static const char stat_112[] PROGMEM = "JSON input has too many pairs"; -static const char stat_113[] PROGMEM = "JSON string too long"; -static const char stat_114[] PROGMEM = "114"; -static const char stat_115[] PROGMEM = "115"; -static const char stat_116[] PROGMEM = "116"; -static const char stat_117[] PROGMEM = "117"; -static const char stat_118[] PROGMEM = "118"; -static const char stat_119[] PROGMEM = "119"; - -static const char stat_120[] PROGMEM = "120"; -static const char stat_121[] PROGMEM = "121"; -static const char stat_122[] PROGMEM = "122"; -static const char stat_123[] PROGMEM = "123"; -static const char stat_124[] PROGMEM = "124"; -static const char stat_125[] PROGMEM = "125"; -static const char stat_126[] PROGMEM = "126"; -static const char stat_127[] PROGMEM = "127"; -static const char stat_128[] PROGMEM = "128"; -static const char stat_129[] PROGMEM = "129"; - -static const char stat_130[] PROGMEM = "Generic Gcode input error"; -static const char stat_131[] PROGMEM = "Gcode command unsupported"; -static const char stat_132[] PROGMEM = "M code unsupported"; -static const char stat_133[] PROGMEM = "Gcode modal group violation"; -static const char stat_134[] PROGMEM = "Axis word missing"; -static const char stat_135[] PROGMEM = "Axis cannot be present"; -static const char stat_136[] PROGMEM = "Axis is invalid for this command"; -static const char stat_137[] PROGMEM = "Axis is disabled"; -static const char stat_138[] PROGMEM = "Axis target position is missing"; -static const char stat_139[] PROGMEM = "Axis target position is invalid"; - -static const char stat_140[] PROGMEM = "Selected plane is missing"; -static const char stat_141[] PROGMEM = "Selected plane is invalid"; -static const char stat_142[] PROGMEM = "Feedrate not specified"; -static const char stat_143[] PROGMEM = "Inverse time mode cannot be used with this command"; -static const char stat_144[] PROGMEM = "Rotary axes cannot be used with this command"; -static const char stat_145[] PROGMEM = "G0 or G1 must be active for G53"; -static const char stat_146[] PROGMEM = "Requested velocity exceeds limits"; -static const char stat_147[] PROGMEM = "Cutter compensation cannot be enabled"; -static const char stat_148[] PROGMEM = "Programmed point same as current point"; -static const char stat_149[] PROGMEM = "Spindle speed below minimum"; - -static const char stat_150[] PROGMEM = "Spindle speed exceeded maximum"; -static const char stat_151[] PROGMEM = "Spindle S word is missing"; -static const char stat_152[] PROGMEM = "Spindle S word is invalid"; -static const char stat_153[] PROGMEM = "Spindle must be off for this command"; -static const char stat_154[] PROGMEM = "Spindle must be turning for this command"; -static const char stat_155[] PROGMEM = "Arc specification error"; -static const char stat_156[] PROGMEM = "Arc specification error - missing axis(es)"; -static const char stat_157[] PROGMEM = "Arc specification error - missing offset(s)"; -static const char stat_158[] PROGMEM = "Arc specification error - radius arc out of tolerance"; // current longest message: 56 chard -static const char stat_159[] PROGMEM = "Arc specification error - endpoint is starting point"; - -static const char stat_160[] PROGMEM = "P word is missing"; -static const char stat_161[] PROGMEM = "P word is invalid"; -static const char stat_162[] PROGMEM = "P word is zero"; -static const char stat_163[] PROGMEM = "P word is negative"; -static const char stat_164[] PROGMEM = "P word is not an integer"; -static const char stat_165[] PROGMEM = "P word is not a valid tool number"; -static const char stat_166[] PROGMEM = "D word is missing"; -static const char stat_167[] PROGMEM = "D word is invalid"; -static const char stat_168[] PROGMEM = "E word is missing"; -static const char stat_169[] PROGMEM = "E word is invalid"; - -static const char stat_170[] PROGMEM = "H word is missing"; -static const char stat_171[] PROGMEM = "H word is invalid"; -static const char stat_172[] PROGMEM = "L word is missing"; -static const char stat_173[] PROGMEM = "L word is invalid"; -static const char stat_174[] PROGMEM = "Q word is missing"; -static const char stat_175[] PROGMEM = "Q word is invalid"; -static const char stat_176[] PROGMEM = "R word is missing"; -static const char stat_177[] PROGMEM = "R word is invalid"; -static const char stat_178[] PROGMEM = "T word is missing"; -static const char stat_179[] PROGMEM = "T word is invalid"; - -static const char stat_180[] PROGMEM = "180"; -static const char stat_181[] PROGMEM = "181"; -static const char stat_182[] PROGMEM = "182"; -static const char stat_183[] PROGMEM = "183"; -static const char stat_184[] PROGMEM = "184"; -static const char stat_185[] PROGMEM = "185"; -static const char stat_186[] PROGMEM = "186"; -static const char stat_187[] PROGMEM = "187"; -static const char stat_188[] PROGMEM = "188"; -static const char stat_189[] PROGMEM = "189"; - -static const char stat_190[] PROGMEM = "190"; -static const char stat_191[] PROGMEM = "191"; -static const char stat_192[] PROGMEM = "192"; -static const char stat_193[] PROGMEM = "193"; -static const char stat_194[] PROGMEM = "194"; -static const char stat_195[] PROGMEM = "195"; -static const char stat_196[] PROGMEM = "196"; -static const char stat_197[] PROGMEM = "197"; -static const char stat_198[] PROGMEM = "198"; -static const char stat_199[] PROGMEM = "199"; - -static const char stat_200[] PROGMEM = "Generic TinyG error"; -static const char stat_201[] PROGMEM = "Move less than minimum length"; -static const char stat_202[] PROGMEM = "Move less than minimum time"; -static const char stat_203[] PROGMEM = "Machine is alarmed - Command not processed"; // current longest message 43 chars (including 0) -static const char stat_204[] PROGMEM = "Limit switch hit - Shutdown occurred"; -static const char stat_205[] PROGMEM = "Trapezoid planner failed to converge"; -static const char stat_206[] PROGMEM = "206"; -static const char stat_207[] PROGMEM = "207"; -static const char stat_208[] PROGMEM = "208"; -static const char stat_209[] PROGMEM = "209"; - -static const char stat_210[] PROGMEM = "210"; -static const char stat_211[] PROGMEM = "211"; -static const char stat_212[] PROGMEM = "212"; -static const char stat_213[] PROGMEM = "213"; -static const char stat_214[] PROGMEM = "214"; -static const char stat_215[] PROGMEM = "215"; -static const char stat_216[] PROGMEM = "216"; -static const char stat_217[] PROGMEM = "217"; -static const char stat_218[] PROGMEM = "218"; -static const char stat_219[] PROGMEM = "219"; - -static const char stat_220[] PROGMEM = "Soft limit exceeded"; -static const char stat_221[] PROGMEM = "Soft limit exceeded - X min"; -static const char stat_222[] PROGMEM = "Soft limit exceeded - X max"; -static const char stat_223[] PROGMEM = "Soft limit exceeded - Y min"; -static const char stat_224[] PROGMEM = "Soft limit exceeded - Y max"; -static const char stat_225[] PROGMEM = "Soft limit exceeded - Z min"; -static const char stat_226[] PROGMEM = "Soft limit exceeded - Z max"; -static const char stat_227[] PROGMEM = "Soft limit exceeded - A min"; -static const char stat_228[] PROGMEM = "Soft limit exceeded - A max"; -static const char stat_229[] PROGMEM = "Soft limit exceeded - B min"; -static const char stat_230[] PROGMEM = "Soft limit exceeded - B max"; -static const char stat_231[] PROGMEM = "Soft limit exceeded - C min"; -static const char stat_232[] PROGMEM = "Soft limit exceeded - C max"; -static const char stat_233[] PROGMEM = "233"; -static const char stat_234[] PROGMEM = "234"; -static const char stat_235[] PROGMEM = "235"; -static const char stat_236[] PROGMEM = "236"; -static const char stat_237[] PROGMEM = "237"; -static const char stat_238[] PROGMEM = "238"; -static const char stat_239[] PROGMEM = "239"; - -static const char stat_240[] PROGMEM = "Homing cycle failed"; -static const char stat_241[] PROGMEM = "Homing Error - Bad or no axis specified"; -static const char stat_242[] PROGMEM = "Homing Error - Search velocity is zero"; -static const char stat_243[] PROGMEM = "Homing Error - Latch velocity is zero"; -static const char stat_244[] PROGMEM = "Homing Error - Travel min & max are the same"; -static const char stat_245[] PROGMEM = "Homing Error - Negative latch backoff"; -static const char stat_246[] PROGMEM = "Homing Error - Homing switches misconfigured"; -static const char stat_247[] PROGMEM = "247"; -static const char stat_248[] PROGMEM = "248"; -static const char stat_249[] PROGMEM = "249"; +stat_t status_code; // allocate a variable for the ritorno macro -static const char stat_250[] PROGMEM = "Probe cycle failed"; -static const char stat_251[] PROGMEM = "Probe endpoint is starting point"; -static const char stat_252[] PROGMEM = "Jogging cycle failed"; +#define MSG(N, TEXT) static const char stat_##N[] PROGMEM = TEXT; +#include "messages.def" +#undef MSG static const char *const stat_msg[] PROGMEM = { - stat_00, stat_01, stat_02, stat_03, stat_04, stat_05, stat_06, stat_07, stat_08, stat_09, - stat_10, stat_11, stat_12, stat_13, stat_14, stat_15, stat_16, stat_17, stat_18, stat_19, - stat_20, stat_21, stat_22, stat_23, stat_24, stat_25, stat_26, stat_27, stat_28, stat_29, - stat_30, stat_31, stat_32, stat_33, stat_34, stat_35, stat_36, stat_37, stat_38, stat_39, - stat_40, stat_41, stat_42, stat_43, stat_44, stat_45, stat_46, stat_47, stat_48, stat_49, - stat_50, stat_51, stat_52, stat_53, stat_54, stat_55, stat_56, stat_57, stat_58, stat_59, - stat_60, stat_61, stat_62, stat_63, stat_64, stat_65, stat_66, stat_67, stat_68, stat_69, - stat_70, stat_71, stat_72, stat_73, stat_74, stat_75, stat_76, stat_77, stat_78, stat_79, - stat_80, stat_81, stat_82, stat_83, stat_84, stat_85, stat_86, stat_87, stat_88, stat_89, - stat_90, stat_91, stat_92, stat_93, stat_94, stat_95, stat_96, stat_97, stat_98, stat_99, - stat_100, stat_101, stat_102, stat_103, stat_104, stat_105, stat_106, stat_107, stat_108, stat_109, - stat_110, stat_111, stat_112, stat_113, stat_114, stat_115, stat_116, stat_117, stat_118, stat_119, - stat_120, stat_121, stat_122, stat_123, stat_124, stat_125, stat_126, stat_127, stat_128, stat_129, - stat_130, stat_131, stat_132, stat_133, stat_134, stat_135, stat_136, stat_137, stat_138, stat_139, - stat_140, stat_141, stat_142, stat_143, stat_144, stat_145, stat_146, stat_147, stat_148, stat_149, - stat_150, stat_151, stat_152, stat_153, stat_154, stat_155, stat_156, stat_157, stat_158, stat_159, - stat_160, stat_161, stat_162, stat_163, stat_164, stat_165, stat_166, stat_167, stat_168, stat_169, - stat_170, stat_171, stat_172, stat_173, stat_174, stat_175, stat_176, stat_177, stat_178, stat_179, - stat_180, stat_181, stat_182, stat_183, stat_184, stat_185, stat_186, stat_187, stat_188, stat_189, - stat_190, stat_191, stat_192, stat_193, stat_194, stat_195, stat_196, stat_197, stat_198, stat_199, - stat_200, stat_201, stat_202, stat_203, stat_204, stat_205, stat_206, stat_207, stat_208, stat_209, - stat_210, stat_211, stat_212, stat_213, stat_214, stat_215, stat_216, stat_217, stat_218, stat_219, - stat_220, stat_221, stat_222, stat_223, stat_224, stat_225, stat_226, stat_227, stat_228, stat_229, - stat_230, stat_231, stat_232, stat_233, stat_234, stat_235, stat_236, stat_237, stat_238, stat_239, - stat_240, stat_241, stat_242, stat_243, stat_244, stat_245, stat_246, stat_247, stat_248, stat_249, - stat_250, stat_251, stat_252 +#define MSG(N, TEXT) stat_##N, +#include "messages.def" +#undef MSG }; /// Return the status message -char *get_status_message(stat_t status) { - return (char *)GET_TEXT_ITEM(stat_msg, status); +void print_status_message(const char *msg, stat_t status) { + printf_P("%S: %S (%d)\n", pgm_read_word(&stat_msg[status])); } diff --git a/src/tinyg.h b/src/status.h similarity index 50% rename from src/tinyg.h rename to src/status.h index 205c5f4..0a78dde 100644 --- a/src/tinyg.h +++ b/src/status.h @@ -1,116 +1,36 @@ -/* - * tinyg.h - tinyg main header - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - * - * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#ifndef TINYG_H_ONCE -#define TINYG_H_ONCE - -// common system includes -#include -#include -#include -#include -#include -#include -#include - -// Revisions -#ifndef TINYG_FIRMWARE_BUILD -#define TINYG_FIRMWARE_BUILD 440.20 // arc test - -#endif -#define TINYG_FIRMWARE_VERSION 0.97 // firmware major version -#define TINYG_HARDWARE_PLATFORM HW_PLATFORM_TINYG_XMEGA // see hardware.h -#define TINYG_HARDWARE_VERSION HW_VERSION_TINYGV8 // see hardware.h -#define TINYG_HARDWARE_VERSION_MAX TINYG_HARDWARE_VERSION - -// Compile-time settings -#define __STEP_CORRECTION -//#define __JERK_EXEC // Use computed jerk (versus forward difference based exec) -//#define __KAHAN // Use Kahan summation in aline exec functions - -#define __TEXT_MODE // enables text mode (~10Kb) -#define __HELP_SCREENS // enables help screens (~3.5Kb) -//#define __CANNED_TESTS // enables $tests (~12Kb) -//#define __TEST_99 // enables diagnostic test 99 (independent of other tests) - -// Development settings -#define __DIAGNOSTIC_PARAMETERS // enables system diagnostic parameters (_xx) in config_app -//#define __DEBUG_SETTINGS // special settings. See settings.h -//#define __CANNED_STARTUP // run any canned startup moves - -#include // defines PROGMEM and PSTR +/******************************************************************************\ -typedef char char_t; + This file is part of the TinyG firmware. - // gets rely on nv->index having been set -#define GET_TABLE_WORD(a) pgm_read_word(&cfgArray[nv->index].a) // get word value from cfgArray -#define GET_TABLE_BYTE(a) pgm_read_byte(&cfgArray[nv->index].a) // get byte value from cfgArray -#define GET_TABLE_FLOAT(a) pgm_read_float(&cfgArray[nv->index].a) // get float value from cfgArray -#define GET_TOKEN_BYTE(a) (char_t)pgm_read_byte(&cfgArray[i].a) // get token byte value from cfgArray + Copyright (c) 2016, Buildbotics LLC + All rights reserved. -// populate the shared buffer with the token string given the index -#define GET_TOKEN_STRING(i,a) strcpy_P(a, (char *)&cfgArray[(index_t)i].token); + The C! library is free software: you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation, either version 2.1 of + the License, or (at your option) any later version. -// get text from an array of strings in PGM and convert to RAM string -#define GET_TEXT_ITEM(b,a) strncpy_P(global_string_buf,(const char *)pgm_read_word(&b[a]), MESSAGE_LEN-1) + The C! library 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. -// get units from array of strings in PGM and convert to RAM string -#define GET_UNITS(a) strncpy_P(global_string_buf,(const char *)pgm_read_word(&msg_units[cm_get_units_mode(a)]), MESSAGE_LEN-1) + You should have received a copy of the GNU Lesser General Public + License along with the C! library. If not, see + . -// String compatibility -#define strtof strtod // strtof is not in the AVR lib + In addition, BSD licensing may be granted on a case by case basis + by written permission from at least one of the copyright holders. + You may request written permission by emailing the authors. + For information regarding this software email: + Joseph Coffland + joseph@buildbotics.com -typedef uint16_t magic_t; // magic number size -#define MAGICNUM 0x12EF // used for memory integrity assertions - -/***** Axes, motors & PWM channels used by the application *****/ -// Axes, motors & PWM channels must be defines (not enums) so #ifdef can be used - -#define AXES 6 // number of axes supported in this version -#define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) -#define MOTORS 4 // number of motors on the board -#define COORDS 6 // number of supported coordinate systems (1-6) -#define PWMS 2 // number of supported PWM channels - -// Note: If you change COORDS you must adjust the entries in cfgArray table in config.c - -#define AXIS_X 0 -#define AXIS_Y 1 -#define AXIS_Z 2 -#define AXIS_A 3 -#define AXIS_B 4 -#define AXIS_C 5 -#define AXIS_U 6 // reserved -#define AXIS_V 7 // reserved -#define AXIS_W 8 // reserved - -#define MOTOR_1 0 // define motor numbers and array indexes -#define MOTOR_2 1 // must be defines. enums don't work -#define MOTOR_3 2 -#define MOTOR_4 3 -#define MOTOR_5 4 -#define MOTOR_6 5 - -#define PWM_1 0 -#define PWM_2 1 +\******************************************************************************/ +#ifndef STATUS_H +#define STATUS_H /************************************************************************************ * STATUS CODES @@ -139,44 +59,41 @@ typedef uint16_t magic_t; // magic number size * an exception report. These are labeled as WARNING */ -typedef uint8_t stat_t; -extern stat_t status_code; // allocated in main.c +#include -#define MESSAGE_LEN 80 // global message string storage allocation -extern char global_string_buf[]; // allocated in main.c +typedef uint8_t stat_t; +extern stat_t status_code; -char *get_status_message(stat_t status); +void print_status_message(const char *msg, stat_t status); // ritorno is a handy way to provide exception returns // It returns only if an error occurred. (ritorno is Italian for return) -#define ritorno(a) if((status_code=a) != STAT_OK) { return status_code; } +#define ritorno(a) if ((status_code = a) != STAT_OK) {return status_code;} // OS, communications and low-level status #define STAT_OK 0 // function completed OK -#define STAT_ERROR 1 // generic error return EPERM +#define STAT_ERROR 1 // generic error return EPERM #define STAT_EAGAIN 2 // function would block here (call again) -#define STAT_NOOP 3 // function had no-operation -#define STAT_COMPLETE 4 // operation is complete -#define STAT_TERMINATE 5 // operation terminated (gracefully) -#define STAT_RESET 6 // operation was hard reset (sig kill) -#define STAT_EOL 7 // function returned end-of-line -#define STAT_EOF 8 // function returned end-of-file +#define STAT_NOOP 3 // function had no-operation +#define STAT_COMPLETE 4 // operation is complete +#define STAT_TERMINATE 5 // operation terminated (gracefully) +#define STAT_RESET 6 // operation was hard reset (sig kill) +#define STAT_EOL 7 // function returned end-of-line +#define STAT_EOF 8 // function returned end-of-file #define STAT_FILE_NOT_OPEN 9 #define STAT_FILE_SIZE_EXCEEDED 10 #define STAT_NO_SUCH_DEVICE 11 #define STAT_BUFFER_EMPTY 12 #define STAT_BUFFER_FULL 13 #define STAT_BUFFER_FULL_FATAL 14 -#define STAT_INITIALIZING 15 // initializing - not ready for use -#define STAT_ENTERING_BOOT_LOADER 16 // this code actually emitted from boot loader, not TinyG +#define STAT_INITIALIZING 15 // initializing - not ready for use +#define STAT_ENTERING_BOOT_LOADER 16 // this code actually emitted from boot loader, not TinyG #define STAT_FUNCTION_IS_STUBBED 17 -#define STAT_ERROR_18 18 -#define STAT_ERROR_19 19 // Internal errors and startup messages #define STAT_INTERNAL_ERROR 20 // unrecoverable internal error -#define STAT_INTERNAL_RANGE_ERROR 21 // number range other than by user input -#define STAT_FLOATING_POINT_ERROR 22 // number conversion error +#define STAT_INTERNAL_RANGE_ERROR 21 // number range other than by user input +#define STAT_FLOATING_POINT_ERROR 22 // number conversion error #define STAT_DIVIDE_BY_ZERO 23 #define STAT_INVALID_ADDRESS 24 #define STAT_READ_ONLY_ADDRESS 25 @@ -191,68 +108,8 @@ char *get_status_message(stat_t status); #define STAT_FLOAT_IS_NAN 33 #define STAT_PERSISTENCE_ERROR 34 #define STAT_BAD_STATUS_REPORT_SETTING 35 -#define STAT_ERROR_36 36 -#define STAT_ERROR_37 37 -#define STAT_ERROR_38 38 -#define STAT_ERROR_39 39 - -#define STAT_ERROR_40 40 -#define STAT_ERROR_41 41 -#define STAT_ERROR_42 42 -#define STAT_ERROR_43 43 -#define STAT_ERROR_44 44 -#define STAT_ERROR_45 45 -#define STAT_ERROR_46 46 -#define STAT_ERROR_47 47 -#define STAT_ERROR_48 48 -#define STAT_ERROR_49 49 - -#define STAT_ERROR_50 50 -#define STAT_ERROR_51 51 -#define STAT_ERROR_52 52 -#define STAT_ERROR_53 53 -#define STAT_ERROR_54 54 -#define STAT_ERROR_55 55 -#define STAT_ERROR_56 56 -#define STAT_ERROR_57 57 -#define STAT_ERROR_58 58 -#define STAT_ERROR_59 59 - -#define STAT_ERROR_60 60 -#define STAT_ERROR_61 61 -#define STAT_ERROR_62 62 -#define STAT_ERROR_63 63 -#define STAT_ERROR_64 64 -#define STAT_ERROR_65 65 -#define STAT_ERROR_66 66 -#define STAT_ERROR_67 67 -#define STAT_ERROR_68 68 -#define STAT_ERROR_69 69 - -#define STAT_ERROR_70 70 -#define STAT_ERROR_71 71 -#define STAT_ERROR_72 72 -#define STAT_ERROR_73 73 -#define STAT_ERROR_74 74 -#define STAT_ERROR_75 75 -#define STAT_ERROR_76 76 -#define STAT_ERROR_77 77 -#define STAT_ERROR_78 78 -#define STAT_ERROR_79 79 - -#define STAT_ERROR_80 80 -#define STAT_ERROR_81 81 -#define STAT_ERROR_82 82 -#define STAT_ERROR_83 83 -#define STAT_ERROR_84 84 -#define STAT_ERROR_85 85 -#define STAT_ERROR_86 86 -#define STAT_ERROR_87 87 -#define STAT_ERROR_88 88 -#define STAT_ERROR_89 89 // Assertion failures - build down from 99 until they meet the system internal errors - #define STAT_CONFIG_ASSERTION_FAILURE 90 #define STAT_ENCODER_ASSERTION_FAILURE 92 #define STAT_STEPPER_ASSERTION_FAILURE 93 @@ -260,7 +117,7 @@ char *get_status_message(stat_t status); #define STAT_CANONICAL_MACHINE_ASSERTION_FAILURE 95 #define STAT_CONTROLLER_ASSERTION_FAILURE 96 #define STAT_STACK_OVERFLOW 97 -#define STAT_MEMORY_FAULT 98 // generic memory corruption detected by magic numbers +#define STAT_MEMORY_FAULT 98 // generic memory corruption #define STAT_GENERIC_ASSERTION_FAILURE 99 // generic assertion failure - unclassified // Application and data input errors @@ -272,7 +129,7 @@ char *get_status_message(stat_t status); #define STAT_UNSUPPORTED_TYPE 103 // An otherwise valid number or JSON type is not supported #define STAT_PARAMETER_IS_READ_ONLY 104 // input error: parameter cannot be set #define STAT_PARAMETER_CANNOT_BE_READ 105 // input error: parameter cannot be set -#define STAT_COMMAND_NOT_ACCEPTED 106 // command cannot be accepted at this time +#define STAT_COMMAND_NOT_ACCEPTED 106 // command cannot be accepted at this time #define STAT_INPUT_EXCEEDS_MAX_LENGTH 107 // input string is too long #define STAT_INPUT_LESS_THAN_MIN_VALUE 108 // input error: value is under minimum #define STAT_INPUT_EXCEEDS_MAX_VALUE 109 // input error: value is over maximum @@ -280,28 +137,10 @@ char *get_status_message(stat_t status); #define STAT_INPUT_VALUE_RANGE_ERROR 110 // input error: value is out-of-range #define STAT_JSON_SYNTAX_ERROR 111 // JSON input string is not well formed #define STAT_JSON_TOO_MANY_PAIRS 112 // JSON input string has too many JSON pairs -#define STAT_JSON_TOO_LONG 113 // JSON input or output exceeds buffer size -#define STAT_ERROR_114 114 -#define STAT_ERROR_115 115 -#define STAT_ERROR_116 116 -#define STAT_ERROR_117 117 -#define STAT_ERROR_118 118 -#define STAT_ERROR_119 119 - -#define STAT_ERROR_120 120 -#define STAT_ERROR_121 121 -#define STAT_ERROR_122 122 -#define STAT_ERROR_123 123 -#define STAT_ERROR_124 124 -#define STAT_ERROR_125 125 -#define STAT_ERROR_126 126 -#define STAT_ERROR_127 127 -#define STAT_ERROR_128 128 -#define STAT_ERROR_129 129 +#define STAT_JSON_TOO_LONG 113 // JSON input or output exceeds buffer size // Gcode errors and warnings (Most originate from NIST - by concept, not number) // Fascinating: http://www.cncalarms.com/ - #define STAT_GCODE_GENERIC_INPUT_ERROR 130 // generic error for gcode input #define STAT_GCODE_COMMAND_UNSUPPORTED 131 // G command is not supported #define STAT_MCODE_COMMAND_UNSUPPORTED 132 // M command is not supported @@ -357,28 +196,6 @@ char *get_status_message(stat_t status); #define STAT_T_WORD_IS_MISSING 178 #define STAT_T_WORD_IS_INVALID 179 -#define STAT_ERROR_180 180 // reserved for Gcode errors -#define STAT_ERROR_181 181 -#define STAT_ERROR_182 182 -#define STAT_ERROR_183 183 -#define STAT_ERROR_184 184 -#define STAT_ERROR_185 185 -#define STAT_ERROR_186 186 -#define STAT_ERROR_187 187 -#define STAT_ERROR_188 188 -#define STAT_ERROR_189 189 - -#define STAT_ERROR_190 190 -#define STAT_ERROR_191 191 -#define STAT_ERROR_192 192 -#define STAT_ERROR_193 193 -#define STAT_ERROR_194 194 -#define STAT_ERROR_195 195 -#define STAT_ERROR_196 196 -#define STAT_ERROR_197 197 -#define STAT_ERROR_198 198 -#define STAT_ERROR_199 199 - // TinyG errors and warnings #define STAT_GENERIC_ERROR 200 #define STAT_MINIMUM_LENGTH_MOVE 201 // move is less than minimum length @@ -386,21 +203,6 @@ char *get_status_message(stat_t status); #define STAT_MACHINE_ALARMED 203 // machine is alarmed. Command not processed #define STAT_LIMIT_SWITCH_HIT 204 // a limit switch was hit causing shutdown #define STAT_PLANNER_FAILED_TO_CONVERGE 205 // trapezoid generator can through this exception -#define STAT_ERROR_206 206 -#define STAT_ERROR_207 207 -#define STAT_ERROR_208 208 -#define STAT_ERROR_209 209 - -#define STAT_ERROR_210 210 -#define STAT_ERROR_211 211 -#define STAT_ERROR_212 212 -#define STAT_ERROR_213 213 -#define STAT_ERROR_214 214 -#define STAT_ERROR_215 215 -#define STAT_ERROR_216 216 -#define STAT_ERROR_217 217 -#define STAT_ERROR_218 218 -#define STAT_ERROR_219 219 #define STAT_SOFT_LIMIT_EXCEEDED 220 // soft limit error - axis unspecified #define STAT_SOFT_LIMIT_EXCEEDED_XMIN 221 // soft limit error - X minimum @@ -416,13 +218,6 @@ char *get_status_message(stat_t status); #define STAT_SOFT_LIMIT_EXCEEDED_BMAX 220 // soft limit error - B maximum #define STAT_SOFT_LIMIT_EXCEEDED_CMIN 231 // soft limit error - C minimum #define STAT_SOFT_LIMIT_EXCEEDED_CMAX 232 // soft limit error - C maximum -#define STAT_ERROR_233 233 -#define STAT_ERROR_234 234 -#define STAT_ERROR_235 235 -#define STAT_ERROR_236 236 -#define STAT_ERROR_237 237 -#define STAT_ERROR_238 238 -#define STAT_ERROR_239 239 #define STAT_HOMING_CYCLE_FAILED 240 // homing cycle did not complete #define STAT_HOMING_ERROR_BAD_OR_NO_AXIS 241 @@ -431,9 +226,6 @@ char *get_status_message(stat_t status); #define STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL 244 #define STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF 245 #define STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION 246 -#define STAT_ERROR_247 247 -#define STAT_ERROR_248 248 -#define STAT_ERROR_249 249 #define STAT_PROBE_CYCLE_FAILED 250 // probing cycle did not complete #define STAT_PROBE_ENDPOINT_IS_STARTING_POINT 251 @@ -441,4 +233,4 @@ char *get_status_message(stat_t status); // !!! Do not exceed 255 without also changing stat_t typedef -#endif // TINYG2_H_ONCE +#endif // STATUS_H diff --git a/src/stepper.c b/src/stepper.c index 5c65f80..0364e40 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -30,16 +30,21 @@ * See stepper.h for a detailed explanation of this module. */ -#include "tinyg.h" -#include "config.h" #include "stepper.h" + +#include "config.h" #include "encoder.h" -#include "planner.h" -#include "report.h" #include "hardware.h" -#include "text_parser.h" #include "util.h" +#include "rtc.h" +#include "report.h" +#include "plan/planner.h" + +#include +#include +#include +#include stConfig_t st_cfg; stPrepSingleton_t st_pre; @@ -52,6 +57,13 @@ static void _request_load_move(); // handy macro #define _f_to_period(f) (uint16_t)((float)F_CPU / (float)f) +#define MOTOR_1 0 +#define MOTOR_2 1 +#define MOTOR_3 2 +#define MOTOR_4 3 +#define MOTOR_5 4 +#define MOTOR_6 5 + /* * Initialize stepper motor subsystem @@ -64,7 +76,6 @@ static void _request_load_move(); */ void stepper_init() { memset(&st_run, 0, sizeof(st_run)); // clear all values, pointers and status - stepper_init_assertions(); // Configure virtual ports PORTCFG.VPCTRLA = PORTCFG_VP0MAP_PORT_MOTOR_1_gc | PORTCFG_VP1MAP_PORT_MOTOR_2_gc; @@ -99,26 +110,63 @@ void stepper_init() { TIMER_EXEC.PER = EXEC_TIMER_PERIOD; // set period st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; - st_reset(); // reset steppers to known state -} - - -void stepper_init_assertions() { - st_run.magic_end = MAGICNUM; - st_run.magic_start = MAGICNUM; - st_pre.magic_end = MAGICNUM; - st_pre.magic_start = MAGICNUM; -} + // defaults + st_cfg.motor_power_timeout = MOTOR_IDLE_TIMEOUT; + + st_cfg.mot[MOTOR_1].motor_map = M1_MOTOR_MAP; + st_cfg.mot[MOTOR_1].step_angle = M1_STEP_ANGLE; + st_cfg.mot[MOTOR_1].travel_rev = M1_TRAVEL_PER_REV; + st_cfg.mot[MOTOR_1].microsteps = M1_MICROSTEPS; + st_cfg.mot[MOTOR_1].polarity = M1_POLARITY; + st_cfg.mot[MOTOR_1].power_mode = M1_POWER_MODE; +#if (MOTORS >= 2) + st_cfg.mot[MOTOR_2].motor_map = M2_MOTOR_MAP; + st_cfg.mot[MOTOR_2].step_angle = M2_STEP_ANGLE; + st_cfg.mot[MOTOR_2].travel_rev = M2_TRAVEL_PER_REV; + st_cfg.mot[MOTOR_2].microsteps = M2_MICROSTEPS; + st_cfg.mot[MOTOR_2].polarity = M2_POLARITY; + st_cfg.mot[MOTOR_2].power_mode = M2_POWER_MODE; +#endif +#if (MOTORS >= 3) + st_cfg.mot[MOTOR_3].motor_map = M3_MOTOR_MAP; + st_cfg.mot[MOTOR_3].step_angle = M3_STEP_ANGLE; + st_cfg.mot[MOTOR_3].travel_rev = M3_TRAVEL_PER_REV; + st_cfg.mot[MOTOR_3].microsteps = M3_MICROSTEPS; + st_cfg.mot[MOTOR_3].polarity = M3_POLARITY; + st_cfg.mot[MOTOR_3].power_mode = M3_POWER_MODE; +#endif +#if (MOTORS >= 4) + st_cfg.mot[MOTOR_4].motor_map = M4_MOTOR_MAP; + st_cfg.mot[MOTOR_4].step_angle = M4_STEP_ANGLE; + st_cfg.mot[MOTOR_4].travel_rev = M4_TRAVEL_PER_REV; + st_cfg.mot[MOTOR_4].microsteps = M4_MICROSTEPS; + st_cfg.mot[MOTOR_4].polarity = M4_POLARITY; + st_cfg.mot[MOTOR_4].power_mode = M4_POWER_MODE; +#endif +#if (MOTORS >= 5) + st_cfg.mot[MOTOR_5].motor_map = M5_MOTOR_MAP; + st_cfg.mot[MOTOR_5].step_angle = M5_STEP_ANGLE; + st_cfg.mot[MOTOR_5].travel_rev = M5_TRAVEL_PER_REV; + st_cfg.mot[MOTOR_5].microsteps = M5_MICROSTEPS; + st_cfg.mot[MOTOR_5].polarity = M5_POLARITY; + st_cfg.mot[MOTOR_5].power_mode = M5_POWER_MODE; +#endif +#if (MOTORS >= 6) + st_cfg.mot[MOTOR_6].motor_map = M6_MOTOR_MAP; + st_cfg.mot[MOTOR_6].step_angle = M6_STEP_ANGLE; + st_cfg.mot[MOTOR_6].travel_rev = M6_TRAVEL_PER_REV; + st_cfg.mot[MOTOR_6].microsteps = M6_MICROSTEPS; + st_cfg.mot[MOTOR_6].polarity = M6_POLARITY; + st_cfg.mot[MOTOR_6].power_mode = M6_POWER_MODE; +#endif -/// Test assertions, return error code if violation exists -stat_t stepper_test_assertions() { - if (st_run.magic_end != MAGICNUM) return STAT_STEPPER_ASSERTION_FAILURE; - if (st_run.magic_start != MAGICNUM) return STAT_STEPPER_ASSERTION_FAILURE; - if (st_pre.magic_end != MAGICNUM) return STAT_STEPPER_ASSERTION_FAILURE; - if (st_pre.magic_start != MAGICNUM) return STAT_STEPPER_ASSERTION_FAILURE; + // Init steps per unit + for (int m = 0; m < MOTORS; m++) + st_cfg.mot[m].steps_per_unit = + (360 * st_cfg.mot[m].microsteps) / (st_cfg.mot[m].travel_rev * st_cfg.mot[m].step_angle); - return STAT_OK; + st_reset(); // reset steppers to known state } @@ -146,19 +194,11 @@ void st_reset() { } -/// Clear diagnostic counters, reset stepper prep -stat_t st_clc(nvObj_t *nv) { - st_reset(); - return STAT_OK; -} - - /* * Motor power management functions * * _deenergize_motor() - remove power from a motor * _energize_motor() - apply power to a motor - * _set_motor_power_level() - set the actual Vref to a specified power level * * st_energize_motors() - apply power to all motors * st_deenergize_motors() - remove power from all motors @@ -228,7 +268,7 @@ void st_deenergize_motors() { */ stat_t st_motor_power_callback() { // called by controller // manage power for each motor individually - for (uint8_t m = MOTOR_1; m < MOTORS; m++) { + for (uint8_t m = 0; m < MOTORS; m++) { // de-energize motor if it's set to MOTOR_DISABLED if (st_cfg.mot[m].power_mode == MOTOR_DISABLED) { @@ -245,7 +285,7 @@ stat_t st_motor_power_callback() { // called by controller // start a countdown if MOTOR_POWERED_IN_CYCLE or MOTOR_POWERED_ONLY_WHEN_MOVING if (st_run.mot[m].power_state == MOTOR_POWER_TIMEOUT_START) { st_run.mot[m].power_state = MOTOR_POWER_TIMEOUT_COUNTDOWN; - st_run.mot[m].power_systick = SysTickTimer_getValue() + + st_run.mot[m].power_systick = rtc_get_time() + (st_cfg.motor_power_timeout * 1000); } @@ -257,10 +297,10 @@ stat_t st_motor_power_callback() { // called by controller // run the countdown if you are in a countdown if (st_run.mot[m].power_state == MOTOR_POWER_TIMEOUT_COUNTDOWN) { - if (SysTickTimer_getValue() > st_run.mot[m].power_systick ) { + if (rtc_get_time() > st_run.mot[m].power_systick ) { st_run.mot[m].power_state = MOTOR_IDLE; _deenergize_motor(m); - sr_request_status_report(SR_TIMED_REQUEST); // request a status report when motors shut down + report_request(); // request a status report when motors shut down } } } @@ -641,7 +681,6 @@ stat_t st_prep_line(float travel_steps[], float following_error[], float segment st_pre.dda_ticks_X_substeps = st_pre.dda_ticks * DDA_SUBSTEPS; // setup motor parameters - float correction_steps; for (uint8_t motor = 0; motor < MOTORS; motor++) { // Skip this motor if there are no new steps. Leave all other values intact. if (fp_ZERO(travel_steps[motor])) { @@ -673,6 +712,8 @@ stat_t st_prep_line(float travel_steps[], float following_error[], float segment } #ifdef __STEP_CORRECTION + float correction_steps; + // 'Nudge' correction strategy. Inject a single, scaled correction value then hold off if ((--st_pre.mot[motor].correction_holdoff < 0) && (fabs(following_error[motor]) > STEP_CORRECTION_THRESHOLD)) { @@ -727,176 +768,53 @@ void st_prep_dwell(float microseconds) { } -/// Helper to return motor number as an index -static int8_t _get_motor(const nvObj_t *nv) { - return nv->group[0] ? nv->group[0] : nv->token[0] - 0x31; -} +float get_ang(int index) {return st_cfg.mot[index].step_angle;} +float get_trvl(int index) {return st_cfg.mot[index].travel_rev;} +uint16_t get_mstep(int index) {return st_cfg.mot[index].microsteps;} +bool get_pol(int index) {return st_cfg.mot[index].polarity;} +uint16_t get_mvel(int index) {return 0;} +uint16_t get_mjerk(int index) {return 0;} -/// This function will need to be rethought if microstep morphing is implemented -static void _set_motor_steps_per_unit(nvObj_t *nv) { - uint8_t m = _get_motor(nv); - st_cfg.mot[m].steps_per_unit = (360 * st_cfg.mot[m].microsteps) / (st_cfg.mot[m].travel_rev * st_cfg.mot[m].step_angle); - st_reset(); +void update_steps_per_unit(int index) { + st_cfg.mot[index].steps_per_unit = + (360 * st_cfg.mot[index].microsteps) / + (st_cfg.mot[index].travel_rev * st_cfg.mot[index].step_angle); } -/// Set motor step angle -stat_t st_set_sa(nvObj_t *nv) { - set_flt(nv); - _set_motor_steps_per_unit(nv); - - return STAT_OK; +void set_ang(int index, float value) { + st_cfg.mot[index].step_angle = value; + update_steps_per_unit(index); } -/// Set motor travel per revolution -stat_t st_set_tr(nvObj_t *nv) { - set_flu(nv); - _set_motor_steps_per_unit(nv); - - return STAT_OK; +void set_trvl(int index, float value) { + st_cfg.mot[index].travel_rev = value; + update_steps_per_unit(index); } -// Set motor microsteps -stat_t st_set_mi(nvObj_t *nv) { - set_int(nv); - _set_motor_steps_per_unit(nv); - - return STAT_OK; -} - - -// Set motor power mode -stat_t st_set_pm(nvObj_t *nv) { - // NOTE: The motor power callback makes these settings take effect immediately - if ((uint8_t)nv->value >= MOTOR_POWER_MODE_MAX_VALUE) - return STAT_INPUT_VALUE_RANGE_ERROR; - - set_ui8(nv); - - return STAT_OK; -} - - -/* - * st_set_pl() - set motor power level - * - * Input value may vary from 0.000 to 1.000 The setting is scaled to allowable PWM range. - * This function sets both the scaled and dynamic power levels, and applies the - * scaled value to the vref. - */ -stat_t st_set_pl(nvObj_t *nv) { - return STAT_OK; -} - - -/// get motor enable power state -stat_t st_get_pwr(nvObj_t *nv) { - nv->value = _motor_is_enabled(_get_motor(nv)); - nv->valuetype = TYPE_INTEGER; - - return STAT_OK; -} - - -/* GLOBAL FUNCTIONS (SYSTEM LEVEL) - * Calling me or md with 0 will enable or disable all motors - * Setting a value of 0 will enable or disable all motors - * Setting a value from 1 to MOTORS will enable or disable that motor only - */ - -/// Set motor timeout in seconds -stat_t st_set_mt(nvObj_t *nv) { - st_cfg.motor_power_timeout = min(MOTOR_TIMEOUT_SECONDS_MAX, max(nv->value, MOTOR_TIMEOUT_SECONDS_MIN)); - return STAT_OK; -} - - -/// Disable motor power -stat_t st_set_md(nvObj_t *nv) { - // Make sure this function is not part of initialization --> f00 - if (((uint8_t)nv->value == 0) || (nv->valuetype == TYPE_0)) - st_deenergize_motors(); - - else { - uint8_t motor = (uint8_t)nv->value; - if (motor > MOTORS) return STAT_INPUT_VALUE_RANGE_ERROR; - _deenergize_motor(motor - 1); // adjust so that motor 1 is actually 0 (etc) +void set_mstep(int index, uint16_t value) { + switch (value) { + case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256: + break; + default: return; } - return STAT_OK; + st_cfg.mot[index].microsteps = value; + update_steps_per_unit(index); } -/// enable motor power -stat_t st_set_me(nvObj_t *nv) { - // Make sure this function is not part of initialization --> f00 - if (((uint8_t)nv->value == 0) || (nv->valuetype == TYPE_0)) - st_energize_motors(); - - else { - uint8_t motor = (uint8_t)nv->value; - if (motor > MOTORS) return STAT_INPUT_VALUE_RANGE_ERROR; - _energize_motor(motor - 1); // adjust so that motor 1 is actually 0 (etc) - } - - return STAT_OK; +void set_pol(int index, bool value) { + st_cfg.mot[index].polarity = value; } -#ifdef __TEXT_MODE - -static const char msg_units0[] PROGMEM = " in"; // used by generic print functions -static const char msg_units1[] PROGMEM = " mm"; -static const char msg_units2[] PROGMEM = " deg"; -static const char *const msg_units[] PROGMEM = {msg_units0, msg_units1, msg_units2}; -#define DEGREE_INDEX 2 - -static const char fmt_me[] PROGMEM = "motors energized\n"; -static const char fmt_md[] PROGMEM = "motors de-energized\n"; -static const char fmt_mt[] PROGMEM = "[mt] motor idle timeout%14.2f Sec\n"; -static const char fmt_0ma[] PROGMEM = "[%s%s] m%s map to axis%15d [0=X,1=Y,2=Z...]\n"; -static const char fmt_0sa[] PROGMEM = "[%s%s] m%s step angle%20.3f%s\n"; -static const char fmt_0tr[] PROGMEM = "[%s%s] m%s travel per revolution%10.4f%s\n"; -static const char fmt_0mi[] PROGMEM = "[%s%s] m%s microsteps%16d [1,2,4,8,16,32,64,128,256]\n"; -static const char fmt_0po[] PROGMEM = "[%s%s] m%s polarity%18d [0=normal,1=reverse]\n"; -static const char fmt_0pm[] PROGMEM = "[%s%s] m%s power management%10d [0=disabled,1=always on,2=in cycle,3=when moving]\n"; -static const char fmt_0pl[] PROGMEM = "[%s%s] m%s motor power level%13.3f [0.000=minimum, 1.000=maximum]\n"; -static const char fmt_pwr[] PROGMEM = "Motor %c power enabled state:%2.0f\n"; - -void st_print_mt(nvObj_t *nv) {text_print_flt(nv, fmt_mt);} -void st_print_me(nvObj_t *nv) {text_print_nul(nv, fmt_me);} -void st_print_md(nvObj_t *nv) {text_print_nul(nv, fmt_md);} - - -static void _print_motor_ui8(nvObj_t *nv, const char *format) { - fprintf_P(stderr, format, nv->group, nv->token, nv->group, (uint8_t)nv->value); +void set_mvel(int index, uint16_t value) { } -static void _print_motor_flt_units(nvObj_t *nv, const char *format, uint8_t units) { - fprintf_P(stderr, format, nv->group, nv->token, nv->group, nv->value, GET_TEXT_ITEM(msg_units, units)); +void set_mjerk(int index, uint16_t value) { } - - -static void _print_motor_flt(nvObj_t *nv, const char *format) { - fprintf_P(stderr, format, nv->group, nv->token, nv->group, nv->value); -} - -static void _print_motor_pwr(nvObj_t *nv, const char *format) { - fprintf_P(stderr, format, nv->token[0], nv->value); -} - - -void st_print_ma(nvObj_t *nv) {_print_motor_ui8(nv, fmt_0ma);} -void st_print_sa(nvObj_t *nv) {_print_motor_flt_units(nv, fmt_0sa, DEGREE_INDEX);} -void st_print_tr(nvObj_t *nv) {_print_motor_flt_units(nv, fmt_0tr, cm_get_units_mode(MODEL));} -void st_print_mi(nvObj_t *nv) {_print_motor_ui8(nv, fmt_0mi);} -void st_print_po(nvObj_t *nv) {_print_motor_ui8(nv, fmt_0po);} -void st_print_pm(nvObj_t *nv) {_print_motor_ui8(nv, fmt_0pm);} -void st_print_pl(nvObj_t *nv) {_print_motor_flt(nv, fmt_0pl);} -void st_print_pwr(nvObj_t *nv){_print_motor_pwr(nv, fmt_pwr);} - -#endif // __TEXT_MODE diff --git a/src/stepper.h b/src/stepper.h index f388c44..ddb14bd 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -25,47 +25,61 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ + /* - * Coordinated motion (line drawing) is performed using a classic Bresenham DDA. - * A number of additional steps are taken to optimize interpolation and pulse train - * timing accuracy to minimize pulse jitter and make for very smooth motion and surface - * finish. - * - * - The DDA is not used as a 'ramp' for acceleration management. Accel is computed - * upstream in the motion planner as 3rd order (controlled jerk) equations. These - * generate accel/decel segments that are passed to the DDA for step output. - * - * - The DDA accepts and processes fractional motor steps as floating point nuymbers - * from the planner. Steps do not need to be whole numbers, and are not expected to be. - * The step values are converted to integer by multiplying by a fixed-point precision - * (DDA_SUBSTEPS, 100000). Rounding is performed to avoid a truncation bias. - * - * - Constant Rate DDA clock: The DDA runs at a constant, maximum rate for every - * segment regardless of actual step rate required. This means that the DDA clock - * is not tuned to the step rate (or a multiple) of the major axis, as is typical - * for most DDAs. Running the DDA flat out might appear to be "wasteful", but it ensures - * that the best aliasing results are achieved, and is part of maintaining step accuracy + * Coordinated motion (line drawing) is performed using a classic + * Bresenham DDA. A number of additional steps are taken to + * optimize interpolation and pulse train timing accuracy to + * minimize pulse jitter and make for very smooth motion and + * surface finish. + * + * - The DDA is not used as a 'ramp' for acceleration + * management. Accel is computed upstream in the motion planner + * as 3rd order (controlled jerk) equations. These generate + * accel/decel segments that are passed to the DDA for step + * output. + * + * - The DDA accepts and processes fractional motor steps as + * floating point numbers from the planner. Steps do not need + * to be whole numbers, and are not expected to be. The step + * values are converted to integer by multiplying by a + * fixed-point precision (DDA_SUBSTEPS, 100000). Rounding is + * performed to avoid a truncation bias. + * + * - Constant Rate DDA clock: The DDA runs at a constant, maximum + * rate for every segment regardless of actual step rate + * required. This means that the DDA clock is not tuned to the + * step rate (or a multiple) of the major axis, as is typical + * for most DDAs. Running the DDA flat out might appear to be + * "wasteful", but it ensures that the best aliasing results + * are achieved, and is part of maintaining step accuracy * across motion segments. * - * The observation is that TinyG is a hard real-time system in which every clock cycle - * is knowable and can be accounted for. So if the system is capable of sustaining - * max pulse rate for the fastest move, it's capable of sustaining this rate for any - * move. So we just run it flat out and get the best pulse resolution for all moves. - * If we were running from batteries or otherwise cared about the energy budget we + * The observation is that TinyG is a hard real-time system in + * which every clock cycle is knowable and can be accounted + * for. So if the system is capable of sustaining max pulse + * rate for the fastest move, it's capable of sustaining this + * rate for any move. So we just run it flat out and get the + * best pulse resolution for all moves. If we were running + * from batteries or otherwise cared about the energy budget we * might not be so cavalier about this. * - * At 50 KHz constant clock rate we have 20 uSec between pulse timer (DDA) interrupts. - * On the Xmega we consume <10 uSec in the interrupt - a whopping 50% of available cycles - * going into pulse generation. - * - * - Pulse timing is also helped by minimizing the time spent loading the next move - * segment. The time budget for the load is less than the time remaining before the - * next DDA clock tick. This means that the load must take < 10 uSec or the time - * between pulses will stretch out when changing segments. This does not affect - * positional accuracy but it would affect jitter and smoothness. To this end as much - * as possible about that move is pre-computed during move execution (prep cycles). - * Also, all moves are loaded from the DDA interrupt level (HI), avoiding the need - * for mutual exclusion locking or volatiles (which slow things down). + * At 50 KHz constant clock rate we have 20 uSec between pulse + * timer (DDA) interrupts. On the Xmega we consume <10 uSec in + * the interrupt - a whopping 50% of available cycles going + * into pulse generation. + * + * - Pulse timing is also helped by minimizing the time spent + * loading the next move segment. The time budget for the load + * is less than the time remaining before the next DDA clock + * tick. This means that the load must take < 10 uSec or the + * time between pulses will stretch out when changing + * segments. This does not affect positional accuracy but it + * would affect jitter and smoothness. To this end as much as + * possible about that move is pre-computed during move + * execution (prep cycles). Also, all moves are loaded from + * the DDA interrupt level (HI), avoiding the need for mutual + * exclusion locking or volatiles (which slow things down). */ /* @@ -200,7 +214,7 @@ * 3b If the steppers are not running this will set a timer to cause an * EXEC "software interrupt" that will ultimately call st_exec_move(). * - * 4 At this point a call to _exec_move() is made, either by the + * 4 At this point a call to _exec_move() is made, either by the * software interrupt from 3b, or once the steppers finish running * the current segment and have loaded the next segment. In either * case the call is initated via the EXEC software interrupt which @@ -243,8 +257,11 @@ * degrees of phase angle results in a step being generated. */ -#ifndef STEPPER_H_ONCE -#define STEPPER_H_ONCE +#ifndef STEPPER_H +#define STEPPER_H + +#include "config.h" +#include "status.h" // See hardware.h for platform specific stepper definitions @@ -257,8 +274,8 @@ enum prepBufferState { // Currently there is no distinction between IDLE and OFF (DEENERGIZED) // In the future IDLE will be powered at a low, torque-maintaining current enum motorPowerState { // used w/start and stop flags to sequence motor power - MOTOR_OFF = 0, // motor is stopped and deenergized - MOTOR_IDLE, // motor is stopped and may be partially energized for torque maintenance + MOTOR_OFF = 0, // motor stopped and deenergized + MOTOR_IDLE, // motor stopped and may be partially energized MOTOR_RUNNING, // motor is running (and fully energized) MOTOR_POWER_TIMEOUT_START, // transitional state to start power-down timeout MOTOR_POWER_TIMEOUT_COUNTDOWN // count down the time to de-energizing motors @@ -269,16 +286,16 @@ enum cmMotorPowerMode { MOTOR_DISABLED = 0, // motor enable is deactivated MOTOR_ALWAYS_POWERED, // motor is always powered while machine is ON MOTOR_POWERED_IN_CYCLE, // motor fully powered during cycles, de-powered out of cycle - MOTOR_POWERED_ONLY_WHEN_MOVING, // motor only powered while moving - idles shortly after it's stopped - even in cycle + MOTOR_POWERED_ONLY_WHEN_MOVING, // idles shortly after it's stopped - even in cycle MOTOR_POWER_MODE_MAX_VALUE // for input range checking }; // Min/Max timeouts allowed for motor disable. Allow for inertial stop; must be non-zero -#define MOTOR_TIMEOUT_SECONDS_MIN (float)0.1 // seconds !!! SHOULD NEVER BE ZERO !!! -#define MOTOR_TIMEOUT_SECONDS_MAX (float)4294967 // (4294967295/1000) -- for conversion to uint32_t -#define MOTOR_TIMEOUT_SECONDS (float)0.1 // seconds in DISABLE_AXIS_WHEN_IDLE mode -#define MOTOR_TIMEOUT_WHEN_MOVING (float)0.25 // timeout for a motor in _ONLY_WHEN_MOVING mode +#define MOTOR_TIMEOUT_SECONDS_MIN (float)0.1 // seconds !!! SHOULD NEVER BE ZERO !!! +#define MOTOR_TIMEOUT_SECONDS_MAX (float)4294967 // (4294967295/1000) -- for conversion to uint32_t +#define MOTOR_TIMEOUT_SECONDS (float)0.1 // seconds in DISABLE_AXIS_WHEN_IDLE mode +#define MOTOR_TIMEOUT_WHEN_MOVING (float)0.25 // timeout for a motor in _ONLY_WHEN_MOVING mode /* DDA substepping * DDA Substepping is a fixed.point scheme to increase the resolution of the DDA pulse generation @@ -286,11 +303,11 @@ enum cmMotorPowerMode { * results in more precise pulse timing and therefore less pulse jitter and smoother motor operation. * * The DDA accumulator is an int32_t, so the accumulator has the number range of about 2.1 billion. - * The DDA_SUBSTEPS is used to multiply the step count for a segment to maximally use this number range. - * DDA_SUBSTEPS can be computed for a given DDA clock rate and segment time not to exceed the available + * The DDA_SUBSTEPS is used to multiply step count for a segment to maximally use this number range. + * DDA_SUBSTEPS can be computed for a given DDA clock rate and segment time not to exceed available * number range. Variables are: * - * MAX_LONG == 2^31, maximum signed long (depth of accumulator. NB: accumulator values are negative) + * MAX_LONG == 2^31, maximum signed long (depth of accumulator. NB: values are negative) * FREQUENCY_DDA == DDA clock rate in Hz. * NOM_SEGMENT_TIME == upper bound of segment time in minutes * 0.90 == a safety factor used to reduce the result from theoretical maximum @@ -304,15 +321,15 @@ enum cmMotorPowerMode { * Step correction settings determine how the encoder error is fed back to correct position errors. * Since the following_error is running 2 segments behind the current segment you have to be careful * not to overcompensate. The threshold determines if a correction should be applied, and the factor - * is how much. The holdoff is how many segments to wait before applying another correction. If threshold + * is how much. The holdoff is how many segments before applying another correction. If threshold * is too small and/or amount too large and/or holdoff is too small you may get a runaway correction * and error will grow instead of shrink (or oscillate). */ -#define STEP_CORRECTION_THRESHOLD (float)2.00 // magnitude of forwarding error to apply correction (in steps) -#define STEP_CORRECTION_FACTOR (float)0.25 // factor to apply to step correction for a single segment -#define STEP_CORRECTION_MAX (float)0.60 // max step correction allowed in a single segment -#define STEP_CORRECTION_HOLDOFF 5 // minimum number of segments to wait between error correction -#define STEP_INITIAL_DIRECTION DIRECTION_CW +#define STEP_CORRECTION_THRESHOLD (float)2.00 // magnitude of forwarding error (in steps) +#define STEP_CORRECTION_FACTOR (float)0.25 // apply to step correction for a single segment +#define STEP_CORRECTION_MAX (float)0.60 // max step correction allowed in a single segment +#define STEP_CORRECTION_HOLDOFF 5 // minimum wait between error correction +#define STEP_INITIAL_DIRECTION DIRECTION_CW /* @@ -333,26 +350,20 @@ enum cmMotorPowerMode { */ // Motor config structure -typedef struct cfgMotor { // per-motor configs - // public - uint8_t motor_map; // map motor to axis - uint32_t microsteps; // microsteps to apply for each axis (ex: 8) - uint8_t polarity; // 0=normal polarity, 1=reverse motor direction - uint8_t power_mode; // See cmMotorPowerMode for enum - float power_level; // set 0.000 to 1.000 for PMW vref setting - float step_angle; // degrees per whole step (ex: 1.8) - float travel_rev; // mm or deg of travel per motor revolution - float steps_per_unit; // microsteps per mm (or degree) of travel - float units_per_step; // mm or degrees of travel per microstep - - // private - float power_level_scaled; // scaled to internal range - must be between 0 and 1 +typedef struct cfgMotor { // per-motor configs + uint8_t motor_map; // map motor to axis + uint32_t microsteps; // microsteps to apply for each axis (ex: 8) + uint8_t polarity; // 0=normal polarity, 1=reverse motor direction + uint8_t power_mode; // See cmMotorPowerMode for enum + float step_angle; // degrees per whole step (ex: 1.8) + float travel_rev; // mm or deg of travel per motor revolution + float steps_per_unit; // microsteps per mm (or degree) of travel } cfgMotor_t; -typedef struct stConfig { // stepper configs - float motor_power_timeout; // seconds before setting motors to idle current (currently this is OFF) - cfgMotor_t mot[MOTORS]; // settings for motors 1-N +typedef struct stConfig { // stepper configs + float motor_power_timeout; // seconds before set to idle current (currently this is OFF) + cfgMotor_t mot[MOTORS]; // settings for motors 1-N } stConfig_t; @@ -366,11 +377,9 @@ typedef struct stRunMotor { // one per controlled motor typedef struct stRunSingleton { // Stepper static values and axis parameters - uint16_t magic_start; // magic number to test memory integrity uint32_t dda_ticks_downcount; // tick down-counter (unscaled) uint32_t dda_ticks_X_substeps; // ticks multiplied by scaling factor stRunMotor_t mot[MOTORS]; // runtime motor structures - uint16_t magic_end; } stRunSingleton_t; @@ -386,18 +395,16 @@ typedef struct stPrepMotor { // following error correction int32_t correction_holdoff; // count down segments between corrections - float corrected_steps; // accumulated correction steps for the cycle (for diagnostic display only) + float corrected_steps; // accumulated correction steps for the cycle (diagnostic) // accumulator phase correction float prev_segment_time; // segment time from previous segment run for this motor float accumulator_correction; // factor for adjusting accumulator between segments uint8_t accumulator_correction_flag;// signals accumulator needs correction - } stPrepMotor_t; typedef struct stPrepSingleton { - uint16_t magic_start; // magic number to test memory integrity volatile uint8_t buffer_state; // prep buffer state - owned by exec or loader struct mpBuffer *bf; // static pointer to relevant buffer uint8_t move_type; // move type @@ -406,27 +413,21 @@ typedef struct stPrepSingleton { uint32_t dda_ticks; // DDA or dwell ticks for the move uint32_t dda_ticks_X_substeps; // DDA ticks scaled by substep factor stPrepMotor_t mot[MOTORS]; // prep time motor structs - uint16_t magic_end; } stPrepSingleton_t; extern stConfig_t st_cfg; // config struct is exposed. The rest are private -extern stPrepSingleton_t st_pre; // only used by config_app diagnostics +extern stPrepSingleton_t st_pre; // used by planner void stepper_init(); -void stepper_init_assertions(); -stat_t stepper_test_assertions(); - uint8_t st_runtime_isbusy(); void st_reset(); void st_cycle_start(); void st_cycle_end(); -stat_t st_clc(nvObj_t *nv); void st_energize_motors(); void st_deenergize_motors(); -void st_set_motor_power(const uint8_t motor); stat_t st_motor_power_callback(); void st_request_exec_move(); @@ -435,45 +436,4 @@ void st_prep_command(void *bf); // use a void pointer since we don't know void st_prep_dwell(float microseconds); stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time); -stat_t st_set_sa(nvObj_t *nv); -stat_t st_set_tr(nvObj_t *nv); -stat_t st_set_mi(nvObj_t *nv); -stat_t st_set_pm(nvObj_t *nv); -stat_t st_set_pl(nvObj_t *nv); -stat_t st_get_pwr(nvObj_t *nv); - -stat_t st_set_mt(nvObj_t *nv); -stat_t st_set_md(nvObj_t *nv); -stat_t st_set_me(nvObj_t *nv); - -#ifdef __TEXT_MODE - -void st_print_ma(nvObj_t *nv); -void st_print_sa(nvObj_t *nv); -void st_print_tr(nvObj_t *nv); -void st_print_mi(nvObj_t *nv); -void st_print_po(nvObj_t *nv); -void st_print_pm(nvObj_t *nv); -void st_print_pl(nvObj_t *nv); -void st_print_pwr(nvObj_t *nv); -void st_print_mt(nvObj_t *nv); -void st_print_me(nvObj_t *nv); -void st_print_md(nvObj_t *nv); - -#else - -#define st_print_ma tx_print_stub -#define st_print_sa tx_print_stub -#define st_print_tr tx_print_stub -#define st_print_mi tx_print_stub -#define st_print_po tx_print_stub -#define st_print_pm tx_print_stub -#define st_print_pl tx_print_stub -#define st_print_pwr tx_print_stub -#define st_print_mt tx_print_stub -#define st_print_me tx_print_stub -#define st_print_md tx_print_stub - -#endif // __TEXT_MODE - -#endif // STEPPER_H_ONCE +#endif // STEPPER_H diff --git a/src/switch.c b/src/switch.c index 50f88d3..957993e 100644 --- a/src/switch.c +++ b/src/switch.c @@ -40,27 +40,23 @@ * and lockout subsequent interrupts for the defined lockout period. Ditto on the method. */ -#include "tinyg.h" -#include "config.h" #include "switch.h" + #include "hardware.h" #include "canonical_machine.h" -#include "text_parser.h" +#include "config.h" #include +#include static void _switch_isr_helper(uint8_t sw_num); /* - * switch_init() - initialize homing/limit switches + * Initialize homing/limit switches * * This function assumes sys_init() and st_init() have been run previously to - * bind the ports and set bit IO directions, repsectively. See system.h for details - */ -/* Note: v7 boards have external strong pullups on GPIO2 pins (2.7K ohm). - * v6 and earlier use internal pullups only. Internal pullups are set - * regardless of board type but are extraneous for v7 boards. + * bind the ports and set bit IO directions, repsectively. */ #define PIN_MODE PORT_OPC_PULLUP_gc // pin mode. see iox192a3.h for details @@ -85,23 +81,30 @@ void switch_init() { hw.sw_port[i]->INTCTRL = GPIO1_INTLVL; // see gpio.h for setting } + // Defaults + sw.switch_type = SWITCH_TYPE; + sw.mode[0] = X_SWITCH_MODE_MIN; + sw.mode[1] = X_SWITCH_MODE_MAX; + sw.mode[2] = Y_SWITCH_MODE_MIN; + sw.mode[3] = Y_SWITCH_MODE_MAX; + sw.mode[4] = Z_SWITCH_MODE_MIN; + sw.mode[5] = Z_SWITCH_MODE_MAX; + sw.mode[6] = A_SWITCH_MODE_MIN; + sw.mode[7] = A_SWITCH_MODE_MAX; + reset_switches(); } /* - * Switch closure processing routines - * - * ISRs - switch interrupt handler vectors - * _isr_helper() - common code for all switch ISRs - * switch_rtc_callback() - called from RTC for each RTC tick. - * * These functions interact with each other to process switch closures and firing. * Each switch has a counter which is initially set to negative SW_DEGLITCH_TICKS. * When a switch closure is DETECTED the count increments for each RTC tick. * When the count reaches zero the switch is tripped and action occurs. * The counter continues to increment positive until the lockout is exceeded. */ + +// Switch interrupt handler vectors ISR(X_MIN_ISR_vect) {_switch_isr_helper(SW_MIN_X);} ISR(Y_MIN_ISR_vect) {_switch_isr_helper(SW_MIN_Y);} ISR(Z_MIN_ISR_vect) {_switch_isr_helper(SW_MIN_Z);} @@ -113,29 +116,32 @@ ISR(A_MAX_ISR_vect) {_switch_isr_helper(SW_MAX_A);} static void _switch_isr_helper(uint8_t sw_num) { - if (sw.mode[sw_num] == SW_MODE_DISABLED) return; // this is never supposed to happen - if (sw.debounce[sw_num] == SW_LOCKOUT) return; // exit if switch is in lockout + if (sw.mode[sw_num] == SW_MODE_DISABLED) return; // this is never supposed to happen + if (sw.debounce[sw_num] == SW_LOCKOUT) return; // exit if switch is in lockout - sw.debounce[sw_num] = SW_DEGLITCHING; // either transitions state from IDLE or overwrites it - sw.count[sw_num] = -SW_DEGLITCH_TICKS; // reset deglitch count regardless of entry state + sw.debounce[sw_num] = SW_DEGLITCHING; // either transitions state from IDLE or overwrites it + sw.count[sw_num] = -SW_DEGLITCH_TICKS; // reset deglitch count regardless of entry state - read_switch(sw_num); // sets the state value in the struct + read_switch(sw_num); // sets the state value in the struct } +/// Called from RTC for each RTC tick void switch_rtc_callback() { for (uint8_t i = 0; i < NUM_SWITCHES; i++) { if (sw.mode[i] == SW_MODE_DISABLED || sw.debounce[i] == SW_IDLE) continue; - if (++sw.count[i] == SW_LOCKOUT_TICKS) { // state is either lockout or deglitching + if (++sw.count[i] == SW_LOCKOUT_TICKS) { // state is either lockout or deglitching sw.debounce[i] = SW_IDLE; + // check if the state has changed while we were in lockout... uint8_t old_state = sw.state[i]; - if(old_state != read_switch(i)) { + if (old_state != read_switch(i)) { sw.debounce[i] = SW_DEGLITCHING; sw.count[i] = -SW_DEGLITCH_TICKS; } + continue; } @@ -195,36 +201,10 @@ uint8_t read_switch(uint8_t sw_num) { case SW_MAX_A: read = hw.sw_port[AXIS_A]->IN & SW_MAX_BIT_bm; break; } + // A NO switch drives the pin LO when thrown if (sw.switch_type == SW_TYPE_NORMALLY_OPEN) - sw.state[sw_num] = (read == 0) ? SW_CLOSED : SW_OPEN; // confusing. An NO switch drives the pin LO when thrown + sw.state[sw_num] = (read == 0) ? SW_CLOSED : SW_OPEN; else sw.state[sw_num] = (read != 0) ? SW_CLOSED : SW_OPEN; return sw.state[sw_num]; } - - -stat_t sw_set_st(nvObj_t *nv) { // switch type (global) - set_01(nv); - switch_init(); - - return STAT_OK; -} - - -stat_t sw_set_sw(nvObj_t *nv) { // switch setting - if (nv->value > SW_MODE_MAX_VALUE) - return STAT_INPUT_VALUE_RANGE_ERROR; - - set_ui8(nv); - switch_init(); - - return STAT_OK; -} - - -#ifdef __TEXT_MODE - -static const char fmt_st[] PROGMEM = "[st] switch type%18d [0=NO,1=NC]\n"; -void sw_print_st(nvObj_t *nv) {text_print_ui8(nv, fmt_st);} - -#endif diff --git a/src/switch.h b/src/switch.h index ba8cd55..7e567ef 100644 --- a/src/switch.h +++ b/src/switch.h @@ -37,8 +37,10 @@ * * Read switch contains the results of read pin and manages edges and debouncing. */ -#ifndef SWITCH_H_ONCE -#define SWITCH_H_ONCE +#ifndef SWITCH_H +#define SWITCH_H + +#include // timer for debouncing switches #define SW_LOCKOUT_TICKS 25 // 25=250ms. RTC ticks are ~10ms each @@ -61,7 +63,7 @@ enum swType { enum swState { SW_DISABLED = -1, SW_OPEN = 0, // also read as 'false' - SW_CLOSED // also read as 'true' + SW_CLOSED // also read as 'true' }; // macros for finding the index into the switch table give the axis number @@ -98,7 +100,7 @@ enum swNums { // indexes into switch arrays //#define GPIO1_INTLVL (PORT_INT0LVL_LO_gc|PORT_INT1LVL_LO_gc) // shouldn;t be low // port assignments for vectors -#define X_MIN_ISR_vect PORTA_INT0_vect // these must line up with the SWITCH assignments in system.h +#define X_MIN_ISR_vect PORTA_INT0_vect #define Y_MIN_ISR_vect PORTD_INT0_vect #define Z_MIN_ISR_vect PORTE_INT0_vect #define A_MIN_ISR_vect PORTF_INT0_vect @@ -137,14 +139,4 @@ void sw_show_switch(); void set_switch_type(uint8_t switch_type); uint8_t get_switch_type(); -// Switch config accessors and text functions -stat_t sw_set_st(nvObj_t *nv); -stat_t sw_set_sw(nvObj_t *nv); - -#ifdef __TEXT_MODE -void sw_print_st(nvObj_t *nv); -#else -#define sw_print_st tx_print_stub -#endif // __TEXT_MODE - -#endif // End of include guard: SWITCH_H_ONCE +#endif // SWITCH_H diff --git a/src/test.c b/src/test.c deleted file mode 100644 index cea709a..0000000 --- a/src/test.c +++ /dev/null @@ -1,89 +0,0 @@ -/* - * test.c - tinyg test sets - * Part of TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart Jr. - * - * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include "tinyg.h" // #1 -#include "config.h" // #2 -#include "controller.h" -#include "planner.h" -#include "test.h" -#include "util.h" - -// regression test files -#ifdef __CANNED_TESTS - -#include "tests/test_001_smoke.h" // basic functionality -#include "tests/test_002_homing.h" // G28.1 homing cycles -#include "tests/test_003_squares.h" // square moves -#include "tests/test_004_arcs.h" // arc moves -#include "tests/test_005_dwell.h" // dwells embedded in move sequences -#include "tests/test_006_feedhold.h" // feedhold - requires manual ! and ~ entry -#include "tests/test_007_Mcodes.h" // M codes synchronized w/moves (planner queue) -#include "tests/test_008_json.h" // JSON parser and IO -#include "tests/test_009_inverse_time.h" // inverse time mode -#include "tests/test_010_rotary.h" // ABC axes -#include "tests/test_011_small_moves.h" // small move test -#include "tests/test_012_slow_moves.h" // slow move test -#include "tests/test_013_coordinate_offsets.h" // what it says -#include "tests/test_014_microsteps.h" // test all microstep settings -#include "tests/test_050_mudflap.h" // mudflap test - entire drawing -#include "tests/test_051_braid.h" // braid test - partial drawing - -#endif - -#ifdef __TEST_99 -#include "tests/test_099.h" // diagnostic test file. used to diagnose specific issues -#endif - -/* - * run_test() - system tests from FLASH invoked by $test=n command - * - * By convention the character array containing the test must have the same - * name as the file name. - */ -uint8_t run_test(nvObj_t *nv) { - switch ((uint8_t)nv->value) { - case 0: return STAT_OK; -#ifdef __CANNED_TESTS - case 1: test_open(PGMFILE(&test_smoke),PGM_FLAGS); break; - case 2: test_open(PGMFILE(&test_homing),PGM_FLAGS); break; - case 3: test_open(PGMFILE(&test_squares),PGM_FLAGS); break; - case 4: test_open(PGMFILE(&test_arcs),PGM_FLAGS); break; - case 5: test_open(PGMFILE(&test_dwell),PGM_FLAGS); break; - case 6: test_open(PGMFILE(&test_feedhold),PGM_FLAGS); break; - case 7: test_open(PGMFILE(&test_Mcodes),PGM_FLAGS); break; - case 8: test_open(PGMFILE(&test_json),PGM_FLAGS); break; - case 9: test_open(PGMFILE(&test_inverse_time),PGM_FLAGS); break; - case 10: test_open(PGMFILE(&test_rotary),PGM_FLAGS); break; - case 11: test_open(PGMFILE(&test_small_moves),PGM_FLAGS); break; - case 12: test_open(PGMFILE(&test_slow_moves),PGM_FLAGS); break; - case 13: test_open(PGMFILE(&test_coordinate_offsets),PGM_FLAGS); break; - case 14: test_open(PGMFILE(&test_microsteps),PGM_FLAGS); break; - case 50: test_open(PGMFILE(&test_mudflap),PGM_FLAGS); break; - case 51: test_open(PGMFILE(&test_braid),PGM_FLAGS); break; -#endif -#ifdef __TEST_99 - case 99: test_open(PGMFILE(&test_99),PGM_FLAGS); break; -#endif - default: - fprintf_P(stderr,PSTR("Test #%d not found\n"),(uint8_t)nv->value); - return STAT_ERROR; - } - - return STAT_OK; -} diff --git a/src/test.h b/src/test.h deleted file mode 100644 index 99ce20b..0000000 --- a/src/test.h +++ /dev/null @@ -1,126 +0,0 @@ -/* - * test.h - tinyg test sets - * Part of TinyG project - * - * Copyright (c) 2010 - 2013 Alden S. Hart Jr. - * - * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -#ifndef test_h -#define test_h - -uint8_t run_test(nvObj_t *nv); - -/***** DEBUG support ****** - * - * DEBUGs are print statements you probably only want enabled during - * debugging, and then probably only for one section of code or another. - * - * DEBUG logging is enabled if __DEBUG is defined. - * __DEBUG enables a set of arbitrary __dbXXXXXX defines that control - * various debug regions, e.g. __dbCONFIG to enable debugging in config.c. - * Each __dbXXXXXX pairs with a dbXXXXXX global variable used as a flag. - * Each dbXXXXXX is initialized to TRUE or FALSE at startup in main.c. - * dbXXXXXX is used as a condition to enable or disable logging. - * No varargs, so you must use the one with the right number of variables. - * A closing semicolon is not required but is recommended for style. - * - * DEBUG usage examples: - * DEBUG0(dbCONFIG, PSTR("String with no variables")); - * DEBUG1(dbCONFIG, PSTR("String with one variable: %f"), float_var); - * DEBUG2(dbCONFIG, PSTR("String with two variables: %4.2f, %d"), float_var, int_var); - * - * DEBUG print statements are coded so they occupy no program space if - * they are not enabled. If you also use __dbXXXX defines to enable debug - * code these will - of course - be in the code regardless. - * - * There are also a variety of module-specific diagnostic print statements - * that are enabled or not depending on whether __DEBUG is defined - */ - -#ifdef __DEBUG -void dump_everything(); -void roll_over_and_die(); -void print_scalar(const char *label, float value); -void print_vector(const char *label, float vector[], uint8_t length); - -// global allocation of debug control variables - uint8_t dbECHO_GCODE_BLOCK; - uint8_t dbALINE_CALLED; - uint8_t dbSHOW_QUEUED_LINE; - uint8_t dbSHOW_LIMIT_SWITCH; - uint8_t dbSHOW_CONFIG_STATE; - uint8_t dbCONFIG_DEBUG_ENABLED; - uint8_t dbSHOW_LOAD_MOVE; - -#define DEBUG0(dbXXXXXX,msg) { if (dbXXXXXX == TRUE) { \ - fprintf_P(stderr,PSTR("DEBUG: ")); \ - fprintf_P(stderr,msg); \ - fprintf_P(stderr,PSTR("\n"));}} - -#define DEBUG1(dbXXXXXX,msg,a) { if (dbXXXXXX == TRUE) { \ - fprintf_P(stderr,PSTR("DEBUG: ")); \ - fprintf_P(stderr,msg,a); \ - fprintf_P(stderr,PSTR("\n"));}} - -#define DEBUG2(dbXXXXXX,msg,a,b) { if (dbXXXXXX == TRUE) { \ - fprintf_P(stderr,PSTR("DEBUG: ")); \ - fprintf_P(stderr,msg,a,b); \ - fprintf_P(stderr,PSTR("\n"));}} - -#define DEBUG3(dbXXXXXX,msg,a,b,c) { if (dbXXXXXX == TRUE) { \ - fprintf_P(stderr,PSTR("DEBUG: ")); \ - fprintf_P(stderr,msg,a,b,c); \ - fprintf_P(stderr,PSTR("\n"));}} -#else -#define DEBUG0(dbXXXXXX,msg) -#define DEBUG1(dbXXXXXX,msg,a) -#define DEBUG2(dbXXXXXX,msg,a,b) -#define DEBUG3(dbXXXXXX,msg,a,b,c) -#endif // __DEBUG - -/***** Runtime Segment Data Logger Stuff ***** - * - * This is independent of __DEBUG and does not need __DEBUG defined - */ -#ifdef __SEGMENT_LOGGER -#define SEGMENT_LOGGER_MAX 256 - -// segment logger structure and index -struct mpSegmentLog { - uint8_t move_state; - uint32_t linenum; - uint32_t segments; - float velocity; - float microseconds; -}; -struct mpSegmentLog sl[SEGMENT_LOGGER_MAX]; -uint16_t sl_index; - -// function prototype and calling macro -void segment_logger(uint8_t move_state, - uint32_t linenum, - uint32_t segments, - uint32_t segment_count, - float velocity, - float microseconds - ); - -#define SEGMENT_LOGGER segment_logger(bf->move_state, \ - mr.linenum, mr.segments, mr.segment_count, \ - mr.segment_velocity, \ - mr.microseconds); -#else -#define SEGMENT_LOGGER -#endif // __SEGMENT_LOGGER -#endif // test_h diff --git a/src/tests/test_001_smoke.h b/src/tests/test_001_smoke.h deleted file mode 100644 index c303d78..0000000 --- a/src/tests/test_001_smoke.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * test_001_smoke.h - * - * This test checks basic functionality: - * - motor 1 CW at full speed ~3 seconds - * - motor 1 CCW at full speed ~3 seconds - * - motor 2 CW at full speed ~3 seconds - * - motor 2 CCW at full speed ~3 seconds - * - motor 3 CW at full speed ~3 seconds - * - motor 3 CCW at full speed ~3 seconds - * - motor 4 CW at full speed ~3 seconds - * - motor 4 CCW at full speed ~3 seconds - * - all motors CW at full speed ~3 seconds - * - all motors CCW at full speed ~3 seconds - * - all motors CW at medium speed ~3 seconds - * - all motors CCW at medium speed ~3 seconds - * - all motors CW at slow speed ~3 seconds - * - all motors CCW at slow speed ~3 seconds - * - light LEDs 1,2 and 4 in sequence for about 1 second each: - * - short finishing move - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - */ -const char test_smoke[] PROGMEM = "\ -(MSG**** Smoke Test [v1] ****)\n\ -G00 G17 G21 G40 G49 G80 G90\n\ -m3g4p1\n\ -m5g4p1\n\ -m4g4p1\n\ -m3g4p1\n\ -m5g4p1\n\ -m7g4p1\n\ -m9g4p1\n\ -m8g4p1\n\ -m9\n\ -g0x0y0z0\n\ -g00 x20\n\ -x0\n\ -y20\n\ -y0\n\ -z20\n\ -z0\n\ -a20\n\ -a0\n\ -G00 x20 y20 z20 a20\n\ -G00 x0 y0 z0 a0\n\ -G01 f30 x2 y2 z2 a2\n\ -x0 y0 z0 a0\n\ -g0x1\n\ -g0x0\n\ -m2"; - -/* -G01 f200 x10 y10 z10 a10\n\ -x0 y0 z0 a0\n\ -*/ -/* -G02 f10000 x0 y0 z40 i27 j27\n\ -G03 f10000 x0 y0 z0 i27 j27\n\ -g0x0y0z0\n\ -*/ diff --git a/src/tests/test_002_homing.h b/src/tests/test_002_homing.h deleted file mode 100644 index 0cd902c..0000000 --- a/src/tests/test_002_homing.h +++ /dev/null @@ -1,13 +0,0 @@ -/* - * test_002_homing.h - * - * - Performs homing cycle in X, Y and Z - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - */ -const char test_homing[] PROGMEM = "\ -(MSG**** Homing Test [v1] ****)\n\ -g28.2x0y0z0\n\ -m30"; diff --git a/src/tests/test_003_squares.h b/src/tests/test_003_squares.h deleted file mode 100644 index 1bbd331..0000000 --- a/src/tests/test_003_squares.h +++ /dev/null @@ -1,62 +0,0 @@ -/* - * test_003_squares.h - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - */ -const char test_squares[] PROGMEM = "\ -(MSG**** Squares Motion Test [v1] ****)\n\ -g00g17g21g40g49g80g90\n\ -g0x0y0\n\ -g0x20 (traverse a 20mm square in X and Y)\n\ -y20\n\ -x0\n\ -y0\n\ -g1x10f500 (draw a 10mm square in X and Y at F500)\n\ -y10\n\ -x0\n\ -y0\n\ -g0z20 (traverse a 20mm square in X and Z)\n\ -x20\n\ -z0\n\ -x0\n\ -g1x20y20z20f500 (feed a 20mm diagonal in X, Y and Z)\n\ -x0y0z0\n\ -x40y40z40 (traverse a 40mm cube in X, Y and Z)\n\ -x40y0z0\n\ -x0y40z40\n\ -x40y40z0\n\ -x0y0z40\n\ -x0y40z0\n\ -x40y0z40\n\ -x0y0z0\n\ -g80\n\ -g0x0y0\n\ -g0x1\n\ -g0x0\n\ -m30"; - -/* -g0x40y40z-40 (traverse a 40mm cube in X, Y and Z)\n\ -x40y0z0\n\ -x0y40z-40\n\ -x40y40z0\n\ -x0y40z-40\n\ -x0y40z0\n\ -x40y0z-40\n\ -x0y0z0\n\ -g80\n\ -m30"; - -x0y0z0\n\ -x40y0z-40\n\ -x0y40z0\n\ -x40y40z-40\n\ -x40y0z0\n\ -x0y0z-40\n\ -x40y40z0\n\ -x0y40z-40\n\ -x0y0z0\n\ - -*/ diff --git a/src/tests/test_004_arcs.h b/src/tests/test_004_arcs.h deleted file mode 100644 index 0cd3871..0000000 --- a/src/tests/test_004_arcs.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * test_004_arcs.h - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - */ - -const char test_arcs[] PROGMEM = "\ -N1 g00g17g21g40g49g80g90\n\ -N10 g0x0y0\n\ -N20 f500\n\ -N30 g2x10y-10i10 (CW 270 degrees)\n\ -N40 g0x0y0\n\ -N50 g3x10y-10i10 (CCW 90 degrees)\n\ -N60 g0x0y0\n\ -N80 g2x20y0i10 (CW 180 degrees)\n\ -N70 g0x0y0\n\ -N80 g3x20y0i10 (CCW 180 degrees)\n\ -N90 g0x0y0\n\ -N99 F1200\n\ -N100 g2x0y0i20 (CW 360 degrees)\n\ -N110 g2x0y0i20 (CW 360 again)\n\ -N120 f700 (Change feed rate while in G2 motion mode)\n\ -N130 g2x0y0i20 (CW 360 3rd time)\n\ -N140 g2x0y0i20 (CW 360 4th time)\n\ -N150 f500\n\ -N160 g3x0y0z40i20 (CCW 360 degrees with linear travel)\n\ -N170 g3x0y0z0i20 (CCW 360 again)\n\ -N180 f700 (msg****Change feed rate while in G3 motion mode****)\n\ -N190 g3x0y0i20 (CCW 360 3rd time)\n\ -N200 g3x0y0i20 (CCW 360 4th time)\n\ -N210 (msg****G18 Eval test****)\n\ -N220 G1 X30.707 Z50.727 F500\n\ -N230 G18 G02 X67.738 Z23.617 R25 F250\n\ -N240 g80\n\ -N250 g0x0y0z0\n\ -N260 m30"; diff --git a/src/tests/test_005_dwell.h b/src/tests/test_005_dwell.h deleted file mode 100644 index eacd85a..0000000 --- a/src/tests/test_005_dwell.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * test_005_dwell.h - * - * Tests a 1 second dwell - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - */ -const char test_dwell[] PROGMEM = "\ -(MSG**** Dwell Test [v1] ****)\n\ -g00g17g21g90\n\ -g55\n\ -g0x0y0\n\ -f500\n\ -g0x10\n\ -g4p1\n\ -g0x20\n\ -g4p1\n\ -g0x10\n\ -g4p1\n\ -g0x00\n\ -g4p1\n\ -y5\n\ -g54\n\ -g0x0y0\n\ -m3\n\ -g4p1\n\ -g0x10y10\n\ -m5\n\ -g4p1\n\ -g0x20y20\n\ -m4\n\ -g4p1\n\ -g0x30y30\n\ -m8\n\ -g4p1\n\ -g0x40y40\n\ -m9\n\ -g4p1\n\ -g0x50y50\n\ -g0x0y0\n\ -m30"; diff --git a/src/tests/test_006_feedhold.h b/src/tests/test_006_feedhold.h deleted file mode 100644 index cc54358..0000000 --- a/src/tests/test_006_feedhold.h +++ /dev/null @@ -1,20 +0,0 @@ -/* - * test_006_feedhold.h - * - * Notes: - * - The character array should be the same name as the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - */ -const char test_feedhold[] PROGMEM = "\ -(MSG**** Feedhold Test [v1] ****)\n\ -(MSG**** Manually enter ! and ~ during movement to test ****)\n\ -g55\n\ -g0x0y0\n\ -g4p3\n\ -g00g17g21g40g49g80g90\n\ -f600\n\ -g0x100v76.765\n\ -x0y0\n\ -g54\n\ -g0x0y0\n\ -m30"; diff --git a/src/tests/test_007_Mcodes.h b/src/tests/test_007_Mcodes.h deleted file mode 100644 index 4a25c75..0000000 --- a/src/tests/test_007_Mcodes.h +++ /dev/null @@ -1,62 +0,0 @@ -/* - * test_007_Mcodes.h - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - * - * Turn the bits on and off in sequence so you can see the LEDs light in a chain - */ -const char test_Mcodes[] PROGMEM = "\ -(MSG**** Mcodes test [v1] ****)\n\ -g55\n\ -g0x0y0\n\ -m3\n\ -g4p1\n\ -m5\n\ -g4p1\n\ -m4\n\ -g4p1\n\ -m5\n\ -g4p1\n\ -m7\n\ -g4p1\n\ -m9\n\ -g4p1\n\ -m8\n\ -g4p1\n\ -m9\n\ -f500\n\ -g0x20\n\ -y20\n\ -m3\n\ -x0\n\ -m4\n\ -y0\n\ -m5\n\ -g1x10\n\ -m7\n\ -y10\n\ -m8\n\ -x0\n\ -m9\n\ -y0\n\ -g2x10y-10i10\n\ -m3\n\ -g0x0y0\n\ -m4\n\ -g3x10y-10i10\n\ -m5\n\ -g0x0y0\n\ -m7\n\ -g2x20y0i10\n\ -m8\n\ -g0x0y0\n\ -m9\n\ -g3x20y0i10\n\ -g0x0y0\n\ -g2x0y0i10\n\ -g3x0y0i10\n\ -g54\n\ -g0x0y0\n\ -m30"; diff --git a/src/tests/test_008_json.h b/src/tests/test_008_json.h deleted file mode 100644 index 2cc7fc4..0000000 --- a/src/tests/test_008_json.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * test_008_json.h - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - */ -const char test_json[] PROGMEM= "\ -{\"gc\":\"g00g17g21g40g49g80g90\"}\n\ -{\"gc\":\"g55\"}\n\ -{\"gc\":\"g0x0y0\"}\n\ -{\"gc\":\"f500\"}\n\ -{\"gc\":\"(MSGsquares)\"}\n\ -{\"gc\":\"g0x20\"}\n\ -{\"gc\":\"y20\"}\n\ -{\"gc\":\"x0\"}\n\ -{\"gc\":\"y0\"}\n\ -{\"gc\":\"g1x10\"}\n\ -{\"gc\":\"y10\"}\n\ -{\"gc\":\"x0\"}\n\ -{\"gc\":\"y0\"}\n\ -{\"gc\":\"(MSGcircles)\"}\n\ -{\"gc\":\"g2x10y-10i10\"}\n\ -{\"gc\":\"g0x0y0\"}\n\ -{\"gc\":\"g3x10y-10i10\"}\n\ -{\"gc\":\"g0x0y0\"}\n\ -{\"gc\":\"g2x20y0i10\"}\n\ -{\"gc\":\"g0x0y0\"}\n\ -{\"gc\":\"g3x20y0i10\"}\n\ -{\"gc\":\"g0x0y0\"}\n\ -{\"gc\":\"g2x0y0i10\"}\n\ -{\"gc\":\"g3x0y0i10\"}\n\ -{\"gc\":\"g54\"}\n\ -{\"gc\":\"g0x0y0\"}\n\ -{\"gc\":\"m30\"}"; diff --git a/src/tests/test_009_inverse_time.h b/src/tests/test_009_inverse_time.h deleted file mode 100644 index 83d2162..0000000 --- a/src/tests/test_009_inverse_time.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - * test_009_inverse_time.h - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - */ -const char test_inverse_time[] PROGMEM = "\ -(MSG**** Inverse Time Motion Test [v1] ****)\n\ -g00g17g21g40g49g80g90\n\ -g55\n\ -g0x0y0\n\ -g93\n\ -f0.01\n\ -g1x10\n\ -y10\n\ -z10\n\ -a10\n\ -x0\n\ -y0\n\ -z0\n\ -a0\n\ -g1x10y10z-10a10\n\ -g0x0y0z0a0\n\ -f0.1\n\ -g2x10y-10z-20i10\n\ -g0x0y0z0\n\ -g3x10y-10z-20i10\n\ -g0x0y0z0\n\ -g2x20y0z-20i10\n\ -g0x0y0z0\n\ -g3x20y0i10\n\ -g0x0y0z0\n\ -g2x0y0z-30i10\n\ -g3x0y0z0i10\n\ -g0x0y0z0\n\ -g2x0y0i20 (CW 360 degrees)\n\ -g3x0y0i20 (CCW 360 degrees)\n\ -g94\n\ -g54\n\ -g0x0y0\n\ -m30"; - - -/* -g93\n\ -f0.01\n\ -g1x10\n\ -y10\n\ -z10\n\ -z10\n\ -x0\n\ -y0\n\ -z0\n\ -a0\n\ -g1x10y10z10a10\n\ -g0x0y0z0a0\n\ -f0.1\n\ -g2x10y-10z20i10\n\ -g0x0y0z0\n\ -g3x10y-10z20i10\n\ -g0x0y0z0\n\ -g2x20y0z20i10\n\ -g0x0y0z0\n\ -g3x20y0i10\n\ -g0x0y0z0\n\ -g2x0y0z30i10\n\ -g3x0y0z0i10\n\ -g94\n\ -m30"; -*/ diff --git a/src/tests/test_010_rotary.h b/src/tests/test_010_rotary.h deleted file mode 100644 index 27ac8ca..0000000 --- a/src/tests/test_010_rotary.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - * test_010_rotary.h - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - */ -const char test_rotary[] PROGMEM = "\ -(MSG**** Rotary Axis Motion Test [v1] ****)\n\ -g00g17g21g40g49g80g90\n\ -g0x0y0z0a0b0c0\n\ -g55\n\ -g0x0y0\n\ -f36000\n\ -g0a360\n\ -b360\n\ -c360\n\ -a0\n\ -b0\n\ -c0\n\ -g1a1440\n\ -b1440\n\ -c1440\n\ -a0\n\ -b0\n\ -c0\n\ -g1a1440b720c360\n\ -x0y0z0a0b0c0\n\ -g1x60a1440b720c360\n\ -x0y0z0a0b0c0\n\ -g1x60y50a1440b720c360\n\ -x0y0z0a0b0c0\n\ -g1x60y50x25a1440b720c360\n\ -x0y0z0a0b0c0\n\ -g54\n\ -g0x0y0\n\ -m30"; diff --git a/src/tests/test_011_small_moves.h b/src/tests/test_011_small_moves.h deleted file mode 100644 index b379dbd..0000000 --- a/src/tests/test_011_small_moves.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - * test_011_small_moves.h - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - */ -const char test_small_moves[] PROGMEM = "\ -(MSG**** Test very short moves [v1] ****)\n\ -g0x0y0\n\ -g4p0.5\n\ -g1x0.1f1000\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -x0\n\ -x0.1\n\ -g4p0.5\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -x0\n\ -x0.01\n\ -g0x0y0\n\ -m2"; diff --git a/src/tests/test_012_slow_moves.h b/src/tests/test_012_slow_moves.h deleted file mode 100644 index bab6f8b..0000000 --- a/src/tests/test_012_slow_moves.h +++ /dev/null @@ -1,22 +0,0 @@ -/* - * test_012_slow_moves.h - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) -$si=3000\n\ -g1y5\n\ -x0\n\ -y0"; - */ - -const char test_slow_moves[] PROGMEM = "\ -(MSG**** Test accuracy of slow moves [v1] ****)\n\ -g55\n\ -g0x0y0\n\ -g4p0.5\n\ -g4p0.5\n\ -g1x10f22\n\ -g54\n\ -g0x0y0\n\ -m30"; diff --git a/src/tests/test_013_coordinate_offsets.h b/src/tests/test_013_coordinate_offsets.h deleted file mode 100644 index b874fb0..0000000 --- a/src/tests/test_013_coordinate_offsets.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * test_013_coordinate_offsets.h - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - */ -const char test_coordinate_offsets[] PROGMEM = "\ -(MSG**** Coordinate offsets test [v1] ****)\n\ -g00g17g21g40g49g80g90\n\ -g54\n\ -g92x0y0z0\n\ -f600\n\ -(MSG**** test G92 offsets****)\n\ -(msgStep 1: Move from 0,0 to 30,0 mm in positive X direction)\n\ -(msgStep 2: Reset origin to 0.0 using G92)\n\ -(msgStep 3: Move to 20,0. Should move 20 mm in positive X direction and status report should start from 0,0)\n\ -(msgStep 4: Move to 0.0 Should move 20 mm in negative X direction and status report should start from 20,0)\n\ -g1x30\n\ -g92x0\n\ -g1x20\n\ -g1x0y0\n\ -$sr\n\ -g92x0\n\ -g4p0.5\n\ -(MSG**** test G55 offsets****)\n\ -(msgStep 1: Set Coordinate system 2 [g55] to offset of 50,50)\n\ -(msgStep 2: Select G55 coordinate system)\n\ -(msgStep 3: Move to 0,0. Head should move diagonally in +X and +Y and status report should start from 0,0)\n\ -g10L2p2x50y50\n\ -g55\n\ -g0x0y0\n\ -g4p0.5\n\ -(MSG**** test G53 absolute overrides ****)\n\ -(msgStep 1: Run a square in G55 system)\n\ -(msgStep 2: Run a square w/G53 override)\n\ -g0x0y0\n\ -g0x20y0\n\ -g0x20y20\n\ -g0x0y20\n\ -g0x0y0\n\ -g4p0.5\n\ -g53g0x0y0\n\ -g53g0x20y0\n\ -g53g0x20y20\n\ -g53g0x0y20\n\ -g53g0x0y0\n\ -g0x0y0\n\ -g4p0.5\n\ -(MSG**** test Return to home ****)\n\ -(msgStep 1: Return to home using G28)\n\ -(msgStep 2: Reset to G54 coord system)\n\ -g28\n\ -g54\n\ -m30"; diff --git a/src/tests/test_014_microsteps.h b/src/tests/test_014_microsteps.h deleted file mode 100644 index 90084c0..0000000 --- a/src/tests/test_014_microsteps.h +++ /dev/null @@ -1,47 +0,0 @@ -/* - * test_014_coordinate_offsets.h - * - * Tests movement in 4 axes with all microatep settings. All moves should be the same length and time - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - */ -const char test_microsteps[] PROGMEM = "\ -(MSG**** Microstep Test [v1] ****)\n\ -G00 G17 G21 G40 G49 G80 G90\n\ -g0x0y0z0\n\ -{\"1mi\":1}\n\ -{\"2mi\":1}\n\ -{\"3mi\":1}\n\ -{\"4mi\":1}\n\ -g0 x10\n\ -{\"1mi\":2}\n\ -x0\n\ -{\"1mi\":4}\n\ -x10\n\ -{\"1mi\":8}\n\ -x0\n\ -g0 y10\n\ -{\"2mi\":2}\n\ -y0\n\ -{\"2mi\":4}\n\ -y10\n\ -{\"2mi\":8}\n\ -y0\n\ -g0 z10\n\ -{\"3mi\":2}\n\ -z0\n\ -{\"3mi\":4}\n\ -z10\n\ -{\"3mi\":8}\n\ -z0\n\ -g0 a10\n\ -{\"4mi\":2}\n\ -a0\n\ -{\"4mi\":4}\n\ -a10\n\ -{\"4mi\":8}\n\ -a0\n\ -m2"; - diff --git a/src/tests/test_050_mudflap.h b/src/tests/test_050_mudflap.h deleted file mode 100644 index fae9a24..0000000 --- a/src/tests/test_050_mudflap.h +++ /dev/null @@ -1,334 +0,0 @@ -/* - * test_011_mudflap.h - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - -N25 G92 X0 Y0 Z0\n\ -N30 X0.076 Y0.341\n\ - - */ -const char test_mudflap[] PROGMEM = "\ -N1 G20\n\ -N5 G40 G17\n\ -N10 T1 M06\n\ -(N15 G90 G0 X0 Y0 Z0)\n\ -N20 S5000 M03 \n\ -N25 X0.064 Y0.326 Z0\n\ -N40 G01 F16.0\n\ -N50 X0.064 Y0.326\n\ -N55 X0.060 Y0.293\n\ -N60 X0.077 Y0.267\n\ -N65 X0.111 Y0.257\n\ -N70 X0.149 Y0.252\n\ -N75 X0.188 Y0.255\n\ -N80 X0.227 Y0.268\n\ -N85 X0.257 Y0.271\n\ -N90 X0.335 Y0.265\n\ -N95 X0.412 Y0.271\n\ -N100 X0.474 Y0.287\n\ -N105 X0.491 Y0.289\n\ -N110 X0.517 Y0.273\n\ -N115 X0.544 Y0.263\n\ -N120 X0.580 Y0.261\n\ -N125 X0.595 Y0.263\n\ -N130 X0.621 Y0.274\n\ -N135 X0.659 Y0.306\n\ -N140 X0.673 Y0.335\n\ -N145 X0.679 Y0.361\n\ -N150 X0.678 Y0.389\n\ -N155 X0.670 Y0.420\n\ -N160 X0.670 Y0.446\n\ -N165 X0.680 Y0.474\n\ -N170 X0.704 Y0.497\n\ -N175 X0.732 Y0.515\n\ -N180 X0.794 Y0.535\n\ -N185 X0.872 Y0.567\n\ -N190 X0.950 Y0.604\n\ -N195 X1.031 Y0.647\n\ -N200 X1.149 Y0.722\n\ -N205 X1.308 Y0.854\n\ -N210 X1.430 Y0.984\n\ -N215 X1.447 Y0.986\n\ -N220 X1.628 Y0.822\n\ -N225 X1.732 Y0.714\n\ -N230 X2.009 Y0.381\n\ -N235 X2.116 Y0.266\n\ -N240 X2.228 Y0.157\n\ -N245 X2.280 Y0.123\n\ -N250 X2.332 Y0.094\n\ -N255 X2.423 Y0.070\n\ -N260 X2.489 Y0.065\n\ -N265 X2.557 Y0.066\n\ -N270 X2.627 Y0.084\n\ -N275 X2.693 Y0.109\n\ -N280 X2.730 Y0.133\n\ -N285 X2.805 Y0.190\n\ -N290 X2.829 Y0.216\n\ -N295 X2.880 Y0.291\n\ -N300 X2.884 Y0.300\n\ -N305 X2.898 Y0.332\n\ -N310 X2.905 Y0.367\n\ -N315 X2.910 Y0.407\n\ -N320 X2.908 Y0.446\n\ -N325 X2.901 Y0.495\n\ -N330 X2.897 Y0.495\n\ -N335 X2.890 Y0.524\n\ -N340 X2.842 Y0.604\n\ -N345 X2.814 Y0.665\n\ -N350 X2.794 Y0.750\n\ -N355 X2.791 Y0.838\n\ -N360 X2.819 Y0.976\n\ -N365 X2.837 Y1.044\n\ -N370 X2.862 Y1.112\n\ -N375 X2.877 Y1.145\n\ -N380 X2.912 Y1.217\n\ -N385 X2.917 Y1.226\n\ -N390 X2.951 Y1.281\n\ -N395 X2.970 Y1.295\n\ -N400 X2.996 Y1.295\n\ -N405 X3.013 Y1.275\n\ -N410 X3.024 Y1.254\n\ -N415 X3.070 Y1.151\n\ -N420 X3.077 Y1.134\n\ -N425 X3.125 Y1.039\n\ -N430 X3.138 Y1.019\n\ -N435 X3.205 Y0.908\n\ -N440 X3.282 Y0.797\n\ -N445 X3.361 Y0.691\n\ -N450 X3.426 Y0.592\n\ -N455 X3.456 Y0.543\n\ -N460 X3.479 Y0.493\n\ -N465 X3.494 Y0.472\n\ -N470 X3.505 Y0.446\n\ -N475 X3.518 Y0.417\n\ -N480 X3.530 Y0.391\n\ -N485 X3.564 Y0.300\n\ -N490 X3.577 Y0.253\n\ -N495 X3.568 Y0.212\n\ -N500 X3.544 Y0.176\n\ -N505 X3.541 Y0.146\n\ -N510 X3.553 Y0.119\n\ -N515 X3.576 Y0.106\n\ -N520 X3.597 Y0.102\n\ -N525 X3.618 Y0.109\n\ -N530 X3.639 Y0.129\n\ -N535 X3.650 Y0.159\n\ -N540 X3.663 Y0.178\n\ -N545 X3.678 Y0.192\n\ -N550 X3.699 Y0.197\n\ -N555 X3.723 Y0.196\n\ -N560 X3.764 Y0.179\n\ -N565 X3.805 Y0.165\n\ -N570 X3.884 Y0.159\n\ -N575 X3.949 Y0.165\n\ -N580 X3.966 Y0.175\n\ -N585 X3.978 Y0.194\n\ -N590 X3.978 Y0.223\n\ -N595 X3.960 Y0.245\n\ -N600 X3.834 Y0.311\n\ -N605 X3.762 Y0.345\n\ -N610 X3.726 Y0.389\n\ -N615 X3.703 Y0.458\n\ -N620 X3.682 Y0.530\n\ -N625 X3.679 Y0.534\n\ -N630 X3.626 Y0.681\n\ -N635 X3.568 Y0.828\n\ -N640 X3.444 Y1.080\n\ -N645 X3.405 Y1.167\n\ -N650 X3.404 Y1.167\n\ -N655 X3.387 Y1.208\n\ -N660 X3.384 Y1.208\n\ -N665 X3.365 Y1.265\n\ -N670 X3.355 Y1.285\n\ -N675 X3.348 Y1.308\n\ -N680 X3.333 Y1.350\n\ -N685 X3.321 Y1.391\n\ -N690 X3.311 Y1.428\n\ -N695 X3.339 Y1.423\n\ -N700 X3.440 Y1.406\n\ -N705 X3.510 Y1.408\n\ -N710 X3.585 Y1.421\n\ -N715 X3.611 Y1.439\n\ -N720 X3.611 Y1.441\n\ -N725 X3.615 Y1.441\n\ -N730 X3.636 Y1.488\n\ -N735 X3.634 Y1.517\n\ -N740 X3.622 Y1.543\n\ -N745 X3.605 Y1.563\n\ -N750 X3.576 Y1.575\n\ -N755 X3.544 Y1.581\n\ -N760 X3.479 Y1.592\n\ -N765 X3.451 Y1.598\n\ -N770 X3.387 Y1.626\n\ -N775 X3.390 Y1.636\n\ -N780 X3.532 Y1.602\n\ -N785 X3.601 Y1.597\n\ -N790 X3.668 Y1.598\n\ -N795 X3.782 Y1.612\n\ -N800 X3.794 Y1.618\n\ -N805 X3.868 Y1.636\n\ -N810 X3.918 Y1.655\n\ -N815 X3.945 Y1.667\n\ -N820 X3.964 Y1.685\n\ -N825 X3.967 Y1.685\n\ -N830 X3.974 Y1.699\n\ -N835 X3.984 Y1.725\n\ -N840 X3.984 Y1.758\n\ -N845 X3.970 Y1.790\n\ -N850 X3.945 Y1.817\n\ -N855 X3.918 Y1.826\n\ -N860 X3.891 Y1.827\n\ -N865 X3.842 Y1.809\n\ -N870 X3.798 Y1.784\n\ -N875 X3.746 Y1.772\n\ -N880 X3.698 Y1.764\n\ -N885 X3.635 Y1.766\n\ -N890 X3.553 Y1.785\n\ -N895 X3.478 Y1.820\n\ -N900 X3.409 Y1.864\n\ -N905 X3.345 Y1.918\n\ -N910 X3.264 Y2.023\n\ -N915 X3.248 Y2.051\n\ -N920 X3.240 Y2.081\n\ -N925 X3.316 Y2.086\n\ -N930 X3.390 Y2.104\n\ -N935 X3.414 Y2.121\n\ -N940 X3.431 Y2.153\n\ -N945 X3.433 Y2.180\n\ -N950 X3.427 Y2.205\n\ -N955 X3.413 Y2.224\n\ -N960 X3.392 Y2.242\n\ -N965 X3.335 Y2.244\n\ -N970 X3.278 Y2.236\n\ -N975 X3.256 Y2.242\n\ -N980 X3.239 Y2.251\n\ -N985 X3.211 Y2.280\n\ -N990 X3.174 Y2.356\n\ -N995 X3.159 Y2.397\n\ -N1000 X3.150 Y2.438\n\ -N1005 X3.140 Y2.481\n\ -N1010 X3.132 Y2.510\n\ -N1015 X3.118 Y2.543\n\ -N1020 X3.099 Y2.576\n\ -N1025 X3.051 Y2.631\n\ -N1030 X3.003 Y2.666\n\ -N1035 X2.957 Y2.685\n\ -N1040 X2.907 Y2.699\n\ -N1045 X2.860 Y2.711\n\ -N1050 X2.819 Y2.727\n\ -N1055 X2.768 Y2.745\n\ -N1060 X2.716 Y2.757\n\ -N1065 X2.675 Y2.757\n\ -N1070 X2.579 Y2.731\n\ -N1075 X2.570 Y2.723\n\ -N1080 X2.559 Y2.720\n\ -N1085 X2.530 Y2.701\n\ -N1090 X2.501 Y2.693\n\ -N1095 X2.471 Y2.683\n\ -N1100 X2.443 Y2.668\n\ -N1105 X2.396 Y2.634\n\ -N1110 X2.354 Y2.591\n\ -N1115 X2.352 Y2.591\n\ -N1120 X2.325 Y2.543\n\ -N1125 X2.315 Y2.521\n\ -N1130 X2.307 Y2.498\n\ -N1135 X2.296 Y2.464\n\ -N1140 X2.293 Y2.427\n\ -N1145 X2.295 Y2.366\n\ -N1150 X2.321 Y2.303\n\ -N1155 X2.358 Y2.247\n\ -N1160 X2.419 Y2.203\n\ -N1165 X2.419 Y2.179\n\ -N1170 X2.405 Y2.158\n\ -N1175 X2.362 Y2.123\n\ -N1180 X2.354 Y2.104\n\ -N1185 X2.354 Y2.091\n\ -N1190 X2.380 Y2.065\n\ -N1195 X2.410 Y2.051\n\ -N1200 X2.410 Y2.047\n\ -N1205 X2.405 Y2.028\n\ -N1210 X2.408 Y2.010\n\ -N1215 X2.416 Y1.995\n\ -N1220 X2.433 Y1.982\n\ -N1225 X2.438 Y1.963\n\ -N1230 X2.447 Y1.948\n\ -N1235 X2.461 Y1.938\n\ -N1240 X2.480 Y1.931\n\ -N1245 X2.478 Y1.899\n\ -N1250 X2.487 Y1.875\n\ -N1255 X2.504 Y1.854\n\ -N1260 X2.527 Y1.848\n\ -N1265 X2.549 Y1.849\n\ -N1270 X2.577 Y1.861\n\ -N1275 X2.604 Y1.866\n\ -N1280 X2.631 Y1.854\n\ -N1285 X2.640 Y1.827\n\ -N1290 X2.640 Y1.792\n\ -N1295 X2.629 Y1.759\n\ -N1300 X2.612 Y1.727\n\ -N1305 X2.576 Y1.694\n\ -N1310 X2.536 Y1.662\n\ -N1315 X2.470 Y1.616\n\ -N1320 X2.345 Y1.551\n\ -N1325 X2.307 Y1.521\n\ -N1330 X2.260 Y1.482\n\ -N1335 X2.243 Y1.477\n\ -N1340 X2.228 Y1.469\n\ -N1345 X2.222 Y1.448\n\ -N1350 X2.228 Y1.425\n\ -N1355 X2.254 Y1.405\n\ -N1360 X2.268 Y1.373\n\ -N1365 X2.279 Y1.342\n\ -N1370 X2.308 Y1.287\n\ -N1375 X2.340 Y1.254\n\ -N1380 X2.407 Y1.201\n\ -N1385 X2.441 Y1.156\n\ -N1390 X2.454 Y1.131\n\ -N1395 X2.460 Y1.106\n\ -N1400 X2.464 Y1.102\n\ -N1405 X2.471 Y1.036\n\ -N1410 X2.466 Y0.971\n\ -N1415 X2.451 Y0.897\n\ -N1420 X2.415 Y0.791\n\ -N1425 X2.403 Y0.767\n\ -N1430 X2.388 Y0.765\n\ -N1435 X2.306 Y0.841\n\ -N1440 X2.222 Y0.911\n\ -N1445 X2.114 Y0.995\n\ -N1450 X2.002 Y1.076\n\ -N1455 X1.905 Y1.139\n\ -N1460 X1.754 Y1.223\n\ -N1465 X1.637 Y1.278\n\ -N1470 X1.582 Y1.301\n\ -N1475 X1.558 Y1.315\n\ -N1480 X1.460 Y1.352\n\ -N1485 X1.399 Y1.360\n\ -N1490 X1.338 Y1.354\n\ -N1495 X1.287 Y1.329\n\ -N1500 X1.248 Y1.294\n\ -N1505 X1.217 Y1.254\n\ -N1510 X1.214 Y1.252\n\ -N1515 X1.172 Y1.195\n\ -N1520 X1.128 Y1.139\n\ -N1525 X1.110 Y1.117\n\ -N1530 X0.958 Y0.941\n\ -N1535 X0.891 Y0.873\n\ -N1540 X0.825 Y0.808\n\ -N1545 X0.755 Y0.747\n\ -N1550 X0.619 Y0.633\n\ -N1555 X0.477 Y0.526\n\ -N1560 X0.477 Y0.524\n\ -N1565 X0.411 Y0.476\n\ -N1570 X0.331 Y0.454\n\ -N1575 X0.278 Y0.427\n\ -N1580 X0.231 Y0.397\n\ -N1585 X0.183 Y0.379\n\ -N1590 X0.135 Y0.365\n\ -N1595 X0.076 Y0.341\n\ -N1605 G00 F30.0\n\ -N1610 M05 \n\ -N1615 G90 X0 Y0 Z0\n\ -N1615 M30\n\ -(End of CNC Program)"; diff --git a/src/tests/test_051_braid.h b/src/tests/test_051_braid.h deleted file mode 100644 index 4fb7e4d..0000000 --- a/src/tests/test_051_braid.h +++ /dev/null @@ -1,2549 +0,0 @@ -/* - * test_051_braid.h - * - * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) - -Search for ********************************* to get to the uncommented code - -N4 (S8000)\n\ -N5 (M3)\n\ -N6 G92X0.327Y-33.521Z-1.000\n\ -N7 G0Z4.000\n\ -N8 F1800.0\n\ -N9 G1 X0.327Y-33.521\n\ (Questionable???) -N10 G1Z-1.000\n\ -N11 X0.654Y-33.526\n\ - -N12 X0.980Y-33.534\n\ -N13 X1.304Y-33.546\n\ -N14 X1.626Y-33.562\n\ -N15 X1.946Y-33.580\n\ -N16 X2.262Y-33.602\n\ -N17 X2.574Y-33.628\n\ -N18 X2.882Y-33.656\n\ -N19 X3.185Y-33.688\n\ -N20 X3.483Y-33.724\n\ -N21 X3.775Y-33.762\n\ -N22 X4.060Y-33.805\n\ -N23 X4.339Y-33.850\n\ -N24 X4.610Y-33.898\n\ -N25 X4.874Y-33.950\n\ -N26 X5.130Y-34.005\n\ -N27 X5.376Y-34.064\n\ -N28 X5.614Y-34.125\n\ -N29 X5.842Y-34.190\n\ -N30 X6.060Y-34.257\n\ -N31 X6.268Y-34.328\n\ -N32 X6.466Y-34.402\n\ -N33 X6.652Y-34.479\n\ -N34 X6.827Y-34.559\n\ -N35 X6.990Y-34.642\n\ -N36 X7.141Y-34.728\n\ -N37 X7.280Y-34.817\n\ -N38 X7.407Y-34.909\n\ -N39 X7.521Y-35.003\n\ -N40 X7.621Y-35.101\n\ -N41 X7.709Y-35.201\n\ -N42 X7.783Y-35.304\n\ -N43 X7.843Y-35.410\n\ -N44 X7.890Y-35.518\n\ -N45 X7.923Y-35.629\n\ -N46 X7.942Y-35.743\n\ -N47 X7.946Y-35.859\n\ -N48 X7.937Y-35.977\n\ -N49 X7.914Y-36.098\n\ -N50 X7.876Y-36.221\n\ -N51 X7.824Y-36.347\n\ -N52 X7.758Y-36.475\n\ -N53 X7.677Y-36.605\n\ -N54 X7.583Y-36.738\n\ -N55 X7.474Y-36.872\n\ -N56 X7.352Y-37.009\n\ -N57 X7.216Y-37.148\n\ -N58 X7.066Y-37.289\n\ -N59 X6.903Y-37.431\n\ -N60 X6.536Y-37.722\n\ -N61 X6.333Y-37.870\n\ -N62 X6.118Y-38.020\n\ -N63 X5.650Y-38.324\n\ -N64 X5.399Y-38.479\n\ -N65 X5.135Y-38.635\n\ -N66 X4.576Y-38.951\n\ -N67 X3.334Y-39.598\n\ -N68 X2.971Y-39.776\n\ -N69 X2.599Y-39.955\n\ -N70 X1.828Y-40.317\n\ -N71 X0.193Y-41.050\n\ -N72 X-3.302Y-42.537\n\ -N73 X-3.747Y-42.723\n\ -N74 X-4.191Y-42.908\n\ -N75 X-5.074Y-43.278\n\ -N76 X-6.803Y-44.012\n\ -N77 X-9.961Y-45.432\n\ -N78 X-10.315Y-45.603\n\ -N79 X-10.659Y-45.773\n\ -N80 X-11.312Y-46.107\n\ -N81 X-11.620Y-46.272\n\ -N82 X-11.915Y-46.434\n\ -N83 X-12.466Y-46.753\n\ -N84 X-12.720Y-46.909\n\ -N85 X-12.960Y-47.063\n\ -N86 X-13.186Y-47.214\n\ -N87 X-13.396Y-47.364\n\ -N88 X-13.591Y-47.510\n\ -N89 X-13.770Y-47.654\n\ -N90 X-13.933Y-47.796\n\ -N91 X-14.080Y-47.935\n\ -N92 X-14.211Y-48.071\n\ -N93 X-14.326Y-48.204\n\ -N94 X-14.424Y-48.334\n\ -N95 X-14.505Y-48.462\n\ -N96 X-14.570Y-48.586\n\ -N97 X-14.618Y-48.707\n\ -N98 X-14.649Y-48.825\n\ -N99 X-14.663Y-48.940\n\ -N100 X-14.661Y-49.052\n\ -N101 X-14.642Y-49.160\n\ -N102 X-14.607Y-49.265\n\ -N103 X-14.556Y-49.366\n\ -N104 X-14.488Y-49.464\n\ -N105 X-14.404Y-49.559\n\ -N106 X-14.305Y-49.649\n\ -N107 X-14.190Y-49.737\n\ -N108 X-14.068Y-49.815\n\ -N109 X-13.934Y-49.889\n\ -N110 X-13.787Y-49.961\n\ -N111 X-13.628Y-50.029\n\ -N112 X-13.456Y-50.094\n\ -N113 X-13.273Y-50.155\n\ -N114 X-13.078Y-50.213\n\ -N115 X-12.872Y-50.267\n\ -N116 X-12.655Y-50.318\n\ -N117 X-12.428Y-50.366\n\ -N118 X-12.190Y-50.410\n\ -N119 X-11.944Y-50.450\n\ -N120 X-11.688Y-50.487\n\ -N121 X-11.423Y-50.520\n\ -N122 X-11.150Y-50.549\n\ -N123 X-10.870Y-50.575\n\ -N124 X-10.582Y-50.597\n\ -N125 X-10.287Y-50.616\n\ -N126 X-9.987Y-50.630\n\ -N127 X-9.680Y-50.641\n\ -N128 X-9.368Y-50.649\n\ -N129 X-9.052Y-50.652\n\ -N130 X-8.732Y-50.652\n\ -N131 X-8.408Y-50.648\n\ -N132 X-8.081Y-50.640\n\ -N133 X-7.751Y-50.628\n\ -N134 X-7.420Y-50.613\n\ -N135 X-7.087Y-50.594\n\ -N136 X-6.754Y-50.571\n\ -N137 X-6.420Y-50.544\n\ -N138 X-6.087Y-50.513\n\ -N139 X-5.754Y-50.479\n\ -N140 X-5.424Y-50.441\n\ -N141 X-5.095Y-50.399\n\ -N142 X-4.769Y-50.353\n\ -N143 X-4.446Y-50.304\n\ -N144 X-4.127Y-50.250\n\ -N145 X-3.812Y-50.193\n\ -N146 X-3.198Y-50.068\n\ -N147 X-2.899Y-50.000\n\ -N148 X-2.606Y-49.928\n\ -N149 X-2.321Y-49.852\n\ -N150 X-2.043Y-49.773\n\ -N151 X-1.773Y-49.690\n\ -N152 X-1.511Y-49.603\n\ -N153 X-1.015Y-49.419\n\ -N154 X-0.781Y-49.322\n\ -N155 X-0.557Y-49.221\n\ -N156 X-0.343Y-49.116\n\ -N157 X-0.141Y-49.009\n\ -N158 X0.050Y-48.897\n\ -N159 X0.230Y-48.783\n\ -N160 X0.398Y-48.664\n\ -N161 X0.553Y-48.543\n\ -N162 X0.696Y-48.418\n\ -N163 X0.826Y-48.290\n\ -N164 X0.943Y-48.159\n\ -N165 X1.046Y-48.024\n\ -N166 X1.136Y-47.887\n\ -N167 X1.213Y-47.746\n\ -N168 X1.275Y-47.602\n\ -N169 X1.323Y-47.455\n\ -N170 X1.357Y-47.308\n\ -N171 X1.377Y-47.159\n\ -N172 X1.383Y-47.006\n\ -N173 X1.375Y-46.851\n\ -N174 X1.353Y-46.693\n\ -N175 X1.317Y-46.532\n\ -N176 X1.267Y-46.369\n\ -N177 X1.203Y-46.203\n\ -N178 X1.126Y-46.035\n\ -N179 X1.034Y-45.864\n\ -N180 X0.929Y-45.691\n\ -N181 X0.810Y-45.515\n\ -N182 X0.677Y-45.338\n\ -N183 X0.531Y-45.157\n\ -N184 X0.199Y-44.791\n\ -N185 X0.014Y-44.604\n\ -N186 X-0.184Y-44.416\n\ -N187 X-0.617Y-44.033\n\ -N188 X-1.624Y-43.245\n\ -N189 X-1.903Y-43.044\n\ -N190 X-2.193Y-42.841\n\ -N191 X-2.801Y-42.432\n\ -N192 X-4.125Y-41.596\n\ -N193 X-4.475Y-41.384\n\ -N194 X-4.833Y-41.171\n\ -N195 X-5.567Y-40.743\n\ -N196 X-7.098Y-39.876\n\ -N197 X-10.296Y-38.118\n\ -N198 X-10.698Y-37.897\n\ -N199 X-11.099Y-37.676\n\ -N200 X-11.894Y-37.235\n\ -N201 X-13.448Y-36.356\n\ -N202 X-13.825Y-36.137\n\ -N203 X-14.197Y-35.919\n\ -N204 X-14.923Y-35.485\n\ -N205 X-16.288Y-34.625\n\ -N206 X-16.636Y-34.395\n\ -N207 X-16.973Y-34.165\n\ -N208 X-17.613Y-33.711\n\ -N209 X-17.915Y-33.486\n\ -N210 X-18.204Y-33.262\n\ -N211 X-18.743Y-32.821\n\ -N212 X-18.992Y-32.602\n\ -N213 X-19.227Y-32.386\n\ -N214 X-19.447Y-32.171\n\ -N215 X-19.652Y-31.959\n\ -N216 X-19.843Y-31.749\n\ -N217 X-20.017Y-31.540\n\ -N218 X-20.176Y-31.335\n\ -N219 X-20.320Y-31.131\n\ -N220 X-20.447Y-30.930\n\ -N221 X-20.558Y-30.731\n\ -N222 X-20.653Y-30.535\n\ -N223 X-20.731Y-30.341\n\ -N224 X-20.793Y-30.150\n\ -N225 X-20.838Y-29.961\n\ -N226 X-20.867Y-29.776\n\ -N227 X-20.879Y-29.593\n\ -N228 X-20.875Y-29.413\n\ -N229 X-20.854Y-29.236\n\ -N230 X-20.817Y-29.062\n\ -N231 X-20.764Y-28.891\n\ -N232 X-20.695Y-28.723\n\ -N233 X-20.610Y-28.559\n\ -N234 X-20.510Y-28.397\n\ -N235 X-20.394Y-28.239\n\ -N236 X-20.263Y-28.084\n\ -N237 X-20.117Y-27.932\n\ -N238 X-19.957Y-27.784\n\ -N239 X-19.782Y-27.639\n\ -N240 X-19.594Y-27.498\n\ -N241 X-19.392Y-27.360\n\ -N242 X-19.177Y-27.226\n\ -N243 X-18.950Y-27.095\n\ -N244 X-18.710Y-26.968\n\ -N245 X-18.459Y-26.845\n\ -N246 X-18.197Y-26.725\n\ -N247 X-17.924Y-26.609\n\ -N248 X-17.641Y-26.497\n\ -N249 X-17.349Y-26.389\n\ -N250 X-16.737Y-26.183\n\ -N251 X-16.419Y-26.086\n\ -N252 X-16.094Y-25.993\n\ -N253 X-15.763Y-25.904\n\ -N254 X-15.426Y-25.818\n\ -N255 X-15.083Y-25.737\n\ -N256 X-14.736Y-25.660\n\ -N257 X-14.030Y-25.516\n\ -N258 X-13.672Y-25.451\n\ -N259 X-13.313Y-25.389\n\ -N260 X-12.953Y-25.332\n\ -N261 X-12.591Y-25.278\n\ -N262 X-12.230Y-25.228\n\ -N263 X-11.870Y-25.182\n\ -N264 X-11.511Y-25.140\n\ -N265 X-11.155Y-25.103\n\ -N266 X-10.824Y-25.071\n\ -N267 X-10.497Y-25.043\n\ -N268 X-10.173Y-25.018\n\ -N269 X-9.853Y-24.996\n\ -N270 X-9.538Y-24.978\n\ -N271 X-9.228Y-24.964\n\ -N272 X-8.924Y-24.952\n\ -N273 X-8.625Y-24.944\n\ -N274 X-8.334Y-24.940\n\ -N275 X-8.049Y-24.939\n\ -N276 X-7.772Y-24.941\n\ -N277 X-7.504Y-24.946\n\ -N278 X-7.243Y-24.955\n\ -N279 X-6.992Y-24.967\n\ -N280 X-6.750Y-24.982\n\ -N281 X-6.518Y-25.000\n\ -N282 X-6.295Y-25.022\n\ -N283 X-6.084Y-25.047\n\ -N284 X-5.883Y-25.075\n\ -N285 X-5.694Y-25.106\n\ -N286 X-5.517Y-25.140\n\ -N287 X-5.351Y-25.177\n\ - -N288 X-5.197Y-25.217\n\ -N289 X-5.057Y-25.260\n\ -N290 X-4.928Y-25.306\n\ -N291 X-4.813Y-25.355\n\ -N292 X-4.711Y-25.407\n\ -N293 X-4.623Y-25.461\n\ -N294 X-4.548Y-25.518\n\ -N295 X-4.487Y-25.578\n\ -N296 X-4.439Y-25.641\n\ -N297 X-4.406Y-25.707\n\ -N298 X-4.387Y-25.775\n\ -N299 X-4.382Y-25.845\n\ -N300 X-4.392Y-25.918\n\ -N301 X-4.416Y-25.994\n\ -N302 X-4.454Y-26.072\n\ -N303 X-4.506Y-26.152\n\ -N304 X-4.572Y-26.235\n\ -N305 X-4.653Y-26.320\n\ -N306 X-4.748Y-26.407\n\ -N307 X-4.857Y-26.496\n\ -N308 X-4.980Y-26.588\n\ -N309 X-5.117Y-26.681\n\ -N310 X-5.267Y-26.777\n\ -N311 X-5.431Y-26.874\n\ -N312 X-5.798Y-27.074\n\ -N313 X-6.001Y-27.177\n\ -N314 X-6.216Y-27.282\n\ -N315 X-6.683Y-27.496\n\ -N316 X-7.755Y-27.942\n\ -N317 X-8.049Y-28.057\n\ -N318 X-8.353Y-28.173\n\ -N319 X-8.990Y-28.409\n\ -N320 X-10.362Y-28.891\n\ -N321 X-10.753Y-29.024\n\ -N322 X-11.151Y-29.158\n\ -N323 X-11.968Y-29.428\n\ -N324 X-13.658Y-29.973\n\ -N325 X-17.113Y-31.067\n\ -N326 X-17.538Y-31.202\n\ -N327 X-17.959Y-31.337\n\ -N328 X-18.788Y-31.604\n\ -N329 X-20.370Y-32.127\n\ -N330 X-20.746Y-32.255\n\ -N331 X-21.113Y-32.382\n\ -N332 X-21.817Y-32.631\n\ -N333 X-23.093Y-33.111\n\ -N334 X-23.381Y-33.226\n\ -N335 X-23.656Y-33.339\n\ -N336 X-24.164Y-33.560\n\ -N337 X-24.397Y-33.667\n\ -N338 X-24.615Y-33.772\n\ -N339 X-25.005Y-33.974\n\ -N340 X-25.177Y-34.072\n\ -N341 X-25.332Y-34.167\n\ -N342 X-25.472Y-34.259\n\ - -N100 G92 X-25.177Y-34.072 Z-1.000 (TEMP, REMOVE LATER)\n\ - - */ - -/***************************************************************************/ -/***************************************************************************/ -/***************************************************************************/ -/***************************************************************************/ - -const char test_braid[] PROGMEM = "\ -N1 T1M6\n\ -N2 G17\n\ -N3 G21\n\ -N8 F1800\n\ -N9 G1\n\ -N100 G92 X-25.177Y-34.072 Z-1.000 (TEMP, REMOVE LATER)\n\ -N343 X-25.595Y-34.349\n\ -N344 X-25.701Y-34.436\n\ -N345 X-25.791Y-34.520\n\ -N346 X-25.864Y-34.601\n\ -N347 X-25.920Y-34.679\n\ -N348 X-25.960Y-34.754\n\ -N349 X-25.982Y-34.826\n\ -N350 X-25.987Y-34.895\n\ -N351 X-25.976Y-34.961\n\ -N352 X-25.948Y-35.023\n\ -N353 X-25.903Y-35.082\n\ -N354 X-25.841Y-35.138\n\ -N355 X-25.763Y-35.190\n\ -N356 X-25.669Y-35.239\n\ -N357 X-25.559Y-35.284\n\ -N358 X-25.433Y-35.325\n\ -N359 X-25.291Y-35.363\n\ -N360 X-25.134Y-35.397\n\ -N361 X-24.963Y-35.428\n\ -N362 X-24.777Y-35.455\n\ -N363 X-24.577Y-35.477\n\ -N364 X-24.367Y-35.496\n\ -N365 X-24.144Y-35.511\n\ -N366 X-23.909Y-35.522\n\ -N367 X-23.661Y-35.529\n\ -N368 X-23.403Y-35.532\n\ -N369 X-23.133Y-35.532\n\ -N370 X-22.853Y-35.527\n\ -/N2518 G0X0.327Y-33.521Z-1.000\n\ -N2519 M30"; - -/* -N371 X-22.563Y-35.519\n\ -N372 X-22.263Y-35.506\n\ -N373 X-21.955Y-35.490\n\ -N374 X-21.638Y-35.470\n\ -N375 X-21.313Y-35.445\n\ -N376 X-20.981Y-35.416\n\ -N377 X-20.643Y-35.384\n\ -N378 X-20.299Y-35.347\n\ -N379 X-19.949Y-35.306\n\ -N380 X-19.595Y-35.261\n\ -N381 X-19.237Y-35.212\n\ -N382 X-18.875Y-35.158\n\ -N383 X-18.511Y-35.101\n\ - -N384 X-18.145Y-35.039 -N385 X-17.777Y-34.973 -N386 X-17.041Y-34.829 -N387 X-16.673Y-34.751 -N388 X-16.306Y-34.668 -N389 X-15.580Y-34.491 -N390 X-15.222Y-34.396 -N391 X-14.867Y-34.297 -N392 X-14.172Y-34.086 -N393 X-13.833Y-33.975 -N394 X-13.501Y-33.859 -N395 X-13.176Y-33.740 -N396 X-12.858Y-33.616 -N397 X-12.548Y-33.488 -N398 X-12.248Y-33.357 -N399 X-11.675Y-33.082 -N400 X-11.404Y-32.938 -N401 X-11.144Y-32.791 -N402 X-10.896Y-32.639 -N403 X-10.659Y-32.484 -N404 X-10.435Y-32.326 -N405 X-10.223Y-32.163 -N406 X-10.025Y-31.997 -N407 X-9.840Y-31.827 -N408 X-9.669Y-31.653 -N409 X-9.512Y-31.476 -N410 X-9.370Y-31.295 -N411 X-9.242Y-31.111 -N412 X-9.130Y-30.923 -N413 X-9.033Y-30.732 -N414 X-8.951Y-30.538 -N415 X-8.885Y-30.340 -N416 X-8.835Y-30.139 -N417 X-8.801Y-29.934 -N418 X-8.782Y-29.727 -N419 X-8.780Y-29.517 -N420 X-8.794Y-29.303 -N421 X-8.825Y-29.087 -N422 X-8.871Y-28.867 -N423 X-8.934Y-28.645 -N424 X-9.006Y-28.435 -N425 X-9.093Y-28.223 -N426 X-9.194Y-28.008 -N427 X-9.308Y-27.792 -N428 X-9.435Y-27.573 -N429 X-9.577Y-27.352 -N430 X-9.898Y-26.903 -N431 X-10.078Y-26.676 -N432 X-10.271Y-26.446 -N433 X-10.693Y-25.982 -N434 X-10.922Y-25.748 -N435 X-11.162Y-25.511 -N436 X-11.675Y-25.034 -N437 X-11.948Y-24.793 -N438 X-12.230Y-24.550 -N439 X-12.824Y-24.061 -N440 X-14.114Y-23.068 -N441 X-17.000Y-21.038 -N442 X-17.380Y-20.782 -N443 X-17.762Y-20.525 -N444 X-18.532Y-20.010 -N445 X-20.077Y-18.979 -N446 X-20.462Y-18.721 -N447 X-20.845Y-18.464 -N448 X-21.602Y-17.949 -N449 X-23.072Y-16.925 -N450 X-23.427Y-16.671 -N451 X-23.776Y-16.417 -N452 X-24.454Y-15.911 -N453 X-25.717Y-14.911 -N454 X-26.011Y-14.664 -N455 X-26.295Y-14.418 -N456 X-26.833Y-13.930 -N457 X-27.086Y-13.688 -N458 X-27.328Y-13.447 -N459 X-27.777Y-12.971 -N460 X-28.000Y-12.715 -N461 X-28.209Y-12.461 -N462 X-28.404Y-12.210 -N463 X-28.583Y-11.961 -N464 X-28.746Y-11.713 -N465 X-28.894Y-11.469 -N466 X-29.026Y-11.226 -N467 X-29.142Y-10.986 -N468 X-29.242Y-10.749 -N469 X-29.325Y-10.514 -N470 X-29.391Y-10.281 -N471 X-29.441Y-10.052 -N472 X-29.475Y-9.825 -N473 X-29.491Y-9.601 -N474 X-29.491Y-9.380 -N475 X-29.475Y-9.162 -N476 X-29.441Y-8.947 -N477 X-29.391Y-8.735 -N478 X-29.325Y-8.526 -N479 X-29.242Y-8.320 -N480 X-29.144Y-8.118 -N481 X-29.029Y-7.919 -N482 X-28.899Y-7.723 -N483 X-28.753Y-7.531 -N484 X-28.592Y-7.342 -N485 X-28.416Y-7.156 -N486 X-28.226Y-6.974 -N487 X-28.022Y-6.796 -N488 X-27.804Y-6.621 -N489 X-27.572Y-6.450 -N490 X-27.071Y-6.119 -N491 X-26.802Y-5.959 -N492 X-26.522Y-5.803 -N493 X-25.929Y-5.502 -N494 X-25.617Y-5.358 -N495 X-25.296Y-5.217 -N496 X-24.628Y-4.948 -N497 X-24.283Y-4.819 -N498 X-23.930Y-4.694 -N499 X-23.207Y-4.457 -N500 X-22.838Y-4.344 -N501 X-22.465Y-4.236 -N502 X-21.707Y-4.031 -N503 X-21.325Y-3.935 -N504 X-20.941Y-3.842 -N505 X-20.170Y-3.670 -N506 X-19.786Y-3.590 -N507 X-19.402Y-3.515 -N508 X-18.642Y-3.375 -N509 X-18.266Y-3.312 -N510 X-17.894Y-3.252 -N511 X-17.527Y-3.197 -N512 X-17.164Y-3.145 -N513 X-16.808Y-3.098 -N514 X-16.459Y-3.055 -N515 X-16.116Y-3.015 -N516 X-15.781Y-2.980 -N517 X-15.477Y-2.951 -N518 X-15.179Y-2.925 -N519 X-14.891Y-2.902 -N520 X-14.611Y-2.883 -N521 X-14.340Y-2.867 -N522 X-14.078Y-2.855 -N523 X-13.827Y-2.845 -N524 X-13.586Y-2.839 -N525 X-13.355Y-2.837 -N526 X-13.136Y-2.837 -N527 X-12.929Y-2.841 -N528 X-12.733Y-2.848 -N529 X-12.549Y-2.858 -N530 X-12.378Y-2.871 -N531 X-12.220Y-2.888 -N532 X-12.074Y-2.907 -N533 X-11.942Y-2.929 -N534 X-11.823Y-2.955 -N535 X-11.717Y-2.983 -N536 X-11.626Y-3.014 -N537 X-11.548Y-3.048 -N538 X-11.484Y-3.084 -N539 X-11.435Y-3.124 -N540 X-11.400Y-3.166 -N541 X-11.379Y-3.210 -N542 X-11.372Y-3.258 -N543 X-11.380Y-3.308 -N544 X-11.403Y-3.360 -N545 X-11.440Y-3.415 -N546 X-11.491Y-3.472 -N547 X-11.556Y-3.532 -N548 X-11.636Y-3.594 -N549 X-11.729Y-3.658 -N550 X-11.837Y-3.724 -N551 X-11.959Y-3.793 -N552 X-12.094Y-3.863 -N553 X-12.242Y-3.936 -N554 X-12.404Y-4.010 -N555 X-12.767Y-4.165 -N556 X-12.967Y-4.245 -N557 X-13.179Y-4.327 -N558 X-13.639Y-4.495 -N559 X-14.691Y-4.850 -N560 X-14.979Y-4.942 -N561 X-15.277Y-5.036 -N562 X-15.897Y-5.225 -N563 X-17.230Y-5.617 -N564 X-20.146Y-6.431 -N565 X-20.516Y-6.532 -N566 X-20.887Y-6.634 -N567 X-21.630Y-6.837 -N568 X-23.106Y-7.241 -N569 X-25.895Y-8.028 -N570 X-26.218Y-8.123 -N571 X-26.535Y-8.218 -N572 X-27.145Y-8.403 -N573 X-28.260Y-8.760 -N574 X-28.514Y-8.846 -N575 X-28.759Y-8.930 -N576 X-29.214Y-9.095 -N577 X-29.425Y-9.175 -N578 X-29.624Y-9.253 -N579 X-29.986Y-9.404 -N580 X-30.149Y-9.477 -N581 X-30.298Y-9.548 -N582 X-30.435Y-9.617 -N583 X-30.559Y-9.684 -N584 X-30.669Y-9.749 -N585 X-30.766Y-9.812 -N586 X-30.849Y-9.872 -N587 X-30.918Y-9.931 -N588 X-30.974Y-9.987 -N589 X-31.016Y-10.041 -N590 X-31.043Y-10.092 -N591 X-31.057Y-10.141 -N592 X-31.057Y-10.188 -N593 X-31.043Y-10.232 -N594 X-31.015Y-10.273 -N595 X-30.972Y-10.312 -N596 X-30.916Y-10.349 -N597 X-30.847Y-10.382 -N598 X-30.763Y-10.413 -N599 X-30.666Y-10.441 -N600 X-30.556Y-10.466 -N601 X-30.432Y-10.489 -N602 X-30.295Y-10.508 -N603 X-30.146Y-10.525 -N604 X-29.984Y-10.539 -N605 X-29.809Y-10.549 -N606 X-29.622Y-10.557 -N607 X-29.424Y-10.562 -N608 X-29.214Y-10.563 -N609 X-28.992Y-10.562 -N610 X-28.760Y-10.557 -N611 X-28.517Y-10.549 -N612 X-28.242Y-10.537 -N613 X-27.955Y-10.521 -N614 X-27.657Y-10.501 -N615 X-27.349Y-10.477 -N616 X-27.031Y-10.450 -N617 X-26.703Y-10.418 -N618 X-26.366Y-10.383 -N619 X-26.021Y-10.343 -N620 X-25.668Y-10.300 -N621 X-25.308Y-10.253 -N622 X-24.568Y-10.146 -N623 X-24.191Y-10.086 -N624 X-23.808Y-10.023 -N625 X-23.032Y-9.883 -N626 X-22.640Y-9.807 -N627 X-22.245Y-9.728 -N628 X-21.453Y-9.556 -N629 X-21.057Y-9.463 -N630 X-20.662Y-9.367 -N631 X-19.876Y-9.162 -N632 X-19.487Y-9.054 -N633 X-19.101Y-8.941 -N634 X-18.343Y-8.704 -N635 X-17.972Y-8.579 -N636 X-17.606Y-8.450 -N637 X-16.896Y-8.180 -N638 X-16.552Y-8.040 -N639 X-16.217Y-7.895 -N640 X-15.574Y-7.594 -N641 X-15.268Y-7.438 -N642 X-14.972Y-7.277 -N643 X-14.414Y-6.946 -N644 X-14.153Y-6.774 -N645 X-13.905Y-6.599 -N646 X-13.669Y-6.420 -N647 X-13.447Y-6.237 -N648 X-13.239Y-6.051 -N649 X-13.044Y-5.862 -N650 X-12.864Y-5.669 -N651 X-12.699Y-5.472 -N652 X-12.549Y-5.272 -N653 X-12.414Y-5.069 -N654 X-12.294Y-4.862 -N655 X-12.190Y-4.652 -N656 X-12.102Y-4.439 -N657 X-12.030Y-4.223 -N658 X-11.975Y-4.003 -N659 X-11.935Y-3.781 -N660 X-11.911Y-3.555 -N661 X-11.904Y-3.327 -N662 X-11.914Y-3.096 -N663 X-11.939Y-2.862 -N664 X-11.981Y-2.625 -N665 X-12.038Y-2.386 -N666 X-12.112Y-2.144 -N667 X-12.202Y-1.899 -N668 X-12.300Y-1.669 -N669 X-12.412Y-1.436 -N670 X-12.537Y-1.201 -N671 X-12.675Y-0.965 -N672 X-12.826Y-0.726 -N673 X-12.990Y-0.486 -N674 X-13.355Y0.001 -N675 X-13.555Y0.246 -N676 X-13.768Y0.494 -N677 X-14.226Y0.993 -N678 X-15.266Y2.009 -N679 X-15.550Y2.266 -N680 X-15.842Y2.524 -N681 X-16.451Y3.043 -N682 X-17.752Y4.093 -N683 X-20.572Y6.219 -N684 X-20.935Y6.486 -N685 X-21.298Y6.753 -N686 X-22.024Y7.287 -N687 X-23.456Y8.353 -N688 X-23.807Y8.618 -N689 X-24.154Y8.883 -N690 X-24.835Y9.411 -N691 X-26.125Y10.458 -N692 X-26.431Y10.717 -N693 X-26.728Y10.975 -N694 X-27.297Y11.489 -N695 X-27.568Y11.744 -N696 X-27.830Y11.997 -N697 X-28.322Y12.500 -N698 X-28.552Y12.749 -N699 X-28.771Y12.997 -N700 X-28.978Y13.243 -N701 X-29.174Y13.487 -N702 X-29.357Y13.729 -N703 X-29.528Y13.970 -N704 X-29.686Y14.209 -N705 X-29.831Y14.446 -N706 X-29.974Y14.700 -N707 X-30.101Y14.953 -N708 X-30.212Y15.202 -N709 X-30.307Y15.449 -N710 X-30.385Y15.694 -N711 X-30.447Y15.936 -N712 X-30.492Y16.175 -N713 X-30.521Y16.411 -N714 X-30.532Y16.644 -N715 X-30.527Y16.874 -N716 X-30.505Y17.102 -N717 X-30.466Y17.326 -N718 X-30.411Y17.547 -N719 X-30.338Y17.765 -N720 X-30.249Y17.980 -N721 X-30.144Y18.191 -N722 X-30.022Y18.399 -N723 X-29.884Y18.603 -N724 X-29.730Y18.805 -N725 X-29.561Y19.002 -N726 X-29.376Y19.196 -N727 X-29.176Y19.386 -N728 X-28.962Y19.573 -N729 X-28.733Y19.756 -N730 X-28.490Y19.935 -N731 X-28.233Y20.111 -N732 X-27.964Y20.282 -N733 X-27.681Y20.450 -N734 X-27.386Y20.614 -N735 X-27.080Y20.774 -N736 X-26.434Y21.081 -N737 X-26.096Y21.229 -N738 X-25.748Y21.373 -N739 X-25.025Y21.648 -N740 X-24.652Y21.780 -N741 X-24.272Y21.907 -N742 X-23.493Y22.149 -N743 X-23.095Y22.264 -N744 X-22.693Y22.375 -N745 X-21.877Y22.583 -N746 X-21.466Y22.681 -N747 X-21.053Y22.775 -N748 X-20.223Y22.949 -N749 X-19.809Y23.030 -N750 X-19.396Y23.107 -N751 X-18.575Y23.247 -N752 X-18.169Y23.311 -N753 X-17.767Y23.371 -N754 X-16.977Y23.478 -N755 X-16.591Y23.525 -N756 X-16.211Y23.568 -N757 X-15.838Y23.606 -N758 X-15.473Y23.641 -N759 X-15.116Y23.671 -N760 X-14.769Y23.698 -N761 X-14.430Y23.720 -N762 X-14.102Y23.739 -N763 X-13.791Y23.753 -N764 X-13.490Y23.763 -N765 X-13.200Y23.769 -N766 X-12.922Y23.772 -N767 X-12.657Y23.771 -N768 X-12.404Y23.766 -N769 X-12.164Y23.758 -N770 X-11.937Y23.746 -N771 X-11.724Y23.730 -N772 X-11.525Y23.711 -N773 X-11.340Y23.688 -N774 X-11.171Y23.662 -N775 X-11.016Y23.633 -N776 X-10.876Y23.600 -N777 X-10.752Y23.564 -N778 X-10.643Y23.524 -N779 X-10.549Y23.481 -N780 X-10.472Y23.435 -N781 X-10.411Y23.386 -N782 X-10.365Y23.334 -N783 X-10.336Y23.279 -N784 X-10.323Y23.221 -N785 X-10.326Y23.161 -N786 X-10.344Y23.097 -N787 X-10.379Y23.030 -N788 X-10.430Y22.961 -N789 X-10.497Y22.889 -N790 X-10.579Y22.815 -N791 X-10.676Y22.738 -N792 X-10.789Y22.659 -N793 X-10.917Y22.577 -N794 X-11.060Y22.493 -N795 X-11.217Y22.407 -N796 X-11.389Y22.318 -N797 X-11.773Y22.135 -N798 X-11.985Y22.041 -N799 X-12.210Y21.945 -N800 X-12.697Y21.747 -N801 X-12.958Y21.645 -N802 X-13.230Y21.542 -N803 X-13.806Y21.331 -N804 X-15.066Y20.894 -N805 X-15.401Y20.782 -N806 X-15.742Y20.670 -N807 X-16.442Y20.441 -N808 X-17.896Y19.977 -N809 X-20.868Y19.035 -N810 X-21.208Y18.926 -N811 X-21.545Y18.817 -N812 X-22.208Y18.600 -N813 X-23.472Y18.173 -N814 X-23.772Y18.068 -N815 X-24.066Y17.964 -N816 X-24.629Y17.759 -N817 X-25.650Y17.361 -N818 X-25.881Y17.265 -N819 X-26.101Y17.170 -N820 X-26.509Y16.984 -N821 X-26.696Y16.893 -N822 X-26.871Y16.804 -N823 X-27.184Y16.631 -N824 X-27.322Y16.547 -N825 X-27.447Y16.465 -N826 X-27.559Y16.385 -N827 X-27.658Y16.306 -N828 X-27.743Y16.230 -N829 X-27.815Y16.156 -N830 X-27.873Y16.084 -N831 X-27.917Y16.014 -N832 X-27.947Y15.946 -N833 X-27.963Y15.880 -N834 X-27.965Y15.817 -N835 X-27.953Y15.756 -N836 X-27.927Y15.697 -N837 X-27.887Y15.641 -N838 X-27.832Y15.588 -N839 X-27.764Y15.537 -N840 X-27.681Y15.488 -N841 X-27.585Y15.442 -N842 X-27.475Y15.399 -N843 X-27.351Y15.358 -N844 X-27.214Y15.321 -N845 X-27.063Y15.286 -N846 X-26.899Y15.253 -N847 X-26.723Y15.224 -N848 X-26.533Y15.198 -N849 X-26.332Y15.174 -N850 X-26.118Y15.154 -N851 X-25.892Y15.136 -N852 X-25.654Y15.122 -N853 X-25.405Y15.110 -N854 X-25.145Y15.102 -N855 X-24.875Y15.097 -N856 X-24.594Y15.095 -N857 X-24.304Y15.096 -N858 X-24.004Y15.100 -N859 X-23.694Y15.107 -N860 X-23.377Y15.118 -N861 X-23.051Y15.132 -N862 X-22.717Y15.149 -N863 X-22.376Y15.170 -N864 X-21.998Y15.196 -N865 X-21.613Y15.226 -N866 X-21.221Y15.260 -N867 X-20.822Y15.298 -N868 X-20.418Y15.340 -N869 X-20.010Y15.385 -N870 X-19.180Y15.489 -N871 X-18.761Y15.547 -N872 X-18.339Y15.609 -N873 X-17.492Y15.744 -N874 X-17.067Y15.818 -N875 X-16.644Y15.896 -N876 X-15.800Y16.064 -N877 X-15.382Y16.154 -N878 X-14.967Y16.248 -N879 X-14.150Y16.448 -N880 X-13.749Y16.554 -N881 X-13.354Y16.664 -N882 X-12.583Y16.895 -N883 X-12.209Y17.017 -N884 X-11.844Y17.142 -N885 X-11.140Y17.405 -N886 X-10.803Y17.542 -N887 X-10.477Y17.683 -N888 X-9.858Y17.975 -N889 X-9.566Y18.127 -N890 X-9.287Y18.283 -N891 X-9.021Y18.442 -N892 X-8.768Y18.604 -N893 X-8.529Y18.771 -N894 X-8.305Y18.940 -N895 X-8.094Y19.113 -N896 X-7.899Y19.290 -N897 X-7.718Y19.470 -N898 X-7.553Y19.653 -N899 X-7.403Y19.839 -N900 X-7.270Y20.029 -N901 X-7.152Y20.221 -N902 X-7.050Y20.417 -N903 X-6.965Y20.616 -N904 X-6.895Y20.818 -N905 X-6.843Y21.023 -N906 X-6.806Y21.230 -N907 X-6.787Y21.441 -N908 X-6.783Y21.654 -N909 X-6.796Y21.870 -N910 X-6.826Y22.088 -N911 X-6.872Y22.309 -N912 X-6.933Y22.532 -N913 X-7.011Y22.758 -N914 X-7.104Y22.986 -N915 X-7.213Y23.217 -N916 X-7.338Y23.449 -N917 X-7.477Y23.684 -N918 X-7.631Y23.921 -N919 X-7.982Y24.401 -N920 X-8.165Y24.627 -N921 X-8.360Y24.855 -N922 X-8.782Y25.315 -N923 X-9.751Y26.251 -N924 X-10.017Y26.488 -N925 X-10.291Y26.726 -N926 X-10.863Y27.204 -N927 X-12.089Y28.170 -N928 X-12.409Y28.413 -N929 X-12.734Y28.657 -N930 X-13.395Y29.145 -N931 X-14.749Y30.124 -N932 X-17.457Y32.077 -N933 X-17.785Y32.320 -N934 X-18.108Y32.561 -N935 X-18.740Y33.042 -N936 X-19.930Y33.994 -N937 X-20.210Y34.229 -N938 X-20.481Y34.463 -N939 X-20.997Y34.927 -N940 X-21.240Y35.157 -N941 X-21.474Y35.386 -N942 X-21.910Y35.838 -N943 X-22.111Y36.062 -N944 X-22.302Y36.284 -N945 X-22.480Y36.505 -N946 X-22.646Y36.723 -N947 X-22.800Y36.940 -N948 X-22.941Y37.155 -N949 X-23.069Y37.367 -N950 X-23.183Y37.578 -N951 X-23.285Y37.786 -N952 X-23.373Y37.993 -N953 X-23.447Y38.197 -N954 X-23.507Y38.398 -N955 X-23.553Y38.598 -N956 X-23.585Y38.795 -N957 X-23.602Y38.989 -N958 X-23.606Y39.181 -N959 X-23.593Y39.386 -N960 X-23.563Y39.588 -N961 X-23.516Y39.787 -N962 X-23.452Y39.983 -N963 X-23.372Y40.175 -N964 X-23.274Y40.364 -N965 X-23.160Y40.550 -N966 X-23.029Y40.732 -N967 X-22.882Y40.910 -N968 X-22.719Y41.086 -N969 X-22.539Y41.257 -N970 X-22.344Y41.425 -N971 X-22.134Y41.589 -N972 X-21.908Y41.749 -N973 X-21.668Y41.906 -N974 X-21.413Y42.059 -N975 X-21.144Y42.208 -N976 X-20.862Y42.353 -N977 X-20.566Y42.494 -N978 X-20.257Y42.631 -N979 X-19.937Y42.763 -N980 X-19.604Y42.892 -N981 X-18.906Y43.138 -N982 X-18.542Y43.254 -N983 X-18.168Y43.366 -N984 X-17.785Y43.474 -N985 X-17.394Y43.578 -N986 X-16.995Y43.678 -N987 X-16.589Y43.773 -N988 X-15.758Y43.950 -N989 X-15.335Y44.033 -N990 X-14.908Y44.111 -N991 X-14.476Y44.184 -N992 X-14.042Y44.253 -N993 X-13.606Y44.318 -N994 X-13.168Y44.379 -N995 X-12.289Y44.486 -N996 X-11.851Y44.534 -N997 X-11.414Y44.576 -N998 X-10.978Y44.615 -N999 X-10.546Y44.649 -N1000 X-10.116Y44.679 -N1001 X-9.691Y44.704 -N1002 X-9.271Y44.726 -N1003 X-8.856Y44.742 -N1004 X-8.447Y44.755 -N1005 X-8.044Y44.763 -N1006 X-7.650Y44.767 -N1007 X-7.263Y44.766 -N1008 X-6.885Y44.762 -N1009 X-6.516Y44.753 -N1010 X-6.157Y44.740 -N1011 X-5.808Y44.723 -N1012 X-5.471Y44.702 -N1013 X-5.145Y44.676 -N1014 X-4.830Y44.647 -N1015 X-4.529Y44.613 -N1016 X-4.240Y44.576 -N1017 X-3.965Y44.535 -N1018 X-3.703Y44.489 -N1019 X-3.456Y44.440 -N1020 X-3.227Y44.388 -N1021 X-3.013Y44.332 -N1022 X-2.813Y44.273 -N1023 X-2.629Y44.210 -N1024 X-2.459Y44.144 -N1025 X-2.305Y44.074 -N1026 X-2.167Y44.001 -N1027 X-2.044Y43.925 -N1028 X-1.938Y43.845 -N1029 X-1.847Y43.762 -N1030 X-1.773Y43.675 -N1031 X-1.715Y43.586 -N1032 X-1.673Y43.494 -N1033 X-1.647Y43.398 -N1034 X-1.638Y43.299 -N1035 X-1.645Y43.198 -N1036 X-1.667Y43.094 -N1037 X-1.706Y42.987 -N1038 X-1.761Y42.877 -N1039 X-1.832Y42.765 -N1040 X-1.918Y42.650 -N1041 X-2.019Y42.532 -N1042 X-2.135Y42.412 -N1043 X-2.267Y42.290 -N1044 X-2.412Y42.166 -N1045 X-2.572Y42.039 -N1046 X-2.934Y41.779 -N1047 X-3.134Y41.646 -N1048 X-3.347Y41.511 -N1049 X-3.810Y41.235 -N1050 X-4.059Y41.095 -N1051 X-4.319Y40.953 -N1052 X-4.869Y40.664 -N1053 X-6.076Y40.071 -N1054 X-8.785Y38.840 -N1055 X-9.139Y38.683 -N1056 X-9.494Y38.526 -N1057 X-10.205Y38.212 -N1058 X-11.612Y37.583 -N1059 X-14.215Y36.343 -N1060 X-14.490Y36.201 -N1061 X-14.756Y36.061 -N1062 X-15.263Y35.782 -N1063 X-15.503Y35.645 -N1064 X-15.734Y35.508 -N1065 X-16.165Y35.239 -N1066 X-16.365Y35.106 -N1067 X-16.553Y34.975 -N1068 X-16.730Y34.845 -N1069 X-16.895Y34.717 -N1070 X-17.049Y34.590 -N1071 X-17.190Y34.465 -N1072 X-17.318Y34.341 -N1073 X-17.434Y34.219 -N1074 X-17.537Y34.099 -N1075 X-17.626Y33.981 -N1076 X-17.702Y33.865 -N1077 X-17.764Y33.751 -N1078 X-17.813Y33.639 -N1079 X-17.847Y33.528 -N1080 X-17.868Y33.420 -N1081 X-17.875Y33.315 -N1082 X-17.867Y33.211 -N1083 X-17.845Y33.110 -N1084 X-17.809Y33.011 -N1085 X-17.759Y32.914 -N1086 X-17.695Y32.820 -N1087 X-17.616Y32.728 -N1088 X-17.523Y32.639 -N1089 X-17.417Y32.552 -N1090 X-17.296Y32.468 -N1091 X-17.162Y32.387 -N1092 X-17.014Y32.308 -N1093 X-16.852Y32.232 -N1094 X-16.677Y32.159 -N1095 X-16.489Y32.089 -N1096 X-16.288Y32.021 -N1097 X-16.075Y31.956 -N1098 X-15.849Y31.895 -N1099 X-15.611Y31.836 -N1100 X-15.361Y31.780 -N1101 X-15.099Y31.727 -N1102 X-14.827Y31.677 -N1103 X-14.543Y31.631 -N1104 X-14.250Y31.587 -N1105 X-13.946Y31.547 -N1106 X-13.632Y31.509 -N1107 X-13.309Y31.475 -N1108 X-12.977Y31.444 -N1109 X-12.637Y31.417 -N1110 X-12.289Y31.392 -N1111 X-11.933Y31.371 -N1112 X-11.570Y31.353 -N1113 X-11.201Y31.338 -N1114 X-10.825Y31.327 -N1115 X-10.444Y31.319 -N1116 X-10.058Y31.314 -N1117 X-9.667Y31.313 -N1118 X-9.273Y31.315 -N1119 X-8.874Y31.320 -N1120 X-8.473Y31.329 -N1121 X-8.069Y31.341 -N1122 X-7.630Y31.358 -N1123 X-7.188Y31.379 -N1124 X-6.746Y31.404 -N1125 X-6.303Y31.433 -N1126 X-5.861Y31.466 -N1127 X-5.421Y31.503 -N1128 X-4.982Y31.544 -N1129 X-4.546Y31.589 -N1130 X-4.113Y31.638 -N1131 X-3.684Y31.691 -N1132 X-3.260Y31.748 -N1133 X-2.841Y31.809 -N1134 X-2.428Y31.873 -N1135 X-2.021Y31.942 -N1136 X-1.231Y32.091 -N1137 X-0.848Y32.171 -N1138 X-0.475Y32.255 -N1139 X-0.110Y32.343 -N1140 X0.243Y32.434 -N1141 X0.587Y32.530 -N1142 X0.919Y32.629 -N1143 X1.547Y32.838 -N1144 X1.842Y32.948 -N1145 X2.124Y33.061 -N1146 X2.392Y33.178 -N1147 X2.647Y33.299 -N1148 X2.887Y33.423 -N1149 X3.112Y33.550 -N1150 X3.323Y33.681 -N1151 X3.518Y33.815 -N1152 X3.698Y33.952 -N1153 X3.862Y34.092 -N1154 X4.010Y34.236 -N1155 X4.142Y34.383 -N1156 X4.257Y34.533 -N1157 X4.356Y34.685 -N1158 X4.439Y34.841 -N1159 X4.505Y35.000 -N1160 X4.554Y35.161 -N1161 X4.586Y35.325 -N1162 X4.602Y35.492 -N1163 X4.602Y35.662 -N1164 X4.584Y35.834 -N1165 X4.551Y36.008 -N1166 X4.501Y36.185 -N1167 X4.435Y36.364 -N1168 X4.353Y36.546 -N1169 X4.255Y36.730 -N1170 X4.142Y36.916 -N1171 X4.014Y37.104 -N1172 X3.871Y37.294 -N1173 X3.713Y37.486 -N1174 X3.356Y37.876 -N1175 X3.157Y38.073 -N1176 X2.944Y38.272 -N1177 X2.483Y38.675 -N1178 X1.425Y39.496 -N1179 X1.155Y39.690 -N1180 X0.877Y39.885 -N1181 X0.300Y40.277 -N1182 X-0.925Y41.070 -N1183 X-3.540Y42.667 -N1184 X-3.871Y42.867 -N1185 X-4.202Y43.066 -N1186 X-4.858Y43.463 -N1187 X-6.136Y44.252 -N1188 X-6.444Y44.447 -N1189 X-6.748Y44.642 -N1190 X-7.337Y45.028 -N1191 X-8.430Y45.788 -N1192 X-8.682Y45.975 -N1193 X-8.926Y46.161 -N1194 X-9.383Y46.527 -N1195 X-9.596Y46.708 -N1196 X-9.799Y46.887 -N1197 X-10.170Y47.241 -N1198 X-10.337Y47.415 -N1199 X-10.493Y47.587 -N1200 X-10.636Y47.757 -N1201 X-10.766Y47.925 -N1202 X-10.884Y48.091 -N1203 X-10.988Y48.255 -N1204 X-11.078Y48.416 -N1205 X-11.155Y48.576 -N1206 X-11.218Y48.733 -N1207 X-11.267Y48.888 -N1208 X-11.301Y49.040 -N1209 X-11.322Y49.190 -N1210 X-11.328Y49.337 -N1211 X-11.319Y49.482 -N1212 X-11.296Y49.624 -N1213 X-11.259Y49.763 -N1214 X-11.207Y49.900 -N1215 X-11.140Y50.033 -N1216 X-11.059Y50.164 -N1217 X-10.963Y50.293 -N1218 X-10.853Y50.418 -N1219 X-10.729Y50.540 -N1220 X-10.591Y50.659 -N1221 X-10.439Y50.775 -N1222 X-10.276Y50.886 -N1223 X-10.100Y50.994 -N1224 X-9.912Y51.098 -N1225 X-9.710Y51.200 -N1226 X-9.496Y51.298 -N1227 X-9.270Y51.394 -N1228 X-8.783Y51.574 -N1229 X-8.522Y51.660 -N1230 X-8.250Y51.742 -N1231 X-7.968Y51.821 -N1232 X-7.675Y51.897 -N1233 X-7.372Y51.969 -N1234 X-7.059Y52.038 -N1235 X-6.407Y52.165 -N1236 X-6.068Y52.223 -N1237 X-5.722Y52.278 -N1238 X-5.368Y52.330 -N1239 X-5.006Y52.378 -N1240 X-4.639Y52.422 -N1241 X-4.265Y52.463 -N1242 X-3.886Y52.500 -N1243 X-3.501Y52.533 -N1244 X-3.112Y52.563 -N1245 X-2.719Y52.590 -N1246 X-2.322Y52.613 -N1247 X-1.923Y52.632 -N1248 X-1.521Y52.647 -N1249 X-1.116Y52.659 -N1250 X-0.711Y52.667 -N1251 X-0.304Y52.672 -N1252 X0.103Y52.673 -N1253 X0.510Y52.670 -N1254 X0.916Y52.664 -N1255 X1.321Y52.654 -N1256 X1.724Y52.640 -N1257 X2.125Y52.623 -N1258 X2.524Y52.602 -N1259 X2.919Y52.577 -N1260 X3.310Y52.549 -N1261 X3.696Y52.517 -N1262 X4.078Y52.481 -N1263 X4.455Y52.442 -N1264 X4.826Y52.400 -N1265 X5.190Y52.354 -N1266 X5.548Y52.304 -N1267 X5.898Y52.251 -N1268 X6.241Y52.194 -N1269 X6.576Y52.134 -N1270 X6.902Y52.070 -N1271 X7.219Y52.003 -N1272 X7.526Y51.933 -N1273 X7.824Y51.859 -N1274 X8.389Y51.701 -N1275 X8.656Y51.617 -N1276 X8.911Y51.530 -N1277 X9.154Y51.439 -N1278 X9.386Y51.346 -N1279 X9.606Y51.249 -N1280 X9.814Y51.149 -N1281 X10.009Y51.046 -N1282 X10.191Y50.939 -N1283 X10.374Y50.821 -N1284 X10.541Y50.698 -N1285 X10.693Y50.573 -N1286 X10.829Y50.444 -N1287 X10.948Y50.311 -N1288 X11.052Y50.175 -N1289 X11.139Y50.036 -N1290 X11.209Y49.894 -N1291 X11.263Y49.748 -N1292 X11.301Y49.600 -N1293 X11.323Y49.448 -N1294 X11.327Y49.293 -N1295 X11.316Y49.136 -N1296 X11.288Y48.975 -N1297 X11.245Y48.812 -N1298 X11.185Y48.646 -N1299 X11.109Y48.478 -N1300 X11.018Y48.307 -N1301 X10.912Y48.133 -N1302 X10.790Y47.957 -N1303 X10.654Y47.779 -N1304 X10.503Y47.598 -N1305 X10.159Y47.230 -N1306 X9.967Y47.043 -N1307 X9.762Y46.854 -N1308 X9.314Y46.470 -N1309 X8.282Y45.681 -N1310 X7.999Y45.480 -N1311 X7.707Y45.278 -N1312 X7.098Y44.870 -N1313 X5.798Y44.041 -N1314 X3.014Y42.350 -N1315 X2.662Y42.138 -N1316 X2.311Y41.925 -N1317 X1.617Y41.500 -N1318 X0.273Y40.654 -N1319 X-0.049Y40.444 -N1320 X-0.365Y40.234 -N1321 X-0.975Y39.817 -N1322 X-2.088Y38.994 -N1323 X-2.324Y38.805 -N1324 X-2.551Y38.617 -N1325 X-2.974Y38.245 -N1326 X-3.170Y38.061 -N1327 X-3.354Y37.878 -N1328 X-3.687Y37.516 -N1329 X-3.836Y37.338 -N1330 X-3.972Y37.162 -N1331 X-4.096Y36.987 -N1332 X-4.206Y36.813 -N1333 X-4.304Y36.642 -N1334 X-4.388Y36.473 -N1335 X-4.459Y36.305 -N1336 X-4.515Y36.139 -N1337 X-4.558Y35.976 -N1338 X-4.587Y35.815 -N1339 X-4.602Y35.655 -N1340 X-4.603Y35.498 -N1341 X-4.589Y35.343 -N1342 X-4.561Y35.191 -N1343 X-4.519Y35.041 -N1344 X-4.462Y34.893 -N1345 X-4.391Y34.748 -N1346 X-4.306Y34.605 -N1347 X-4.207Y34.465 -N1348 X-4.094Y34.327 -N1349 X-3.967Y34.192 -N1350 X-3.826Y34.060 -N1351 X-3.671Y33.930 -N1352 X-3.503Y33.803 -N1353 X-3.321Y33.679 -N1354 X-3.126Y33.558 -N1355 X-2.919Y33.440 -N1356 X-2.698Y33.324 -N1357 X-2.466Y33.212 -N1358 X-2.221Y33.102 -N1359 X-1.697Y32.893 -N1360 X-1.418Y32.792 -N1361 X-1.128Y32.695 -N1362 X-0.828Y32.601 -N1363 X-0.518Y32.510 -N1364 X-0.198Y32.422 -N1365 X0.131Y32.338 -N1366 X0.814Y32.178 -N1367 X1.168Y32.104 -N1368 X1.529Y32.032 -N1369 X1.897Y31.964 -N1370 X2.271Y31.899 -N1371 X2.651Y31.838 -N1372 X3.037Y31.780 -N1373 X3.822Y31.674 -N1374 X4.221Y31.626 -N1375 X4.622Y31.581 -N1376 X5.027Y31.540 -N1377 X5.434Y31.502 -N1378 X5.842Y31.468 -N1379 X6.252Y31.437 -N1380 X6.662Y31.410 -N1381 X7.072Y31.386 -N1382 X7.515Y31.363 -N1383 X7.957Y31.345 -N1384 X8.398Y31.331 -N1385 X8.835Y31.321 -N1386 X9.269Y31.315 -N1387 X9.699Y31.313 -N1388 X10.123Y31.315 -N1389 X10.543Y31.321 -N1390 X10.956Y31.330 -N1391 X11.362Y31.344 -N1392 X11.761Y31.362 -N1393 X12.152Y31.383 -N1394 X12.534Y31.409 -N1395 X12.907Y31.438 -N1396 X13.269Y31.471 -N1397 X13.622Y31.508 -N1398 X13.963Y31.549 -N1399 X14.293Y31.593 -N1400 X14.611Y31.641 -N1401 X14.916Y31.693 -N1402 X15.209Y31.749 -N1403 X15.487Y31.808 -N1404 X15.753Y31.870 -N1405 X16.003Y31.936 -N1406 X16.240Y32.006 -N1407 X16.461Y32.079 -N1408 X16.667Y32.155 -N1409 X16.858Y32.235 -N1410 X17.033Y32.318 -N1411 X17.191Y32.404 -N1412 X17.334Y32.493 -N1413 X17.460Y32.586 -N1414 X17.570Y32.681 -N1415 X17.662Y32.780 -N1416 X17.739Y32.881 -N1417 X17.798Y32.986 -N1418 X17.840Y33.093 -N1419 X17.866Y33.203 -N1420 X17.875Y33.316 -N1421 X17.867Y33.431 -N1422 X17.842Y33.549 -N1423 X17.801Y33.670 -N1424 X17.743Y33.793 -N1425 X17.669Y33.918 -N1426 X17.579Y34.045 -N1427 X17.473Y34.175 -N1428 X17.352Y34.307 -N1429 X17.215Y34.441 -N1430 X17.063Y34.577 -N1431 X16.897Y34.715 -N1432 X16.522Y34.997 -N1433 X16.314Y35.140 -N1434 X16.093Y35.285 -N1435 X15.614Y35.580 -N1436 X15.357Y35.729 -N1437 X15.089Y35.879 -N1438 X14.522Y36.184 -N1439 X13.280Y36.806 -N1440 X12.957Y36.960 -N1441 X12.628Y37.115 -N1442 X11.954Y37.427 -N1443 X10.561Y38.054 -N1444 X7.746Y39.303 -N1445 X7.404Y39.457 -N1446 X7.067Y39.611 -N1447 X6.409Y39.915 -N1448 X5.172Y40.511 -N1449 X4.883Y40.657 -N1450 X4.603Y40.802 -N1451 X4.074Y41.087 -N1452 X3.825Y41.227 -N1453 X3.588Y41.365 -N1454 X3.148Y41.636 -N1455 X2.947Y41.769 -N1456 X2.760Y41.900 -N1457 X2.585Y42.029 -N1458 X2.425Y42.156 -N1459 X2.278Y42.280 -N1460 X2.146Y42.402 -N1461 X2.028Y42.522 -N1462 X1.926Y42.639 -N1463 X1.839Y42.754 -N1464 X1.767Y42.866 -N1465 X1.711Y42.976 -N1466 X1.671Y43.083 -N1467 X1.646Y43.187 -N1468 X1.638Y43.289 -N1469 X1.645Y43.387 -N1470 X1.669Y43.483 -N1471 X1.709Y43.576 -N1472 X1.765Y43.665 -N1473 X1.838Y43.752 -N1474 X1.926Y43.835 -N1475 X2.030Y43.915 -N1476 X2.150Y43.992 -N1477 X2.286Y44.065 -N1478 X2.438Y44.135 -N1479 X2.605Y44.202 -N1480 X2.787Y44.265 -N1481 X2.984Y44.324 -N1482 X3.195Y44.380 -N1483 X3.421Y44.433 -N1484 X3.661Y44.481 -N1485 X3.915Y44.526 -N1486 X4.182Y44.568 -N1487 X4.443Y44.603 -N1488 X4.714Y44.635 -N1489 X4.996Y44.663 -N1490 X5.289Y44.688 -N1491 X5.591Y44.710 -N1492 X5.902Y44.728 -N1493 X6.223Y44.743 -N1494 X6.552Y44.754 -N1495 X6.890Y44.762 -N1496 X7.235Y44.766 -N1497 X7.587Y44.767 -N1498 X7.946Y44.764 -N1499 X8.311Y44.758 -N1500 X8.683Y44.748 -N1501 X9.059Y44.735 -N1502 X9.440Y44.717 -N1503 X9.826Y44.697 -N1504 X10.216Y44.672 -N1505 X10.608Y44.644 -N1506 X11.004Y44.613 -N1507 X11.401Y44.578 -N1508 X11.800Y44.539 -N1509 X12.602Y44.450 -N1510 X13.003Y44.400 -N1511 X13.403Y44.347 -N1512 X13.803Y44.289 -N1513 X14.201Y44.229 -N1514 X14.596Y44.164 -N1515 X14.990Y44.096 -N1516 X15.766Y43.949 -N1517 X16.148Y43.870 -N1518 X16.525Y43.787 -N1519 X17.263Y43.611 -N1520 X17.623Y43.518 -N1521 X17.976Y43.421 -N1522 X18.661Y43.217 -N1523 X18.991Y43.109 -N1524 X19.313Y42.999 -N1525 X19.626Y42.884 -N1526 X19.929Y42.767 -N1527 X20.223Y42.645 -N1528 X20.506Y42.521 -N1529 X21.041Y42.262 -N1530 X21.292Y42.127 -N1531 X21.531Y41.990 -N1532 X21.758Y41.849 -N1533 X21.974Y41.704 -N1534 X22.176Y41.557 -N1535 X22.366Y41.407 -N1536 X22.544Y41.253 -N1537 X22.708Y41.097 -N1538 X22.859Y40.937 -N1539 X22.996Y40.775 -N1540 X23.119Y40.609 -N1541 X23.229Y40.441 -N1542 X23.325Y40.269 -N1543 X23.407Y40.095 -N1544 X23.475Y39.919 -N1545 X23.529Y39.739 -N1546 X23.571Y39.541 -N1547 X23.597Y39.341 -N1548 X23.606Y39.137 -N1549 X23.599Y38.930 -N1550 X23.575Y38.721 -N1551 X23.534Y38.508 -N1552 X23.477Y38.293 -N1553 X23.404Y38.075 -N1554 X23.315Y37.855 -N1555 X23.211Y37.632 -N1556 X23.091Y37.406 -N1557 X22.955Y37.178 -N1558 X22.805Y36.948 -N1559 X22.640Y36.716 -N1560 X22.268Y36.244 -N1561 X22.061Y36.005 -N1562 X21.841Y35.765 -N1563 X21.364Y35.278 -N1564 X20.274Y34.284 -N1565 X19.975Y34.031 -N1566 X19.668Y33.778 -N1567 X19.029Y33.267 -N1568 X17.670Y32.234 -N1569 X17.316Y31.974 -N1570 X16.959Y31.713 -N1571 X16.236Y31.190 -N1572 X14.771Y30.140 -N1573 X14.405Y29.877 -N1574 X14.040Y29.614 -N1575 X13.319Y29.089 -N1576 X11.923Y28.043 -N1577 X11.588Y27.783 -N1578 X11.260Y27.524 -N1579 X10.626Y27.008 -N1580 X10.322Y26.752 -N1581 X10.027Y26.497 -N1582 X9.467Y25.990 -N1583 X9.203Y25.738 -N1584 X8.951Y25.488 -N1585 X8.482Y24.992 -N1586 X8.266Y24.747 -N1587 X8.064Y24.503 -N1588 X7.700Y24.021 -N1589 X7.550Y23.799 -N1590 X7.412Y23.578 -N1591 X7.288Y23.359 -N1592 X7.177Y23.143 -N1593 X7.079Y22.928 -N1594 X6.995Y22.715 -N1595 X6.925Y22.504 -N1596 X6.868Y22.295 -N1597 X6.826Y22.089 -N1598 X6.798Y21.884 -N1599 X6.784Y21.682 -N1600 X6.785Y21.483 -N1601 X6.800Y21.285 -N1602 X6.829Y21.091 -N1603 X6.873Y20.898 -N1604 X6.931Y20.708 -N1605 X7.003Y20.521 -N1606 X7.090Y20.336 -N1607 X7.191Y20.154 -N1608 X7.306Y19.975 -N1609 X7.434Y19.799 -N1610 X7.577Y19.625 -N1611 X7.733Y19.454 -N1612 X7.903Y19.286 -N1613 X8.085Y19.121 -N1614 X8.281Y18.959 -N1615 X8.489Y18.800 -N1616 X8.710Y18.644 -N1617 X8.943Y18.491 -N1618 X9.188Y18.341 -N1619 X9.711Y18.051 -N1620 X9.989Y17.910 -N1621 X10.278Y17.773 -N1622 X10.884Y17.508 -N1623 X11.201Y17.381 -N1624 X11.527Y17.257 -N1625 X12.203Y17.019 -N1626 X12.552Y16.905 -N1627 X12.908Y16.795 -N1628 X13.639Y16.584 -N1629 X14.012Y16.484 -N1630 X14.390Y16.387 -N1631 X15.159Y16.204 -N1632 X15.548Y16.118 -N1633 X15.940Y16.035 -N1634 X16.729Y15.880 -N1635 X17.126Y15.808 -N1636 X17.522Y15.739 -N1637 X18.315Y15.612 -N1638 X18.709Y15.554 -N1639 X19.102Y15.499 -N1640 X19.492Y15.448 -N1641 X19.879Y15.401 -N1642 X20.263Y15.357 -N1643 X20.642Y15.316 -N1644 X21.017Y15.279 -N1645 X21.387Y15.245 -N1646 X21.744Y15.215 -N1647 X22.095Y15.189 -N1648 X22.440Y15.166 -N1649 X22.778Y15.146 -N1650 X23.109Y15.129 -N1651 X23.431Y15.116 -N1652 X23.746Y15.106 -N1653 X24.052Y15.099 -N1654 X24.349Y15.095 -N1655 X24.636Y15.095 -N1656 X24.914Y15.097 -N1657 X25.181Y15.103 -N1658 X25.438Y15.112 -N1659 X25.684Y15.123 -N1660 X25.919Y15.138 -N1661 X26.142Y15.156 -N1662 X26.354Y15.177 -N1663 X26.553Y15.200 -N1664 X26.740Y15.227 -N1665 X26.914Y15.256 -N1666 X27.076Y15.288 -N1667 X27.225Y15.323 -N1668 X27.360Y15.361 -N1669 X27.482Y15.402 -N1670 X27.591Y15.445 -N1671 X27.686Y15.491 -N1672 X27.767Y15.539 -N1673 X27.835Y15.590 -N1674 X27.888Y15.643 -N1675 X27.928Y15.699 -N1676 X27.954Y15.757 -N1677 X27.965Y15.818 -N1678 X27.963Y15.881 -N1679 X27.947Y15.946 -N1680 X27.917Y16.014 -N1681 X27.873Y16.083 -N1682 X27.816Y16.155 -N1683 X27.744Y16.229 -N1684 X27.660Y16.305 -N1685 X27.562Y16.382 -N1686 X27.451Y16.462 -N1687 X27.327Y16.544 -N1688 X27.042Y16.712 -N1689 X26.881Y16.799 -N1690 X26.707Y16.887 -N1691 X26.326Y17.069 -N1692 X26.118Y17.162 -N1693 X25.900Y17.256 -N1694 X25.433Y17.449 -N1695 X24.385Y17.849 -N1696 X21.934Y18.690 -N1697 X21.575Y18.807 -N1698 X21.212Y18.925 -N1699 X20.474Y19.161 -N1700 X18.977Y19.635 -N1701 X16.040Y20.572 -N1702 X15.691Y20.686 -N1703 X15.348Y20.800 -N1704 X14.684Y21.024 -N1705 X13.458Y21.457 -N1706 X13.176Y21.562 -N1707 X12.904Y21.666 -N1708 X12.396Y21.868 -N1709 X12.159Y21.966 -N1710 X11.936Y22.063 -N1711 X11.528Y22.250 -N1712 X11.345Y22.340 -N1713 X11.176Y22.429 -N1714 X11.022Y22.515 -N1715 X10.882Y22.599 -N1716 X10.757Y22.681 -N1717 X10.648Y22.760 -N1718 X10.554Y22.836 -N1719 X10.476Y22.910 -N1720 X10.413Y22.982 -N1721 X10.367Y23.051 -N1722 X10.337Y23.117 -N1723 X10.323Y23.180 -N1724 X10.325Y23.240 -N1725 X10.344Y23.298 -N1726 X10.379Y23.352 -N1727 X10.430Y23.403 -N1728 X10.497Y23.452 -N1729 X10.580Y23.497 -N1730 X10.680Y23.538 -N1731 X10.795Y23.577 -N1732 X10.926Y23.612 -N1733 X11.072Y23.644 -N1734 X11.233Y23.673 -N1735 X11.410Y23.698 -N1736 X11.601Y23.719 -N1737 X11.807Y23.737 -N1738 X12.027Y23.751 -N1739 X12.260Y23.762 -N1740 X12.507Y23.769 -N1741 X12.767Y23.772 -N1742 X13.040Y23.771 -N1743 X13.325Y23.767 -N1744 X13.601Y23.760 -N1745 X13.887Y23.749 -N1746 X14.182Y23.734 -N1747 X14.486Y23.717 -N1748 X14.799Y23.696 -N1749 X15.120Y23.671 -N1750 X15.448Y23.643 -N1751 X15.784Y23.612 -N1752 X16.126Y23.577 -N1753 X16.475Y23.538 -N1754 X16.829Y23.496 -N1755 X17.188Y23.450 -N1756 X17.552Y23.401 -N1757 X17.921Y23.348 -N1758 X18.667Y23.232 -N1759 X19.045Y23.169 -N1760 X19.424Y23.102 -N1761 X20.187Y22.957 -N1762 X20.569Y22.879 -N1763 X20.951Y22.797 -N1764 X21.712Y22.623 -N1765 X22.090Y22.531 -N1766 X22.466Y22.435 -N1767 X23.208Y22.232 -N1768 X23.574Y22.125 -N1769 X23.935Y22.015 -N1770 X24.641Y21.784 -N1771 X24.985Y21.663 -N1772 X25.323Y21.538 -N1773 X25.977Y21.279 -N1774 X26.292Y21.144 -N1775 X26.599Y21.006 -N1776 X27.186Y20.719 -N1777 X27.465Y20.571 -N1778 X27.734Y20.419 -N1779 X27.993Y20.264 -N1780 X28.241Y20.106 -N1781 X28.477Y19.944 -N1782 X28.702Y19.779 -N1783 X29.116Y19.440 -N1784 X29.305Y19.266 -N1785 X29.481Y19.088 -N1786 X29.644Y18.908 -N1787 X29.794Y18.724 -N1788 X29.930Y18.538 -N1789 X30.053Y18.349 -N1790 X30.162Y18.156 -N1791 X30.258Y17.961 -N1792 X30.339Y17.763 -N1793 X30.406Y17.563 -N1794 X30.459Y17.359 -N1795 X30.498Y17.153 -N1796 X30.522Y16.944 -N1797 X30.532Y16.733 -N1798 X30.528Y16.519 -N1799 X30.510Y16.303 -N1800 X30.474Y16.066 -N1801 X30.421Y15.826 -N1802 X30.352Y15.583 -N1803 X30.266Y15.337 -N1804 X30.164Y15.089 -N1805 X30.046Y14.839 -N1806 X29.912Y14.586 -N1807 X29.762Y14.330 -N1808 X29.597Y14.073 -N1809 X29.417Y13.813 -N1810 X29.014Y13.287 -N1811 X28.791Y13.021 -N1812 X28.555Y12.753 -N1813 X28.044Y12.211 -N1814 X27.769Y11.938 -N1815 X27.483Y11.663 -N1816 X26.879Y11.109 -N1817 X25.553Y9.985 -N1818 X22.573Y7.693 -N1819 X22.183Y7.405 -N1820 X21.791Y7.116 -N1821 X21.005Y6.537 -N1822 X19.442Y5.382 -N1823 X19.058Y5.094 -N1824 X18.678Y4.806 -N1825 X17.931Y4.232 -N1826 X16.513Y3.095 -N1827 X16.178Y2.813 -N1828 X15.852Y2.532 -N1829 X15.229Y1.974 -N1830 X14.933Y1.697 -N1831 X14.649Y1.422 -N1832 X14.115Y0.876 -N1833 X13.867Y0.606 -N1834 X13.632Y0.337 -N1835 X13.411Y0.071 -N1836 X13.204Y-0.194 -N1837 X13.010Y-0.457 -N1838 X12.832Y-0.717 -N1839 X12.668Y-0.976 -N1840 X12.520Y-1.232 -N1841 X12.395Y-1.469 -N1842 X12.284Y-1.703 -N1843 X12.188Y-1.936 -N1844 X12.105Y-2.167 -N1845 X12.036Y-2.395 -N1846 X11.982Y-2.621 -N1847 X11.942Y-2.844 -N1848 X11.916Y-3.065 -N1849 X11.905Y-3.284 -N1850 X11.908Y-3.500 -N1851 X11.926Y-3.713 -N1852 X11.959Y-3.924 -N1853 X12.005Y-4.132 -N1854 X12.067Y-4.338 -N1855 X12.142Y-4.540 -N1856 X12.232Y-4.740 -N1857 X12.336Y-4.937 -N1858 X12.453Y-5.131 -N1859 X12.584Y-5.322 -N1860 X12.729Y-5.510 -N1861 X12.887Y-5.695 -N1862 X13.059Y-5.877 -N1863 X13.243Y-6.055 -N1864 X13.439Y-6.231 -N1865 X13.648Y-6.403 -N1866 X13.869Y-6.572 -N1867 X14.101Y-6.738 -N1868 X14.345Y-6.901 -N1869 X14.599Y-7.060 -N1870 X14.864Y-7.216 -N1871 X15.423Y-7.518 -N1872 X15.717Y-7.664 -N1873 X16.020Y-7.806 -N1874 X16.650Y-8.080 -N1875 X16.976Y-8.212 -N1876 X17.309Y-8.340 -N1877 X17.994Y-8.586 -N1878 X18.344Y-8.704 -N1879 X18.700Y-8.818 -N1880 X19.423Y-9.035 -N1881 X20.902Y-9.426 -N1882 X21.276Y-9.515 -N1883 X21.650Y-9.600 -N1884 X22.396Y-9.759 -N1885 X22.767Y-9.833 -N1886 X23.137Y-9.903 -N1887 X23.868Y-10.033 -N1888 X24.228Y-10.092 -N1889 X24.584Y-10.148 -N1890 X24.936Y-10.200 -N1891 X25.282Y-10.249 -N1892 X25.622Y-10.294 -N1893 X25.956Y-10.336 -N1894 X26.604Y-10.408 -N1895 X26.910Y-10.438 -N1896 X27.208Y-10.465 -N1897 X27.498Y-10.489 -N1898 X27.779Y-10.509 -N1899 X28.051Y-10.527 -N1900 X28.313Y-10.540 -N1901 X28.565Y-10.551 -N1902 X28.807Y-10.558 -N1903 X29.038Y-10.562 -N1904 X29.258Y-10.563 -N1905 X29.466Y-10.561 -N1906 X29.663Y-10.556 -N1907 X29.848Y-10.547 -N1908 X30.020Y-10.536 -N1909 X30.180Y-10.522 -N1910 X30.327Y-10.504 -N1911 X30.462Y-10.484 -N1912 X30.583Y-10.461 -N1913 X30.690Y-10.435 -N1914 X30.784Y-10.406 -N1915 X30.865Y-10.374 -N1916 X30.932Y-10.340 -N1917 X30.984Y-10.303 -N1918 X31.023Y-10.263 -N1919 X31.048Y-10.221 -N1920 X31.058Y-10.176 -N1921 X31.055Y-10.128 -N1922 X31.037Y-10.078 -N1923 X31.006Y-10.026 -N1924 X30.960Y-9.971 -N1925 X30.900Y-9.914 -N1926 X30.827Y-9.855 -N1927 X30.739Y-9.794 -N1928 X30.638Y-9.730 -N1929 X30.524Y-9.664 -N1930 X30.396Y-9.596 -N1931 X30.255Y-9.527 -N1932 X30.100Y-9.455 -N1933 X29.754Y-9.306 -N1934 X29.562Y-9.228 -N1935 X29.359Y-9.149 -N1936 X28.917Y-8.986 -N1937 X27.902Y-8.642 -N1938 X27.624Y-8.553 -N1939 X27.336Y-8.462 -N1940 X26.734Y-8.278 -N1941 X25.440Y-7.896 -N1942 X22.592Y-7.100 -N1943 X22.191Y-6.990 -N1944 X21.787Y-6.879 -N1945 X20.978Y-6.659 -N1946 X19.371Y-6.218 -N1947 X18.976Y-6.109 -N1948 X18.584Y-6.000 -N1949 X17.814Y-5.783 -N1950 X16.349Y-5.360 -N1951 X16.002Y-5.257 -N1952 X15.664Y-5.155 -N1953 X15.016Y-4.954 -N1954 X14.708Y-4.856 -N1955 X14.410Y-4.759 -N1956 X13.850Y-4.569 -N1957 X13.589Y-4.477 -N1958 X13.340Y-4.387 -N1959 X12.883Y-4.212 -N1960 X12.675Y-4.127 -N1961 X12.482Y-4.045 -N1962 X12.140Y-3.886 -N1963 X11.991Y-3.810 -N1964 X11.859Y-3.737 -N1965 X11.741Y-3.665 -N1966 X11.640Y-3.597 -N1967 X11.555Y-3.531 -N1968 X11.486Y-3.467 -N1969 X11.433Y-3.406 -N1970 X11.397Y-3.348 -N1971 X11.377Y-3.293 -N1972 X11.373Y-3.241 -N1973 X11.386Y-3.191 -N1974 X11.415Y-3.145 -N1975 X11.460Y-3.101 -N1976 X11.522Y-3.061 -N1977 X11.600Y-3.024 -N1978 X11.694Y-2.990 -N1979 X11.803Y-2.959 -N1980 X11.928Y-2.932 -N1981 X12.068Y-2.908 -N1982 X12.223Y-2.887 -N1983 X12.393Y-2.870 -N1984 X12.578Y-2.856 -N1985 X12.776Y-2.846 -N1986 X12.989Y-2.840 -N1987 X13.214Y-2.837 -N1988 X13.453Y-2.838 -N1989 X13.704Y-2.842 -N1990 X13.967Y-2.850 -N1991 X14.242Y-2.862 -N1992 X14.527Y-2.878 -N1993 X14.824Y-2.897 -N1994 X15.130Y-2.921 -N1995 X15.425Y-2.946 -N1996 X15.727Y-2.975 -N1997 X16.037Y-3.007 -N1998 X16.353Y-3.042 -N1999 X16.676Y-3.081 -N2000 X17.005Y-3.124 -N2001 X17.678Y-3.219 -N2002 X18.021Y-3.272 -N2003 X18.368Y-3.329 -N2004 X19.071Y-3.452 -N2005 X19.426Y-3.519 -N2006 X19.783Y-3.590 -N2007 X20.499Y-3.742 -N2008 X20.857Y-3.823 -N2009 X21.214Y-3.908 -N2010 X21.925Y-4.088 -N2011 X22.277Y-4.183 -N2012 X22.626Y-4.282 -N2013 X23.314Y-4.491 -N2014 X23.652Y-4.600 -N2015 X23.984Y-4.713 -N2016 X24.632Y-4.949 -N2017 X24.947Y-5.073 -N2018 X25.254Y-5.199 -N2019 X25.846Y-5.463 -N2020 X26.129Y-5.600 -N2021 X26.404Y-5.740 -N2022 X26.669Y-5.884 -N2023 X26.925Y-6.031 -N2024 X27.170Y-6.181 -N2025 X27.405Y-6.334 -N2026 X27.842Y-6.650 -N2027 X28.043Y-6.813 -N2028 X28.232Y-6.979 -N2029 X28.409Y-7.149 -N2030 X28.573Y-7.321 -N2031 X28.725Y-7.496 -N2032 X28.863Y-7.674 -N2033 X28.989Y-7.855 -N2034 X29.101Y-8.039 -N2035 X29.199Y-8.226 -N2036 X29.283Y-8.416 -N2037 X29.353Y-8.609 -N2038 X29.409Y-8.804 -N2039 X29.451Y-9.002 -N2040 X29.479Y-9.203 -N2041 X29.492Y-9.406 -N2042 X29.491Y-9.612 -N2043 X29.475Y-9.820 -N2044 X29.445Y-10.031 -N2045 X29.401Y-10.244 -N2046 X29.342Y-10.459 -N2047 X29.269Y-10.677 -N2048 X29.181Y-10.897 -N2049 X29.080Y-11.119 -N2050 X28.964Y-11.344 -N2051 X28.823Y-11.589 -N2052 X28.666Y-11.837 -N2053 X28.493Y-12.087 -N2054 X28.305Y-12.340 -N2055 X28.102Y-12.594 -N2056 X27.883Y-12.851 -N2057 X27.403Y-13.370 -N2058 X27.142Y-13.632 -N2059 X26.868Y-13.897 -N2060 X26.281Y-14.430 -N2061 X24.969Y-15.514 -N2062 X24.615Y-15.788 -N2063 X24.252Y-16.064 -N2064 X23.501Y-16.617 -N2065 X21.917Y-17.734 -N2066 X18.570Y-19.985 -N2067 X18.148Y-20.266 -N2068 X17.729Y-20.547 -N2069 X16.897Y-21.108 -N2070 X15.285Y-22.222 -N2071 X14.896Y-22.498 -N2072 X14.515Y-22.774 -N2073 X13.775Y-23.321 -N2074 X12.409Y-24.400 -N2075 X12.095Y-24.666 -N2076 X11.792Y-24.930 -N2077 X11.223Y-25.453 -N2078 X10.958Y-25.712 -N2079 X10.707Y-25.968 -N2080 X10.246Y-26.475 -N2081 X10.038Y-26.725 -N2082 X9.845Y-26.973 -N2083 X9.667Y-27.218 -N2084 X9.505Y-27.461 -N2085 X9.359Y-27.702 -N2086 X9.229Y-27.940 -N2087 X9.115Y-28.175 -N2088 X9.017Y-28.407 -N2089 X8.938Y-28.632 -N2090 X8.874Y-28.855 -N2091 X8.827Y-29.074 -N2092 X8.796Y-29.291 -N2093 X8.781Y-29.505 -N2094 X8.782Y-29.716 -N2095 X8.799Y-29.923 -N2096 X8.833Y-30.128 -N2097 X8.882Y-30.329 -N2098 X8.947Y-30.527 -N2099 X9.028Y-30.722 -N2100 X9.125Y-30.914 -N2101 X9.236Y-31.102 -N2102 X9.363Y-31.286 -N2103 X9.505Y-31.467 -N2104 X9.661Y-31.645 -N2105 X9.832Y-31.819 -N2106 X10.016Y-31.989 -N2107 X10.214Y-32.155 -N2108 X10.425Y-32.318 -N2109 X10.649Y-32.477 -N2110 X10.885Y-32.633 -N2111 X11.393Y-32.932 -N2112 X11.664Y-33.076 -N2113 X11.945Y-33.215 -N2114 X12.536Y-33.483 -N2115 X12.846Y-33.611 -N2116 X13.163Y-33.735 -N2117 X13.821Y-33.971 -N2118 X14.160Y-34.082 -N2119 X14.504Y-34.190 -N2120 X15.209Y-34.393 -N2121 X15.568Y-34.488 -N2122 X15.930Y-34.579 -N2123 X16.661Y-34.748 -N2124 X17.029Y-34.827 -N2125 X17.397Y-34.901 -N2126 X17.766Y-34.971 -N2127 X18.134Y-35.037 -N2128 X18.501Y-35.099 -N2129 X18.865Y-35.157 -N2130 X19.585Y-35.260 -N2131 X19.940Y-35.305 -N2132 X20.290Y-35.346 -N2133 X20.634Y-35.383 -N2134 X20.973Y-35.416 -N2135 X21.305Y-35.444 -N2136 X21.630Y-35.469 -N2137 X21.947Y-35.490 -N2138 X22.256Y-35.506 -N2139 X22.556Y-35.519 -N2140 X22.847Y-35.527 -N2141 X23.128Y-35.532 -N2142 X23.398Y-35.532 -N2143 X23.657Y-35.529 -N2144 X23.904Y-35.522 -N2145 X24.140Y-35.511 -N2146 X24.363Y-35.496 -N2147 X24.559Y-35.479 -N2148 X24.745Y-35.459 -N2149 X24.918Y-35.435 -N2150 X25.079Y-35.408 -N2151 X25.228Y-35.378 -N2152 X25.364Y-35.345 -N2153 X25.487Y-35.309 -N2154 X25.597Y-35.269 -N2155 X25.694Y-35.227 -N2156 X25.777Y-35.182 -N2157 X25.847Y-35.133 -N2158 X25.902Y-35.082 -N2159 X25.944Y-35.028 -N2160 X25.972Y-34.972 -N2161 X25.986Y-34.912 -N2162 X25.986Y-34.850 -N2163 X25.971Y-34.785 -N2164 X25.942Y-34.717 -N2165 X25.899Y-34.647 -N2166 X25.842Y-34.575 -N2167 X25.771Y-34.499 -N2168 X25.685Y-34.422 -N2169 X25.586Y-34.342 -N2170 X25.472Y-34.260 -N2171 X25.345Y-34.175 -N2172 X25.204Y-34.088 -N2173 X24.883Y-33.909 -N2174 X24.702Y-33.815 -N2175 X24.509Y-33.720 -N2176 X24.085Y-33.525 -N2177 X23.855Y-33.424 -N2178 X23.613Y-33.321 -N2179 X23.095Y-33.112 -N2180 X21.933Y-32.673 -N2181 X19.198Y-31.738 -N2182 X12.952Y-29.747 -N2183 X12.533Y-29.612 -N2184 X12.117Y-29.477 -N2185 X11.302Y-29.208 -N2186 X9.752Y-28.680 -N2187 X9.384Y-28.550 -N2188 X9.026Y-28.422 -N2189 X8.340Y-28.168 -N2190 X7.104Y-27.677 -N2191 X6.826Y-27.559 -N2192 X6.561Y-27.442 -N2193 X6.073Y-27.213 -N2194 X5.850Y-27.102 -N2195 X5.643Y-26.992 -N2196 X5.272Y-26.780 -N2197 X5.111Y-26.677 -N2198 X4.965Y-26.577 -N2199 X4.835Y-26.479 -N2200 X4.721Y-26.383 -N2201 X4.623Y-26.290 -N2202 X4.542Y-26.199 -N2203 X4.478Y-26.112 -N2204 X4.430Y-26.026 -N2205 X4.398Y-25.944 -N2206 X4.384Y-25.865 -N2207 X4.385Y-25.788 -N2208 X4.403Y-25.714 -N2209 X4.438Y-25.644 -N2210 X4.489Y-25.576 -N2211 X4.556Y-25.512 -N2212 X4.639Y-25.450 -N2213 X4.738Y-25.392 -N2214 X4.852Y-25.337 -N2215 X4.981Y-25.286 -N2216 X5.126Y-25.238 -N2217 X5.285Y-25.193 -N2218 X5.458Y-25.152 -N2219 X5.646Y-25.115 -N2220 X5.847Y-25.080 -N2221 X6.061Y-25.050 -N2222 X6.287Y-25.023 -N2223 X6.527Y-25.000 -N2224 X6.777Y-24.980 -N2225 X7.040Y-24.964 -N2226 X7.313Y-24.952 -N2227 X7.596Y-24.944 -N2228 X7.889Y-24.939 -N2229 X8.191Y-24.939 -N2230 X8.502Y-24.942 -N2231 X8.820Y-24.949 -N2232 X9.146Y-24.960 -N2233 X9.478Y-24.975 -N2234 X9.817Y-24.994 -N2235 X10.161Y-25.017 -N2236 X10.510Y-25.044 -N2237 X10.839Y-25.072 -N2238 X11.171Y-25.104 -N2239 X11.506Y-25.140 -N2240 X11.842Y-25.179 -N2241 X12.180Y-25.221 -N2242 X12.519Y-25.267 -N2243 X13.196Y-25.370 -N2244 X13.534Y-25.427 -N2245 X13.870Y-25.487 -N2246 X14.204Y-25.550 -N2247 X14.535Y-25.617 -N2248 X14.864Y-25.688 -N2249 X15.188Y-25.762 -N2250 X15.824Y-25.920 -N2251 X16.134Y-26.004 -N2252 X16.438Y-26.092 -N2253 X16.736Y-26.183 -N2254 X17.027Y-26.277 -N2255 X17.311Y-26.375 -N2256 X17.587Y-26.476 -N2257 X18.113Y-26.689 -N2258 X18.362Y-26.800 -N2259 X18.602Y-26.914 -N2260 X18.832Y-27.032 -N2261 X19.052Y-27.152 -N2262 X19.260Y-27.276 -N2263 X19.457Y-27.403 -N2264 X19.643Y-27.534 -N2265 X19.817Y-27.667 -N2266 X19.979Y-27.803 -N2267 X20.128Y-27.943 -N2268 X20.264Y-28.085 -N2269 X20.387Y-28.230 -N2270 X20.497Y-28.379 -N2271 X20.594Y-28.530 -N2272 X20.676Y-28.684 -N2273 X20.745Y-28.840 -N2274 X20.800Y-29.000 -N2275 X20.841Y-29.162 -N2276 X20.867Y-29.327 -N2277 X20.879Y-29.494 -N2278 X20.876Y-29.664 -N2279 X20.859Y-29.837 -N2280 X20.828Y-30.012 -N2281 X20.781Y-30.189 -N2282 X20.721Y-30.369 -N2283 X20.645Y-30.551 -N2284 X20.556Y-30.735 -N2285 X20.452Y-30.922 -N2286 X20.333Y-31.110 -N2287 X20.201Y-31.301 -N2288 X20.055Y-31.494 -N2289 X19.894Y-31.688 -N2290 X19.721Y-31.885 -N2291 X19.533Y-32.084 -N2292 X19.120Y-32.486 -N2293 X18.893Y-32.690 -N2294 X18.655Y-32.896 -N2295 X18.142Y-33.311 -N2296 X17.874Y-33.517 -N2297 X17.595Y-33.724 -N2298 X17.007Y-34.142 -N2299 X15.720Y-34.991 -N2300 X12.798Y-36.727 -N2301 X12.410Y-36.947 -N2302 X12.018Y-37.167 -N2303 X11.225Y-37.607 -N2304 X9.621Y-38.487 -N2305 X6.455Y-40.236 -N2306 X6.074Y-40.452 -N2307 X5.697Y-40.668 -N2308 X4.960Y-41.096 -N2309 X3.566Y-41.941 -N2310 X3.237Y-42.149 -N2311 X2.916Y-42.356 -N2312 X2.302Y-42.766 -N2313 X1.194Y-43.568 -N2314 X0.944Y-43.764 -N2315 X0.706Y-43.959 -N2316 X0.264Y-44.342 -N2317 X0.062Y-44.531 -N2318 X-0.128Y-44.718 -N2319 X-0.469Y-45.085 -N2320 X-0.620Y-45.266 -N2321 X-0.758Y-45.444 -N2322 X-0.882Y-45.620 -N2323 X-0.993Y-45.794 -N2324 X-1.090Y-45.965 -N2325 X-1.173Y-46.134 -N2326 X-1.243Y-46.301 -N2327 X-1.298Y-46.465 -N2328 X-1.342Y-46.640 -N2329 X-1.371Y-46.812 -N2330 X-1.382Y-46.980 -N2331 X-1.378Y-47.146 -N2332 X-1.357Y-47.308 -N2333 X-1.320Y-47.467 -N2334 X-1.267Y-47.622 -N2335 X-1.198Y-47.775 -N2336 X-1.114Y-47.923 -N2337 X-1.014Y-48.068 -N2338 X-0.899Y-48.210 -N2339 X-0.769Y-48.348 -N2340 X-0.625Y-48.482 -N2341 X-0.466Y-48.612 -N2342 X-0.293Y-48.739 -N2343 X-0.107Y-48.862 -N2344 X0.093Y-48.981 -N2345 X0.305Y-49.097 -N2346 X0.530Y-49.208 -N2347 X0.766Y-49.315 -N2348 X1.014Y-49.419 -N2349 X1.273Y-49.518 -N2350 X1.822Y-49.705 -N2351 X2.110Y-49.792 -N2352 X2.407Y-49.875 -N2353 X2.713Y-49.954 -N2354 X3.026Y-50.029 -N2355 X3.346Y-50.100 -N2356 X3.672Y-50.166 -N2357 X4.004Y-50.229 -N2358 X4.341Y-50.287 -N2359 X4.683Y-50.340 -N2360 X5.028Y-50.390 -N2361 X5.376Y-50.435 -N2362 X5.727Y-50.476 -N2363 X6.080Y-50.513 -N2364 X6.433Y-50.545 -N2365 X6.787Y-50.573 -N2366 X7.141Y-50.597 -N2367 X7.494Y-50.617 -N2368 X7.845Y-50.632 -N2369 X8.194Y-50.643 -N2370 X8.540Y-50.650 -N2371 X8.882Y-50.652 -N2372 X9.220Y-50.651 -N2373 X9.553Y-50.645 -N2374 X9.881Y-50.635 -N2375 X10.202Y-50.620 -N2376 X10.516Y-50.602 -N2377 X10.823Y-50.579 -N2378 X11.122Y-50.552 -N2379 X11.412Y-50.521 -N2380 X11.693Y-50.486 -N2381 X11.964Y-50.447 -N2382 X12.225Y-50.404 -N2383 X12.475Y-50.356 -N2384 X12.714Y-50.305 -N2385 X12.941Y-50.250 -N2386 X13.155Y-50.191 -N2387 X13.357Y-50.128 -N2388 X13.546Y-50.061 -N2389 X13.721Y-49.990 -N2390 X13.882Y-49.916 -N2391 X14.020Y-49.843 -N2392 X14.144Y-49.767 -N2393 X14.256Y-49.688 -N2394 X14.355Y-49.605 -N2395 X14.441Y-49.520 -N2396 X14.513Y-49.431 -N2397 X14.571Y-49.340 -N2398 X14.615Y-49.245 -N2399 X14.645Y-49.148 -N2400 X14.662Y-49.048 -N2401 X14.664Y-48.944 -N2402 X14.651Y-48.838 -N2403 X14.625Y-48.730 -N2404 X14.584Y-48.618 -N2405 X14.529Y-48.504 -N2406 X14.459Y-48.387 -N2407 X14.376Y-48.268 -N2408 X14.278Y-48.146 -N2409 X14.166Y-48.022 -N2410 X14.040Y-47.896 -N2411 X13.900Y-47.767 -N2412 X13.747Y-47.635 -N2413 X13.399Y-47.366 -N2414 X13.206Y-47.228 -N2415 X12.999Y-47.088 -N2416 X12.548Y-46.803 -N2417 X11.504Y-46.209 -N2418 X11.215Y-46.057 -N2419 X10.916Y-45.902 -N2420 X10.287Y-45.589 -N2421 X8.923Y-44.948 -N2422 X5.870Y-43.614 -N2423 X5.468Y-43.444 -N2424 X5.063Y-43.274 -N2425 X4.246Y-42.931 -N2426 X2.603Y-42.244 -N2427 X-0.599Y-40.871 -N2428 X-1.025Y-40.682 -N2429 X-1.444Y-40.493 -N2430 X-2.257Y-40.118 -N2431 X-3.764Y-39.380 -N2432 X-4.113Y-39.199 -N2433 X-4.450Y-39.019 -N2434 X-5.084Y-38.665 -N2435 X-5.380Y-38.490 -N2436 X-5.662Y-38.317 -N2437 X-6.181Y-37.977 -N2438 X-6.417Y-37.810 -N2439 X-6.637Y-37.645 -N2440 X-6.841Y-37.483 -N2441 X-7.028Y-37.322 -N2442 X-7.199Y-37.165 -N2443 X-7.352Y-37.009 -N2444 X-7.488Y-36.856 -N2445 X-7.607Y-36.706 -N2446 X-7.708Y-36.559 -N2447 X-7.791Y-36.414 -N2448 X-7.856Y-36.272 -N2449 X-7.904Y-36.133 -N2450 X-7.934Y-35.997 -N2451 X-7.946Y-35.864 -N2452 X-7.941Y-35.734 -N2453 X-7.918Y-35.608 -N2454 X-7.877Y-35.484 -N2455 X-7.819Y-35.364 -N2456 X-7.744Y-35.247 -N2457 X-7.652Y-35.134 -N2458 X-7.543Y-35.024 -N2459 X-7.418Y-34.918 -N2460 X-7.277Y-34.815 -N2461 X-7.121Y-34.716 -N2462 X-6.949Y-34.620 -N2463 X-6.762Y-34.528 -N2464 X-6.560Y-34.440 -N2465 X-6.345Y-34.356 -N2466 X-6.116Y-34.276 -N2467 X-5.874Y-34.199 -N2468 X-5.620Y-34.127 -N2469 X-5.353Y-34.058 -N2470 X-5.076Y-33.993 -N2471 X-4.787Y-33.933 -N2472 X-4.489Y-33.876 -N2473 X-4.181Y-33.824 -N2474 X-3.864Y-33.775 -N2475 X-3.539Y-33.731 -N2476 X-3.207Y-33.691 -N2477 X-2.867Y-33.655 -N2478 X-2.522Y-33.623 -N2479 X-2.172Y-33.596 -N2480 X-1.817Y-33.572 -N2481 X-1.458Y-33.553 -N2482 X-1.096Y-33.538 -N2483 X-0.732Y-33.528 -N2484 X-0.366Y-33.521 -N2485 X0.000Y-33.519 -N2486 G0Z4.000 -N2487 G0X37.560Y12.327Z6.000 -N2488 G1Z-1.000 -N2489 G1Y0.876 -N2490 X49.011 -N2491 Y12.327 -N2492 X37.560 -N2493 G0Z4.000 -N2494 G0Y0.876 -N2495 G1Z-1.000 -N2496 G1Y-10.575 -N2497 X49.011 -N2498 Y0.876 -N2499 X37.560 -N2500 G0Z4.000 -N2501 G0X49.011Y12.327 -N2502 G1Z-1.000 -N2503 G1X52.084Y15.011 -N2504 G0Z4.000 -N2505 G0X49.011Y0.876 -N2506 G1Z-1.000 -N2507 G1X52.084Y6.213 -N2508 Y15.011 -N2509 X43.286 -N2510 X37.560Y12.327 -N2511 G0Z4.000 -N2512 G0X49.011Y-10.575 -N2513 G1Z-1.000 -N2514 G1X52.084Y-2.585 -N2515 Y6.213 -N2516 X49.011Y0.876 -N2517 G0Z4.000 - -*/ diff --git a/src/tests/test_099.h b/src/tests/test_099.h deleted file mode 100644 index a8c9b58..0000000 --- a/src/tests/test_099.h +++ /dev/null @@ -1,112 +0,0 @@ -/* - * test_099.h - Arbitrary test file for debugging - */ -/* -N0 T1M6\n\ -N1 G17\n\ -N2 G0Z5.000\n\ -N3 G0X0.000Y0.000\n\ -N4 (S12000M3)\n\ -N5 G92 X74.888 Y0.000 Z5.000\n\ -N6 G1Z-2.391F100.0\n\ -*/ - -// back and forth in Z -const char test_99[] PROGMEM = "\ -N6 G92 X75 Y0 Z0\n\ -N7 F500.0\n\ -N8 G1Y0.291Z-2.198\n\ -N9 X74.767Y0.058Z-2.355\n\ -N10 X74.709Y-0.000Z-2.371\n\ -N11 X74.529Z-2.264\n\ -N12 X74.634Y0.105Z-2.319\n\ -N13 X74.843Y0.314Z-2.183\n\ -N14 X75.000Y0.471Z-2.126\n\ -N15 Y0.650Z-2.100\n\ -N16 X74.783Y0.434Z-2.131\n\ -N17 X74.566Y0.217Z-2.244\n\ -N18 X74.350Y-0.000Z-2.182\n\ -N19 X74.170Z-2.144\n\ -N20 X74.429Y0.259Z-2.165\n\ -N21 X74.533Y0.363Z-2.191\n\ -N22 X74.689Y0.519Z-2.114\n\ -N23 X74.844Y0.674Z-2.096\n\ -N24 X75.000Y0.830Z-2.114\n\ -N25 Y1.010Z-2.144\n\ -N26 X74.787Y0.797Z-2.098\n\ -N27 X74.628Y0.638Z-2.108\n\ -N28 X74.469Y0.478Z-2.157\n\ -N29 X74.309Y0.319Z-2.110\n\ -N30 X74.044Y0.053Z-2.108\n\ -N31 X73.990Y0.000Z-2.077\n\ -N32 X73.811Z-1.947\n\ -N33 X73.973Y0.162Z-2.079\n\ -N34 X74.243Y0.432Z-2.084\n\ -N35 X74.459Y0.649Z-2.157\n\ -N36 X74.676Y0.865Z-2.109\n\ -N37 X74.838Y1.027Z-2.111\n\ -N38 X75.000Y1.189Z-2.160\n\ -N39 Y1.369Z-2.194\n\ -N40 X74.842Y1.211Z-2.134\n\ -N41 X74.684Y1.053Z-2.120\n\ -N42 X74.421Y0.790Z-2.176\n\ -N43 X74.316Y0.684Z-2.137\n\ -N44 X74.158Y0.526Z-2.077\n\ -N45 X73.947Y0.316Z-2.067\n\ -N46 X73.894Y0.263Z-2.046\n\ -N47 X73.737Y0.105Z-1.918\n\ -N48 X73.631Y0.000Z-1.872\n\ -N49 X73.452Z-1.815\n\ -N50 X73.665Y0.214Z-1.914\n\ -N51 X73.825Y0.374Z-2.028\n\ -N52 X73.879Y0.427Z-2.076\n\ -N53 X74.092Y0.641Z-2.094\n\ -N54 X74.306Y0.854Z-2.192\n\ -N55 X74.519Y1.068Z-2.141\n\ -N56 X74.733Y1.281Z-2.143\n\ -N57 X74.840Y1.388Z-2.174\n\ -N58 X74.893Y1.442Z-2.169\n\ -N59 X75.000Y1.548Z-2.121\n\ -N60 Y1.728Z-2.076\n\ -N61 X74.843Y1.571Z-2.130\n\ -N62 X74.738Y1.466Z-2.195\n\ -N63 X74.476Y1.204Z-2.147\n\ -N64 X74.215Y0.943Z-2.202\n\ -N65 X73.900Y0.628Z-2.101\n\ -N66 X73.848Y0.576Z-2.096\n\ -N67 X73.586Y0.314Z-1.913\n\ -N68 X73.377Y0.105Z-1.808\n\ -N69 X73.272Y0.000Z-1.787\n\ -N70 X73.182Z-1.787\n\ -N71 X73.092Z-1.750\n\ -N72 X73.410Y0.318Z-1.868\n\ -N73 X73.781Y0.689Z-2.086\n\ -N74 X74.099Y1.007Z-2.224\n\ -N75 X74.205Y1.113Z-2.211\n\ -N76 X74.364Y1.272Z-2.176\n\ -N77 X74.629Y1.537Z-2.191\n\ -N78 X74.682Y1.590Z-2.177\n\ -N79 X74.894Y1.802Z-2.066\n\ -N80 X75.000Y1.908Z-2.046\n\ -N81 Y2.087Z-2.051\n\ -N82 X74.839Y1.927Z-2.062\n\ -N83 X74.679Y1.766Z-2.136\n\ -N84 X74.625Y1.713Z-2.170\n\ -N85 X74.572Y1.659Z-2.180\n\ -N86 X74.358Y1.445Z-2.157\n\ -N87 X74.197Y1.284Z-2.195\n\ -N88 X74.090Y1.177Z-2.245\n\ -N89 X73.983Y1.070Z-2.225\n\ -N90 X73.823Y0.910Z-2.138\n\ -N91 X73.394Y0.482Z-1.917\n\ -N92 X73.180Y0.268Z-1.825\n\ -N93 X72.966Y0.054Z-1.711\n\ -N94 X72.913Y-0.000Z-1.693\n\ -N95 X72.733Z-1.646\n\ -N96 X72.997Y0.264Z-1.753\n\ -N97 X73.260Y0.527Z-1.902\n\ -N98 X73.629Y0.896Z-2.077\n\ -N99 X73.840Y1.107Z-2.224\n\ -N100 X73.893Y1.160Z-2.245\n\ -N101 X75Y0Z0\n"; - diff --git a/src/text_parser.c b/src/text_parser.c deleted file mode 100644 index 69cc432..0000000 --- a/src/text_parser.c +++ /dev/null @@ -1,271 +0,0 @@ -/* - * text_parser.c - text parser for TinyG - * This file is part of the TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include "tinyg.h" -#include "config.h" -#include "canonical_machine.h" -#include "text_parser.h" -#include "json_parser.h" -#include "report.h" -#include "help.h" -#include "util.h" - -txtSingleton_t txt; // declare the singleton for either __TEXT_MODE setting - -#ifndef __TEXT_MODE - -stat_t text_parser_stub(char_t *str) {return STAT_OK;} -void text_response_stub(const stat_t status, char_t *buf) {} -void text_print_list_stub (stat_t status, uint8_t flags) {} -void tx_print_stub(nvObj_t *nv) {} - -#else // __TEXT_MODE - -static stat_t _text_parser_kernal(char_t *str, nvObj_t *nv); - -/****************************************************************************** - * text_parser() - update a config setting from a text block (text mode) - * _text_parser_kernal() - helper for above - * - * Use cases handled: - * - $xfr=1200 set a parameter (strict separators)) - * - $xfr 1200 set a parameter (relaxed separators) - * - $xfr display a parameter - * - $x display a group - * - ? generate a status report (multiline format) - */ -stat_t text_parser(char_t *str) { - nvObj_t *nv = nv_reset_nv_list(); // returns first object in the body - stat_t status = STAT_OK; - - // trap special displays - if (str[0] == '?') { // handle status report case - sr_run_text_status_report(); - return STAT_OK; - } - - if (str[0] == 'H') { // print help screens - help_general((nvObj_t *)0); - return STAT_OK; - } - - // pre-process the command - if ((str[0] == '$') && (str[1] == 0)) // treat a lone $ as a sys request - strcat(str, "sys"); - - // parse and execute the command (only processes 1 command per line) - ritorno(_text_parser_kernal(str, nv)); // run the parser to decode the command - if ((nv->valuetype == TYPE_0) || (nv->valuetype == TYPE_PARENT)) { - if (nv_get(nv) == STAT_COMPLETE) // populate value, group values, or run uber-group displays - return STAT_OK; // return for uber-group displays so they don't print twice - - } else { // process SET and RUN commands - if (cm.machine_state == MACHINE_ALARM) return STAT_MACHINE_ALARMED; - status = nv_set(nv); // set (or run) single value - if (status == STAT_OK) nv_persist(nv); // conditionally persist depending on flags in array - } - - nv_print_list(status, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); // print the results - return status; -} - - -static stat_t _text_parser_kernal(char_t *str, nvObj_t *nv) { - char_t *rd, *wr; // read and write pointers - char_t separators[] = {" =:|\t"}; // RELAXED: any separator someone might use - - // pre-process and normalize the string - nv_copy_string(nv, str); // make a copy for eventual reporting - if (*str == '$') str++; // ignore leading $ - - for (rd = wr = str; *rd != 0; rd++, wr++) { - *wr = tolower(*rd); // convert string to lower case - if (*rd == ',') { *wr = *(++rd);} // skip over commas - } - *wr = 0; // terminate the string - - // parse fields into the nv struct - nv->valuetype = TYPE_0; - if ((rd = strpbrk(str, separators)) == 0) // no value part - strncpy(nv->token, str, TOKEN_LEN); - - else { - *rd = 0; // terminate at end of name - strncpy(nv->token, str, TOKEN_LEN); - str = ++rd; - nv->value = strtof(str, &rd); // rd used as end pointer - if (rd != str) nv->valuetype = TYPE_FLOAT; - } - - // validate and post-process the token - if ((nv->index = nv_get_index("", nv->token)) == NO_MATCH) // get index or fail it - return STAT_UNRECOGNIZED_NAME; - - strcpy_P(nv->group, cfgArray[nv->index].group); // capture the group string if there is one - - // see if you need to strip the token - if (nv->group[0] != 0) { - wr = nv->token; - rd = nv->token + strlen(nv->group); - while (*rd != 0) *(wr)++ = *(rd)++; - *wr = 0; - } - - return STAT_OK; -} - - -/// text mode responses -static const char prompt_ok[] PROGMEM = "tinyg [%s] ok> "; -static const char prompt_err[] PROGMEM = "tinyg [%s] err: %s: %s "; - -void text_response(const stat_t status, char_t *buf) { - if (txt.text_verbosity == TV_SILENT) return; // skip all this - - char units[] = "inch"; - if (cm_get_units_mode(MODEL) != INCHES) strcpy(units, "mm"); - - if ((status == STAT_OK) || (status == STAT_EAGAIN) || (status == STAT_NOOP)) - fprintf_P(stderr, prompt_ok, units); - else fprintf_P(stderr, prompt_err, units, get_status_message(status), buf); - - nvObj_t *nv = nv_body + 1; - - if (nv_get_type(nv) == NV_TYPE_MESSAGE) fprintf(stderr, (char *)*nv->stringp); - - fprintf(stderr, "\n"); -} - - -/***** PRINT FUNCTIONS ******************************************************** - * json_print_list() - command to select and produce a JSON formatted output - * text_print_inline_pairs() - * text_print_inline_values() - * text_print_multiline_formatted() - */ -void text_print_list(stat_t status, uint8_t flags) { - switch (flags) { - case TEXT_NO_PRINT: break; - case TEXT_INLINE_PAIRS: text_print_inline_pairs(nv_body); break; - case TEXT_INLINE_VALUES: text_print_inline_values(nv_body); break; - case TEXT_MULTILINE_FORMATTED: text_print_multiline_formatted(nv_body); - } -} - - -void text_print_inline_pairs(nvObj_t *nv) { - uint32_t *v = (uint32_t*)&nv->value; - for (uint8_t i=0; ivaluetype) { - case TYPE_PARENT: if ((nv = nv->nx) == 0) return; continue; // 0 means parent with no child - case TYPE_FLOAT: - preprocess_float(nv); - fntoa(global_string_buf, nv->value, nv->precision); - fprintf_P(stderr, PSTR("%s:%s"), nv->token, global_string_buf); - break; - case TYPE_INTEGER: fprintf_P(stderr,PSTR("%s:%1.0f"), nv->token, nv->value); break; - case TYPE_DATA: fprintf_P(stderr,PSTR("%s:%lu"), nv->token, *v); break; - case TYPE_STRING: fprintf_P(stderr,PSTR("%s:%s"), nv->token, *nv->stringp); break; - case TYPE_EMPTY: fprintf_P(stderr,PSTR("\n")); return; - } - if ((nv = nv->nx) == 0) return; - if (nv->valuetype != TYPE_EMPTY) { fprintf_P(stderr,PSTR(","));} - } -} - - -void text_print_inline_values(nvObj_t *nv) { - uint32_t *v = (uint32_t*)&nv->value; - for (uint8_t i=0; ivaluetype) { - case TYPE_PARENT: if ((nv = nv->nx) == 0) return; continue; // 0 means parent with no child - case TYPE_FLOAT: - preprocess_float(nv); - fntoa(global_string_buf, nv->value, nv->precision); - fprintf_P(stderr,PSTR("%s"), global_string_buf); - break; - case TYPE_INTEGER: fprintf_P(stderr,PSTR("%1.0f"), nv->value); break; - case TYPE_DATA: fprintf_P(stderr,PSTR("%lu"), *v); break; - case TYPE_STRING: fprintf_P(stderr,PSTR("%s"), *nv->stringp); break; - case TYPE_EMPTY: fprintf_P(stderr,PSTR("\n")); return; - } - - if ((nv = nv->nx) == 0) return; - if (nv->valuetype != TYPE_EMPTY) { fprintf_P(stderr,PSTR(","));} - } -} - - -void text_print_multiline_formatted(nvObj_t *nv) { - for (uint8_t i=0; ivaluetype != TYPE_PARENT) { - preprocess_float(nv); - nv_print(nv); - } - - if ((nv = nv->nx) == 0) return; - if (nv->valuetype == TYPE_EMPTY) break; - } -} - - -/// Text print primitives using generic formats -static const char fmt_str[] PROGMEM = "%s\n"; // generic format for string message (with no formatting) -static const char fmt_ui8[] PROGMEM = "%d\n"; // generic format for ui8s -static const char fmt_int[] PROGMEM = "%lu\n"; // generic format for ui16's and ui32s -static const char fmt_flt[] PROGMEM = "%f\n"; // generic format for floats - -void tx_print_nul(nvObj_t *nv) {} -void tx_print_str(nvObj_t *nv) {text_print_str(nv, fmt_str);} -void tx_print_ui8(nvObj_t *nv) {text_print_ui8(nv, fmt_ui8);} -void tx_print_int(nvObj_t *nv) {text_print_int(nv, fmt_int);} -void tx_print_flt(nvObj_t *nv) {text_print_flt(nv, fmt_flt);} - - -/* - * Text print primitives using external formats - * - * NOTE: format's are passed in as flash strings (PROGMEM) - */ -void text_print_nul(nvObj_t *nv, const char *format) {fprintf_P(stderr, format);} // just print the format string -void text_print_str(nvObj_t *nv, const char *format) {fprintf_P(stderr, format, *nv->stringp);} -void text_print_ui8(nvObj_t *nv, const char *format) {fprintf_P(stderr, format, (uint8_t)nv->value);} -void text_print_int(nvObj_t *nv, const char *format) {fprintf_P(stderr, format, (uint32_t)nv->value);} -void text_print_flt(nvObj_t *nv, const char *format) {fprintf_P(stderr, format, nv->value);} - - -void text_print_flt_units(nvObj_t *nv, const char *format, const char *units) { - fprintf_P(stderr, format, nv->value, units); -} - - -/// Formatted print supporting the text parser -static const char fmt_tv[] PROGMEM = "[tv] text verbosity%15d [0=silent,1=verbose]\n"; - -void tx_print_tv(nvObj_t *nv) {text_print_ui8(nv, fmt_tv);} - -#endif // __TEXT_MODE diff --git a/src/text_parser.h b/src/text_parser.h deleted file mode 100644 index 2e62b3b..0000000 --- a/src/text_parser.h +++ /dev/null @@ -1,102 +0,0 @@ -/* - * text_parser.h - text parser and text mode support for tinyg2 - * This file is part of the TinyG project - * - * Copyright (c) 2013 - 2014 Alden S. Hart, Jr. - * - * 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 . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#ifndef TEXT_PARSER_H_ONCE -#define TEXT_PARSER_H_ONCE - -enum textVerbosity { - TV_SILENT = 0, // no response is provided - TV_VERBOSE // response is provided. Error responses ech message and failed commands -}; - -enum textFormats { // text output print modes - TEXT_NO_PRINT = 0, // don't print anything if you find yourself in TEXT mode - TEXT_INLINE_PAIRS, // print key:value pairs as comma separated pairs - TEXT_INLINE_VALUES, // print values as commas separated values - TEXT_MULTILINE_FORMATTED // print formatted values on separate lines with formatted print per line -}; - -typedef struct txtSingleton { // text mode data - - /*** config values (PUBLIC) ***/ - - char_t format[NV_FORMAT_LEN+1]; - - /*** runtime values (PRIVATE) ***/ - - uint8_t text_verbosity; // see enum in this file for settings - -} txtSingleton_t; -extern txtSingleton_t txt; - -/**** Global Scope Functions ****/ - -#ifdef __TEXT_MODE - - stat_t text_parser(char_t *str); - void text_response(const stat_t status, char_t *buf); - void text_print_list(stat_t status, uint8_t flags); - void text_print_inline_pairs(nvObj_t *nv); - void text_print_inline_values(nvObj_t *nv); - void text_print_multiline_formatted(nvObj_t *nv); - - void tx_print_nul(nvObj_t *nv); - void tx_print_str(nvObj_t *nv); - void tx_print_ui8(nvObj_t *nv); - void tx_print_int(nvObj_t *nv); - void tx_print_flt(nvObj_t *nv); - - void text_print_nul(nvObj_t *nv, const char *format); - void text_print_str(nvObj_t *nv, const char *format); - void text_print_ui8(nvObj_t *nv, const char *format); - void text_print_int(nvObj_t *nv, const char *format); - void text_print_flt(nvObj_t *nv, const char *format); - void text_print_flt_units(nvObj_t *nv, const char *format, const char *units); - - void tx_print_tv(nvObj_t *nv); - -#else - - #define text_parser text_parser_stub - #define text_response text_response_stub - #define text_print_list text_print_list_stub - #define tx_print_nul tx_print_stub - #define tx_print_ui8 tx_print_stub - #define tx_print_int tx_print_stub - #define tx_print_flt tx_print_stub - #define tx_print_str tx_print_stub - #define tx_print_tv tx_print_stub - - void tx_print_stub(nvObj_t *nv); - -#endif - -stat_t text_parser_stub(char_t *str); -void text_response_stub(const stat_t status, char_t *buf); -void text_print_list_stub(stat_t status, uint8_t flags); - -#endif // End of include guard: TEXT_PARSER_H_ONCE diff --git a/src/tmc2660.c b/src/tmc2660.c index ddc1e9c..3d46ffc 100644 --- a/src/tmc2660.c +++ b/src/tmc2660.c @@ -29,13 +29,16 @@ \******************************************************************************/ -#include "tinyg.h" -#include "config.h" #include "tmc2660.h" +#include "status.h" #include #include +#include + #include +#include +#include typedef enum { @@ -48,6 +51,7 @@ typedef enum { SPI_STATE_SELECT, SPI_STATE_WRITE, SPI_STATE_READ, + SPI_STATE_QUIT, } spi_state_t; typedef struct { @@ -55,8 +59,8 @@ typedef struct { uint8_t reset; uint8_t reg; - int16_t mstep; - uint8_t status; + uint16_t sguard; + uint8_t flags; uint32_t regs[5]; } tmc2660_driver_t; @@ -136,7 +140,7 @@ void spi_next() { break; case TMC2660_STATE_MONITOR: - spi_out = reg_addrs[TMC2660_DRVCONF] | drv->regs[TMC2660_DRVCONF]; + spi_out = reg_addrs[TMC2660_DRVCTRL] | drv->regs[TMC2660_DRVCTRL]; break; case TMC2660_STATE_RESET: @@ -155,6 +159,7 @@ void spi_next() { case SPI_STATE_READ: // Deselect driver spi_cs(driver, 0); + spi_state = SPI_STATE_SELECT; switch (drv->state) { case TMC2660_STATE_CONFIG: @@ -166,9 +171,8 @@ void spi_next() { case TMC2660_STATE_MONITOR: // Read response - drv->mstep = (int16_t)((spi_in >> 14) & 0x1ff); - if (spi_in & (1UL << 19)) drv->mstep = -drv->mstep; - drv->status = spi_in >> 4; + drv->sguard = (uint16_t)((spi_in >> 14) & 0x1ff); + drv->flags = spi_in >> 4; if (drv->reset) { drv->state = TMC2660_STATE_RESET; @@ -177,6 +181,7 @@ void spi_next() { } else if (++driver == TMC2660_NUM_DRIVERS) { driver = 0; spi_delay = 500; + //spi_state = SPI_STATE_QUIT; break; } break; @@ -187,8 +192,9 @@ void spi_next() { } // Next state (delay set above) - spi_state = SPI_STATE_SELECT; break; + + case SPI_STATE_QUIT: break; } TMC2660_TIMER.PER = spi_delay; @@ -216,18 +222,19 @@ void tmc2660_init() { drivers[i].state = TMC2660_STATE_CONFIG; drivers[i].reg = 0; - drivers[i].regs[TMC2660_DRVCTRL] = TMC2660_DRVCTRL_DEDGE | TMC2660_DRVCTRL_MRES_8; + drivers[i].regs[TMC2660_DRVCTRL] = TMC2660_DRVCTRL_DEDGE | + TMC2660_DRVCTRL_MRES_8; drivers[i].regs[TMC2660_CHOPCONF] = TMC2660_CHOPCONF_TBL_36 | - TMC2660_CHOPCONF_HEND(0) | TMC2660_CHOPCONF_HSTART(4) | + TMC2660_CHOPCONF_HEND(3) | TMC2660_CHOPCONF_HSTART(7) | TMC2660_CHOPCONF_TOFF(4); //drivers[i].regs[TMC2660_CHOPCONF] = TMC2660_CHOPCONF_TBL_36 | // TMC2660_CHOPCONF_CHM | TMC2660_CHOPCONF_HEND(7) | - // TMC2660_CHOPCONF_HSTART(6) | TMC2660_CHOPCONF_TOFF(7); - //drivers[i].regs[TMC2660_SMARTEN] = TMC2660_SMARTEN_SEIMIN | - // TMC2660_SMARTEN_MAX(2) | TMC2660_SMARTEN_MIN(2); + // TMC2660_CHOPCONF_FASTD(6) | TMC2660_CHOPCONF_TOFF(7); + drivers[i].regs[TMC2660_SMARTEN] = TMC2660_SMARTEN_SEIMIN | + TMC2660_SMARTEN_MAX(2) | TMC2660_SMARTEN_MIN(2); drivers[i].regs[TMC2660_SGCSCONF] = TMC2660_SGCSCONF_SFILT | TMC2660_SGCSCONF_THRESH(63) | TMC2660_SGCSCONF_CS(6); - drivers[i].regs[TMC2660_DRVCONF] = TMC2660_DRVCONF_RDSEL_MSTEP; + drivers[i].regs[TMC2660_DRVCONF] = TMC2660_DRVCONF_RDSEL_SG; } // Setup pins @@ -268,18 +275,13 @@ void tmc2660_init() { } -uint8_t tmc2660_status(int driver) { - return drivers[driver].status; -} - - -uint16_t tmc2660_step(int driver) { - return drivers[driver].mstep; +uint8_t tmc2660_flags(int driver) { + return driver < TMC2660_NUM_DRIVERS ? drivers[driver].flags : 0; } void tmc2660_reset(int driver) { - drivers[driver].reset = 1; + if (driver < TMC2660_NUM_DRIVERS) drivers[driver].reset = 1; } @@ -297,59 +299,61 @@ int tmc2660_all_ready() { } -static void tmc2660_get_status_flags(uint8_t status, char buf[35]) { +void tmc2660_get_flags(uint8_t flags, char buf[35]) { buf[0] = 0; - if (TMC2660_DRVSTATUS_STST & status) strcat(buf, "stst,"); - if (TMC2660_DRVSTATUS_OLB & status) strcat(buf, "olb,"); - if (TMC2660_DRVSTATUS_OLA & status) strcat(buf, "ola,"); - if (TMC2660_DRVSTATUS_S2GB & status) strcat(buf, "s2gb,"); - if (TMC2660_DRVSTATUS_S2GA & status) strcat(buf, "s2ga,"); - if (TMC2660_DRVSTATUS_OTPW & status) strcat(buf, "otpw,"); - if (TMC2660_DRVSTATUS_OT & status) strcat(buf, "ot,"); - if (TMC2660_DRVSTATUS_SG & status) strcat(buf, "sg,"); + if (TMC2660_DRVSTATUS_STST & flags) strcat(buf, "stst,"); + if (TMC2660_DRVSTATUS_OLB & flags) strcat(buf, "olb,"); + if (TMC2660_DRVSTATUS_OLA & flags) strcat(buf, "ola,"); + if (TMC2660_DRVSTATUS_S2GB & flags) strcat(buf, "s2gb,"); + if (TMC2660_DRVSTATUS_S2GA & flags) strcat(buf, "s2ga,"); + if (TMC2660_DRVSTATUS_OTPW & flags) strcat(buf, "otpw,"); + if (TMC2660_DRVSTATUS_OT & flags) strcat(buf, "ot,"); + if (TMC2660_DRVSTATUS_SG & flags) strcat(buf, "sg,"); if (buf[0] != 0) buf[strlen(buf) - 1] = 0; // Remove last comma } -static const char mst_fmt[] PROGMEM = "Motor %i step: %i\n"; -static const char mfl_fmt[] PROGMEM = "Motor %i flags: %s\n"; +uint8_t get_dflags(int driver) {return drivers[driver].flags;} -void tmc2660_print_motor_step(nvObj_t *nv) { - int driver = nv->token[3] - '1'; - fprintf_P(stderr, mst_fmt, driver + 1, drivers[driver].mstep); +float get_dcur(int driver) { + uint8_t x = drivers[driver].regs[TMC2660_SGCSCONF] & 31; + return (x + 1) / 32.0; } -void tmc2660_print_motor_flags(nvObj_t *nv) { - int driver = nv->token[3] - '1'; +void set_dcur(int driver, float value) { + if (value < 0 || 1 < value) return; - char buf[35]; - tmc2660_get_status_flags(drivers[driver].status, buf); + uint8_t x = value * 32.0 - 1; + if (x < 0) x = 1; - fprintf_P(stderr, mfl_fmt, driver + 1, buf); + tmc2660_driver_t *d = &drivers[driver]; + d->regs[TMC2660_SGCSCONF] = (d->regs[TMC2660_SGCSCONF] & ~31) | x; + + tmc2660_reset(driver); } -stat_t tmc2660_get_motor_step(nvObj_t *nv) { - int driver = nv->token[3] - '1'; +uint16_t get_sguard(int driver) { + return drivers[driver].sguard; +} - nv->value = drivers[driver].mstep; - nv->valuetype = TYPE_INTEGER; - return STAT_OK; +int8_t get_sgt(int driver) { + uint8_t x = (drivers[driver].regs[TMC2660_SGCSCONF] & 0x7f) >> 8; + return (x & (1 << 6)) ? (x & 0xc0) : x; } -stat_t tmc2660_get_motor_flags(nvObj_t *nv) { - int driver = nv->token[3] - '1'; - char buf[35]; +void set_sgt(int driver, int8_t value) { + if (value < -64 || 63 < value) return; - tmc2660_get_status_flags(drivers[driver].status, buf); - nv_copy_string(nv, buf); - nv->valuetype = TYPE_STRING; + tmc2660_driver_t *d = &drivers[driver]; + d->regs[TMC2660_SGCSCONF] = (d->regs[TMC2660_SGCSCONF] & ~31) | + TMC2660_SGCSCONF_THRESH(value); - return STAT_OK; + tmc2660_reset(driver); } diff --git a/src/tmc2660.h b/src/tmc2660.h index ec99b80..2dee6f0 100644 --- a/src/tmc2660.h +++ b/src/tmc2660.h @@ -32,6 +32,8 @@ #ifndef TMC2660_H #define TMC2660_H +#include "config.h" + #include #define TMC2660_SPI_PORT PORTC @@ -51,22 +53,16 @@ #define TMC2660_SPI_SSB_PORT PORTB #define TMC2660_SPI_SSB_PIN 3 -#define TMC2660_NUM_DRIVERS 3 +#define TMC2660_NUM_DRIVERS MOTORS #define TMC2660_TIMER TCC1 void tmc2660_init(); uint8_t tmc2660_status(int driver); -uint16_t tmc2660_step(int driver); void tmc2660_reset(int driver); int tmc2660_ready(int driver); int tmc2660_all_ready(); -void tmc2660_print_motor_step(nvObj_t *nv); -void tmc2660_print_motor_flags(nvObj_t *nv); -stat_t tmc2660_get_motor_step(nvObj_t *nv); -stat_t tmc2660_get_motor_flags(nvObj_t *nv); - #define TMC2660_DRVCTRL 0 #define TMC2660_DRVCTRL_ADDR (0UL << 18) #define TMC2660_DRVCTRL_PHA (1UL << 17) @@ -102,7 +98,7 @@ stat_t tmc2660_get_motor_flags(nvObj_t *nv); #define TMC2660_CHOPCONF_HEND(x) ((((int32_t)x + 3) & 0xf) << 7) #define TMC2660_CHOPCONF_SWO(x) ((((int32_t)x + 3) & 0xf) << 7) #define TMC2660_CHOPCONF_HSTART(x) ((((int32_t)x - 1) & 7) << 4) -#define TMC2660_CHOPCONF_FASTD(x) ((((int32_t)x & 8) << 11) | (x & 7) << 4)) +#define TMC2660_CHOPCONF_FASTD(x) ((((int32_t)x & 8) << 11) | ((x & 7) << 4)) #define TMC2660_CHOPCONF_TOFF_TBL (1 << 0) #define TMC2660_CHOPCONF_TOFF(x) (((int32_t)x & 0xf) << 0) diff --git a/src/util.c b/src/util.c index a254506..c152bf9 100644 --- a/src/util.c +++ b/src/util.c @@ -24,16 +24,21 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/* util contains a dog's breakfast of supporting functions that are not specific to tinyg: - * including: + +/* util contains: * - math and min/max utilities and extensions * - vector manipulation utilities */ -#include "tinyg.h" #include "util.h" -#include "xmega/xmega_rtc.h" +#include + +#include +#include +#include +#include +#include float vector[AXES]; // statically allocated global for vector utilities @@ -142,15 +147,15 @@ float max4(float x1, float x2, float x3, float x4) { /// isdigit that also accepts plus, minus, and decimal point -uint8_t isnumber(char_t c) { +uint8_t isnumber(char c) { return c == '.' || c == '-' || c == '+' || isdigit(c); } /// Add escapes to a string - currently for quotes only -char_t *escape_string(char_t *dst, char_t *src) { - char_t c; - char_t *start_dst = dst; +char *escape_string(char *dst, char *src) { + char c; + char *start_dst = dst; while ((c = *(src++)) != 0) { // 0 if (c == '"') *(dst++) = '\\'; @@ -161,24 +166,12 @@ char_t *escape_string(char_t *dst, char_t *src) { } -/* - * Return an AVR style progmem string as a RAM string. - * - * This function copies a string from FLASH to a pre-allocated RAM buffer - - * see main.c for allocation and max length. - */ -char_t *pstr2str(const char *pgm_string) { - strncpy_P(global_string_buf, pgm_string, MESSAGE_LEN); - return global_string_buf; -} - - /* * Return ASCII string given a float and a decimal precision value * * Returns length of string, less the terminating 0 character */ -char_t fntoa(char_t *str, float n, uint8_t precision) { +char fntoa(char *str, float n, uint8_t precision) { // handle special cases if (isnan(n)) { strcpy(str, "nan"); @@ -191,15 +184,15 @@ char_t fntoa(char_t *str, float n, uint8_t precision) { } switch (precision) { - case 0: return (char_t)sprintf((char *)str, "%0.0f", (double)n); - case 1: return (char_t)sprintf((char *)str, "%0.1f", (double)n); - case 2: return (char_t)sprintf((char *)str, "%0.2f", (double)n); - case 3: return (char_t)sprintf((char *)str, "%0.3f", (double)n); - case 4: return (char_t)sprintf((char *)str, "%0.4f", (double)n); - case 5: return (char_t)sprintf((char *)str, "%0.5f", (double)n); - case 6: return (char_t)sprintf((char *)str, "%0.6f", (double)n); - case 7: return (char_t)sprintf((char *)str, "%0.7f", (double)n); - default: return (char_t)sprintf((char *)str, "%f", (double)n); + 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); } } @@ -214,7 +207,7 @@ char_t fntoa(char_t *str, float n, uint8_t precision) { */ #define HASHMASK 9999 -uint16_t compute_checksum(char_t const *string, const uint16_t length) { +uint16_t compute_checksum(char const *string, const uint16_t length) { uint32_t h = 0; uint16_t len = strlen(string); @@ -225,9 +218,3 @@ uint16_t compute_checksum(char_t const *string, const uint16_t length) { return h % HASHMASK; } - - -/// This is a hack to get around some compatibility problems -uint32_t SysTickTimer_getValue() { - return rtc.sys_ticks; -} diff --git a/src/util.h b/src/util.h index 295d1d7..e7ee9dd 100644 --- a/src/util.h +++ b/src/util.h @@ -33,14 +33,18 @@ * - support for debugging routines */ -#ifndef UTIL_H_ONCE -#define UTIL_H_ONCE +#ifndef UTIL_H +#define UTIL_H + +#include "config.h" + +#include // Vector utilities extern float vector[AXES]; // vector of axes for passing to subroutines -#define clear_vector(a) (memset(a,0,sizeof(a))) -#define copy_vector(d,s) (memcpy(d,s,sizeof(d))) +#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[]); @@ -55,33 +59,25 @@ float max4(float x1, float x2, float x3, float x4); // String utilities -uint8_t isnumber(char_t c); -char_t *escape_string(char_t *dst, char_t *src); -char_t *pstr2str(const char *pgm_string); -char_t fntoa(char_t *str, float n, uint8_t precision); -uint16_t compute_checksum(char_t const *string, const uint16_t length); - -// Other utilities -uint32_t SysTickTimer_getValue(); - -// Math support -#ifndef square -#define square(x) ((x)*(x)) /* UNSAFE */ -#endif +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; }) +#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. + + In addition, BSD licensing may be granted on a case by case basis + by written permission from at least one of the copyright holders. + You may request written permission by emailing the authors. + + For information regarding this software email: + Joseph Coffland + joseph@buildbotics.com + +\******************************************************************************/ + +#include "vars.h" + +#include "cpp_magic.h" +#include "status.h" +#include "config.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +// Type names +static const char bool_name [] PROGMEM = ""; +#define TYPE_NAME(TYPE) static const char TYPE##_name [] PROGMEM = "<" #TYPE ">" +MAP(TYPE_NAME, SEMI, string, float, int8_t, uint8_t, uint16_t); + + +static void var_print_bool(bool x) { + printf_P(x ? PSTR("true") : PSTR("false")); +} + + +static void var_print_float(float x) { + char buf[20]; + + int len = sprintf_P(buf, PSTR("%.6f"), x); + + // Remove trailing zeros + for (int i = len; 0 < i; i--) { + if (buf[i - 1] == '.') buf[i - 1] = 0; + else if (buf[i - 1] == '0') { + buf[i - 1] = 0; + continue; + } + + break; + } + + printf(buf); +} + + +static void var_print_int8_t(int8_t x) { + printf_P(PSTR("%"PRIi8), x); +} + + +static void var_print_uint8_t(uint8_t x) { + printf_P(PSTR("%"PRIu8), x); +} + + +static void var_print_uint16_t(uint16_t x) { + printf_P(PSTR("%"PRIu16), x); +} + + +static bool var_parse_bool(const char *value) { + return strcmp_P(value, PSTR("false")); +} + + +static float var_parse_float(const char *value) { + return strtod(value, 0); +} + + +static int8_t var_parse_int8_t(const char *value) { + return strtol(value, 0, 0); +} + + +static uint16_t var_parse_uint16_t(const char *value) { + return strtol(value, 0, 0); +} + + +// Var forward declarations +#define VAR(NAME, TYPE, INDEX, SET, ...) \ + TYPE get_##NAME(IF(INDEX)(int index)); \ + IF(SET) \ + (void set_##NAME(IF(INDEX)(int index,) TYPE value);) + +#include "vars.def" +#undef VAR + +// Var names, count & help +#define VAR(NAME, TYPE, INDEX, SET, DEFAULT, HELP) \ + static const char NAME##_name[] PROGMEM = #NAME; \ + static const int NAME##_count = INDEX ? INDEX : 1; \ + static const char NAME##_help[] PROGMEM = HELP; + +#include "vars.def" +#undef VAR + +// Last value +#define VAR(NAME, TYPE, INDEX, ...) \ + static TYPE NAME##_last IF(INDEX)([INDEX]); + +#include "vars.def" +#undef VAR + + +void vars_init() { +#define VAR(NAME, TYPE, INDEX, SET, DEFAULT, ...) \ + IF(SET) \ + (IF(INDEX)(for (int i = 0; i < INDEX; i++)) { \ + set_##NAME(IF(INDEX)(i,) DEFAULT); \ + (NAME##_last)IF(INDEX)([i]) = DEFAULT; \ + }) + +#include "vars.def" +#undef VAR +} + + +void vars_report(bool full) { + bool reported = false; + + static const char value_fmt[] PROGMEM = "\"%S\":"; + static const char index_value_fmt[] PROGMEM = "\"%S%c\":"; + +#define VAR(NAME, TYPE, INDEX, ...) \ + IF(INDEX)(for (int i = 0; i < NAME##_count; i++)) { \ + TYPE value = get_##NAME(IF(INDEX)(i)); \ + \ + if (value != (NAME##_last)IF(INDEX)([i]) || full) { \ + (NAME##_last)IF(INDEX)([i]) = value; \ + \ + if (!reported) { \ + reported = true; \ + putchar('{'); \ + } else putchar(','); \ + \ + printf_P \ + (IF_ELSE(INDEX)(index_value_fmt, value_fmt), \ + NAME##_name IF(INDEX)(, INDEX##_LABEL[i])); \ + \ + var_print_##TYPE(value); \ + } \ + } + +#include "vars.def" +#undef VAR + + if (reported) printf("}\n"); +} + + +void vars_set(const char *name, const char *value) { + uint8_t i; + unsigned len = strlen(name); + + if (!len) return; + +#define VAR(NAME, TYPE, INDEX, SET, ...) \ + IF(SET) \ + (if (!strncmp_P(name, NAME##_name, IF_ELSE(INDEX)(len - 1, len))) { \ + IF(INDEX) \ + (i = strchr(INDEX##_LABEL, name[len - 1]) - INDEX##_LABEL; \ + if (INDEX <= i) return); \ + \ + set_##NAME(IF(INDEX)(i ,) var_parse_##TYPE(value)); \ + \ + return; \ + }) + +#include "vars.def" +#undef VAR +} + + +int vars_parser(char *vars) { + if (!*vars || !*vars++ == '{') return STAT_OK; + + while (*vars) { + while (isspace(*vars)) vars++; + if (*vars == '}') return STAT_OK; + if (*vars != '"') return STAT_COMMAND_NOT_ACCEPTED; + + // Parse name + vars++; // Skip " + const char *name = vars; + while (*vars && *vars != '"') vars++; + *vars++ = 0; + + while (isspace(*vars)) vars++; + if (*vars != ':') return STAT_COMMAND_NOT_ACCEPTED; + vars++; + while (isspace(*vars)) vars++; + + // Parse value + const char *value = vars; + while (*vars && *vars != ',' && *vars != '}') vars++; + if (*vars) { + char c = *vars; + *vars++ = 0; + vars_set(name, value); + if (c == '}') break; + } + } + + return STAT_OK; +} + + +void vars_print_help() { + static const char fmt[] PROGMEM = " %-8S %-10S %S\n"; + +#define VAR(NAME, TYPE, ...) printf_P(fmt, NAME##_name, TYPE##_name, NAME##_help); +#include "vars.def" +#undef VAR +} diff --git a/src/vars.def b/src/vars.def new file mode 100644 index 0000000..9b85c4f --- /dev/null +++ b/src/vars.def @@ -0,0 +1,48 @@ +/******************************************************************************\ + + This file is part of the TinyG firmware. + + Copyright (c) 2016, Buildbotics LLC + All rights reserved. + + The C! library is free software: you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation, either version 2.1 of + the License, or (at your option) any later version. + + The C! library 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 C! library. If not, see + . + + In addition, BSD licensing may be granted on a case by case basis + by written permission from at least one of the copyright holders. + You may request written permission by emailing the authors. + + For information regarding this software email: + Joseph Coffland + joseph@buildbotics.com + +\******************************************************************************/ + +#define MOTORS_LABEL "xyzabcuvw" + +// VAR(name, type, index, settable, default, help) +VAR(pos, float, MOTORS, 0, 0, "Current axis position") +VAR(vel, float, 0, 0, 0, "Current velosity") +VAR(ang, float, MOTORS, 1, 0, "Rotation angle in deg per full step") +VAR(trvl, float, MOTORS, 1, 0, "Travel in mm per revolution") +VAR(mstep, uint16_t, MOTORS, 1, 0, "Microsteps per full step") +VAR(pol, bool, MOTORS, 1, 0, "Normal or reversed") +VAR(mvel, uint16_t, MOTORS, 1, 0, "Maxium velocity in mm/min") +VAR(mjerk, uint16_t, MOTORS, 1, 0, "Maxium jerk in m/min^3") + +// Motor driver +VAR(dflags, uint8_t, MOTORS, 0, 0, "Current motor status flags") +VAR(dcur, float, MOTORS, 1, 0, "Driver current as a percentage") +VAR(sguard, uint16_t, MOTORS, 0, 0, "Driver StallGuard reading") +VAR(sgt, int8_t, MOTORS, 1, 0, "Driver StallGuard threshold -64 to 63") diff --git a/src/vars.h b/src/vars.h new file mode 100644 index 0000000..9656a78 --- /dev/null +++ b/src/vars.h @@ -0,0 +1,40 @@ +/******************************************************************************\ + + This file is part of the TinyG firmware. + + Copyright (c) 2016, Buildbotics LLC + All rights reserved. + + The C! library is free software: you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation, either version 2.1 of + the License, or (at your option) any later version. + + The C! library 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 C! library. If not, see + . + + In addition, BSD licensing may be granted on a case by case basis + by written permission from at least one of the copyright holders. + You may request written permission by emailing the authors. + + For information regarding this software email: + Joseph Coffland + joseph@buildbotics.com + +\******************************************************************************/ + +#pragma once + +#include + +void vars_init(); +void vars_report(bool full); +void vars_set(const char *name, const char *value); +int vars_parser(char *vars); +void vars_print_help(); diff --git a/src/xmega/xmega_eeprom.c b/src/xmega/xmega_eeprom.c deleted file mode 100644 index 2fa151a..0000000 --- a/src/xmega/xmega_eeprom.c +++ /dev/null @@ -1,497 +0,0 @@ -/* Xmega Non Volatile Memory functions - * - * Ahem. Before you waste days trying to figure out why none of this works - * in the simulator, you should realize that IT DOESN'T WORK IN THE WINAVR - * SIMULATOR (now I'll calm down now.). - * - * Then before you waste more time figuring out why it doesn't work AT ALL, - * realize that there is a serious bug in the xmega A3 processor family. - * This is documented in Atmel release note AVR1008 and in the chip Errata. - * This file contains workarounds to those problems. - * - * Some code was incorporated from the avr-xboot project: - * https://github.com/alexforencich/xboot - * http://code.google.com/p/avr-xboot/wiki/Documentation - * - * These refs were also helpful: - * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=669385 - * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&t=88416 - * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=printview&t=89810&start=0 - */ - -/*************************************************************************** - * - * XMEGA EEPROM driver source file. - * - * This file contains the function prototypes and enumerator - * definitions for various configuration parameters for the - * XMEGA EEPROM driver. - * - * The driver is not intended for size and/or speed critical code, - * since most functions are just a few lines of code, and the - * function call overhead would decrease code performance. The driver - * is intended for rapid prototyping and documentation purposes for - * getting started with the XMEGA EEPROM module. - * - * For size and/or speed critical code, it is recommended to copy the - * function contents directly into your application instead of making - * a function call (or just inline the functions, bonehead). - * - * Besides which, it doesn't work in the simulator, so how would - * you ever know? - * - * Notes: - * See AVR1315: Accessing the XMEGA EEPROM + Code eeprom_driver.c /.h - * - * Author: - * \author - * Original Author: Atmel Corporation: http://www.atmel.com - * Adapted by: Alden S. Hart Jr; 2010 - * - * Copyright (c) 2008, Atmel Corporation All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * - * 3. The name of ATMEL may not be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED - * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND - * SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY - * DIRECT,INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS - * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, - * STRICT LIABILITY, OR FART (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING - * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - **************************************************************************/ - -/* - * Modified to support Xmega family processors - * Modifications Copyright (c) 2010 Alden S. Hart, Jr. - */ - -#include -#include "xmega_eeprom.h" -#include -#include -#include -#include "../tinyg.h" // only used to define __UNIT_TEST_EEPROM. can be removed. - -#ifdef __UNIT_TEST_EEPROM -#include -#include -#include // for memset() -#include -#endif - -#define __USE_AVR1008_EEPROM // use the AVR1008 workaround code -#define ARBITRARY_MAX_LENGTH 80 // string max for NNVM write - - -// Inline assembly to support NVM operations -static inline void NVM_EXEC() { - void *z = (void *)&NVM_CTRLA; - - __asm__ volatile("out %[ccp], %[ioreg]" "\n\t" - "st z, %[cmdex]" - : - : [ccp] "I" (_SFR_IO_ADDR(CCP)), - [ioreg] "d" (CCP_IOREG_gc), - [cmdex] "r" (NVM_CMDEX_bm), - [z] "z" (z) - ); -} - -#ifdef __USE_AVR1008_EEPROM - - -// Interrupt handler for for EEPROM write "done" interrupt -ISR(NVM_EE_vect) { - NVM.INTCTRL = (NVM.INTCTRL & ~NVM_EELVL_gm); // Disable EEPROM interrupt -} - - -// Wrapper for NVM_EXEC that executes the workaround code -static inline void NVM_EXEC_WRAPPER() { - uint8_t sleepCtr = SLEEP.CTRL; // Save the Sleep register - // Set sleep mode to IDLE - SLEEP.CTRL = (SLEEP.CTRL & ~SLEEP.CTRL) | SLEEP_SMODE_IDLE_gc; - uint8_t statusStore = PMIC.STATUS; // Save the PMIC Status... - uint8_t pmicStore = PMIC.CTRL; //...and control registers - // Enable only hi interrupts - PMIC.CTRL = (PMIC.CTRL & ~(PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm)) | PMIC_HILVLEN_bm; - uint8_t globalInt = SREG; // Save SREG for later use - sei(); // Enable global interrupts - SLEEP.CTRL |= SLEEP_SEN_bm; // Set sleep enabled - uint8_t eepromintStore = NVM.INTCTRL; // Save eeprom int settings - NVM_EXEC(); // exec EEPROM command - NVM.INTCTRL = NVM_EELVL0_bm | NVM_EELVL1_bm;// Enable EEPROM int - sleep_cpu(); // Sleep before 2.5uS passed - SLEEP.CTRL = sleepCtr; // Restore sleep settings - PMIC.STATUS = statusStore; // Restore PMIC status... - PMIC.CTRL = pmicStore; //...and control registers - NVM.INTCTRL = eepromintStore; // Restore EEPROM int settings - SREG = globalInt; // Restore global int settings -} -#else -#define NVM_EXEC_WRAPPER NVM_EXEC -#endif // __USE_AVR1008_EEPROM - - -/* - * EEPROM_WriteString() - write string to EEPROM; may span multiple pages - * - * This function writes a character string to IO mapped EEPROM. - * If memory mapped EEPROM is enabled this function will not work. - * This functiom will cancel all ongoing EEPROM page buffer loading - * operations, if any. - * - * A string may span multiple EEPROM pages. For each affected page it: - * - loads the page buffer from EEPROM with the contents for that page - * - writes the string bytes for that page into the page buffer - * - performs an atomic write for that page (erase and write) - * - does the next page until the string is complete. - * - * If 'terminate' is TRUE, add a null to the string, if FALSE, don't. - * - * Note that only the page buffer locations that have been copied into - * the page buffer (during string copy) will be affected when writing to - * the EEPROM (using AtomicPageWrite). Page buffer locations that have not - * been loaded will be left untouched in EEPROM. - * - * address must be between 0 and top-of-EEPROM (0x0FFF in a 256 or 192) - * string must be null-terminated - * terminate set TRUE to write string termination 0 to EEPROM - * set FALSE to not write 0 (useful for writing "files") - * - * returns pointer to next EEPROM location past the last byte written - * - * Background: The xmega EEPROM is rated at 100,000 operations (endurance). - * The endurance figure is dominated by the erase operation. Reads do not - * affect the endurance and writes have minimal effect. Erases occur at - * the page level, so a strategy to minimize page erases is needed. This - * function is a reasonably high endurance solution since erases only occur - * once for each time a string crosses a page, as opposed to once per byte - * written (as in EEPROM_WriteByte()). A slightly higher endurance solution - * would be to store up multiple contiguous string writes and perform them - * as single page operations. - */ - -uint16_t EEPROM_WriteString(const uint16_t address, const char *buf, const uint8_t terminate) { - uint16_t addr = address; // local copy - uint8_t i = 0; // index into string - - EEPROM_DisableMapping(); - while (buf[i]) EEPROM_WriteByte(addr++, buf[i++]); - if (terminate) EEPROM_WriteByte(addr++, 0); - - return addr; // return next address in EEPROM -} - - -/* - * EEPROM_ReadString() - read string from EEPROM; may span multiple pages - * - * This function reads a character string to IO mapped EEPROM. - * If memory mapped EEPROM is enabled this function will not work. - * A string may span multiple EEPROM pages. - * - * address starting address of string in EEPROM space - * buf buffer to read string into - * size cutoff string and terminate at this length - * return next address past string termination - */ -uint16_t EEPROM_ReadString(const uint16_t address, char *buf, const uint16_t size) { - uint16_t addr = address; // local copy - uint16_t i = 0; // index into strings - - EEPROM_DisableMapping(); - - for (i = 0; i < size; i++) { - NVM.ADDR0 = addr & 0xFF; // set read address - NVM.ADDR1 = (addr++ >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - - EEPROM_WaitForNVM(); // Wait until NVM is not busy - NVM.CMD = NVM_CMD_READ_EEPROM_gc; // issue EEPROM Read command - NVM_EXEC(); - if (!(buf[i] = NVM.DATA0)) break; - } - - if (i == size) buf[i] = 0; // null terinate the buffer overflow case - - return addr; -} - - -/* - * EEPROM_WriteBytes() - write N bytes to EEPROM; may span multiple pages - * - * This function writes a byte buffer to IO mapped EEPROM. - * If memory mapped EEPROM is enabled this function will not work. - * This functiom will cancel all ongoing EEPROM page buffer loading - * operations, if any. - * - * Returns address past the write - */ -uint16_t EEPROM_WriteBytes(const uint16_t address, const int8_t *buf, const uint16_t size) { - uint16_t addr = address; // local copy - - EEPROM_DisableMapping(); - for (uint16_t i = 0; i < size; i++) - EEPROM_WriteByte(addr++, buf[i]); - - return addr; // return next address in EEPROM -} - - -/* - * EEPROM_ReadBytes() - read N bytes to EEPROM; may span multiple pages - * - * This function reads a character string to IO mapped EEPROM. - * If memory mapped EEPROM is enabled this function will not work. - * A string may span multiple EEPROM pages. - */ -uint16_t EEPROM_ReadBytes(const uint16_t address, int8_t *buf, const uint16_t size) { - uint16_t addr = address; // local copy - - EEPROM_DisableMapping(); - - for (uint16_t i = 0; i < size; i++) { - NVM.ADDR0 = addr & 0xFF; // set read address - NVM.ADDR1 = (addr++ >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - - EEPROM_WaitForNVM(); // Wait until NVM is not busy - NVM.CMD = NVM_CMD_READ_EEPROM_gc; // issue EEPROM Read command - NVM_EXEC(); - buf[i] = NVM.DATA0; - } - - return addr; -} - - -/* - * EEPROM_WaitForNVM() - Wait for any NVM access to finish - * - * This function blocks waiting for any NVM access to finish including EEPROM - * Use this function before any EEPROM accesses if you are not certain that - * any previous operations are finished yet, like an EEPROM write. - */ - -void EEPROM_WaitForNVM() { - while ((NVM.STATUS & NVM_NVMBUSY_bm) == NVM_NVMBUSY_bm); -} - - -/* - * EEPROM_FlushBuffer() - Flush temporary EEPROM page buffer. - * - * This function flushes the EEPROM page buffers, and will cancel - * any ongoing EEPROM page buffer loading operations, if any. - * This function also works for memory mapped EEPROM access. - * - * Note: The EEPROM write operations will automatically flush the buffer for you - */ -void EEPROM_FlushBuffer() { - EEPROM_WaitForNVM(); // Wait until NVM is not busy - - if ((NVM.STATUS & NVM_EELOAD_bm) != 0) { // Flush page buffer if necessary - NVM.CMD = NVM_CMD_ERASE_EEPROM_BUFFER_gc; - NVM_EXEC(); - } -} - - -/* - * EEPROM_WriteByte() - write one byte to EEPROM using IO mapping - * EEPROM_WriteByteByPage() - write one byte using page addressing (MACRO) - * - * This function writes one byte to EEPROM using IO-mapped access. - * If memory mapped EEPROM is enabled this function will not work. - * This function flushes the EEPROM page buffers, and will cancel - * any ongoing EEPROM page buffer loading operations, if any. - * - * address EEPROM address, between 0 and EEPROM_SIZE - * value Byte value to write to EEPROM. - * - * Note: DO NOT USE THIS FUNCTION IF YOU CAN AVOID IT AS ENDURANCE SUCKS - * Use EEPROM_WriteString(), or write a new routine for binary blocks. - */ -void EEPROM_WriteByte(uint16_t address, uint8_t value) { - EEPROM_DisableMapping(); // *** SAFETY *** - EEPROM_FlushBuffer(); // prevent unintentional write - NVM.CMD = NVM_CMD_LOAD_EEPROM_BUFFER_gc;// load page_load command - NVM.ADDR0 = address & 0xFF; // set buffer addresses - NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - NVM.DATA0 = value; // load write data - triggers EEPROM page buffer load - NVM.CMD = NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc;// Atomic Write (Erase&Write) - NVM_EXEC_WRAPPER(); // Load command, write protection signature & exec command -} - - -/* - * EEPROM_ReadByte() - Read one byte from EEPROM using IO mapping. - * EEPROM_ReadChar() - Read one char from EEPROM using IO mapping. - * EEPROM_ReadByteByPage() - Read one byte using page addressing - * EEPROM_ReadCharByPage() - Read one char using page addressing - * - * This function reads one byte from EEPROM using IO-mapped access. - * If memory mapped EEPROM is enabled, this function will not work. - * The other funcs are defined as macros. See the .h file. - * - * address EEPROM address, between 0 and EEPROM_SIZE - * returns byte value read from EEPROM. - */ -uint8_t EEPROM_ReadByte(uint16_t address) { - EEPROM_DisableMapping(); // *** SAFETY *** - EEPROM_WaitForNVM(); // Wait until NVM is not busy - NVM.ADDR0 = address & 0xFF; // set read address - NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - NVM.CMD = NVM_CMD_READ_EEPROM_gc; // issue EEPROM Read command - NVM_EXEC(); - - return NVM.DATA0; -} - - -/* - * EEPROM_LoadByte() - Load single byte into temporary page buffer. - * - * This function loads one byte into the temporary EEPROM page buffers. - * Make sure that the buffer is flushed before starting to load bytes. - * Also, if multiple bytes are loaded into the same location, they will - * be ANDed together, thus 0x55 and 0xAA will result in 0x00 in the buffer. - * If memory mapped EEPROM is enabled this function will not work. - * - * Note: Only one page buffer exists, thus only one page can be loaded with - * data and programmed into one page. If data needs to be written to - * different pages the loading and writing needs to be repeated. - * - * byteAddr EEPROM Byte address, between 0 and EEPROM_PAGESIZE. - * value Byte value to write to buffer. - */ -void EEPROM_LoadByte(uint8_t byteAddr, uint8_t value) { - EEPROM_DisableMapping(); // +++ SAFETY - EEPROM_WaitForNVM(); // wait until NVM is not busy - NVM.CMD = NVM_CMD_LOAD_EEPROM_BUFFER_gc; // prepare NVM command - NVM.ADDR0 = byteAddr & EEPROM_ADDR1_MASK_gm;// set address - NVM.ADDR1 = 0x00; - NVM.ADDR2 = 0x00; - NVM.DATA0 = value; // Set data, which triggers loading EEPROM page buffer -} - - -/* - * EEPROM_LoadPage() - Load entire page into temporary EEPROM page buffer. - * - * This function loads an entire EEPROM page from an SRAM buffer to - * the EEPROM page buffers. - * Make sure that the buffer is flushed before starting to load bytes. - * If memory mapped EEPROM is enabled this function will not work. - * - * Note: Only the lower part of the address is used to address the buffer. - * Therefore, no address parameter is needed. In the end, the data - * is written to the EEPROM page given by the address parameter to the - * EEPROM write page operation. - * - * values Pointer to SRAM buffer containing an entire page. - */ -void EEPROM_LoadPage(const uint8_t * values) { - EEPROM_DisableMapping(); // +++ SAFETY - EEPROM_WaitForNVM(); // wait until NVM not busy - NVM.CMD = NVM_CMD_LOAD_EEPROM_BUFFER_gc; - NVM.ADDR1 = 0x00; // set upper addr's to zero - NVM.ADDR2 = 0x00; - - for (uint8_t i = 0; i < EEPROM_PAGESIZE; ++i) { // load multiple bytes - NVM.ADDR0 = i; - NVM.DATA0 = *values; - ++values; - } -} - - -/* - * EEPROM_AtomicWritePage() - Write already loaded page into EEPROM. - * - * This function writes the contents of an already loaded EEPROM page - * buffer into EEPROM memory. As this is an atomic write, the page in - * EEPROM will be erased automatically before writing. Note that only the - * page buffer locations that have been loaded will be used when writing - * to EEPROM. Page buffer locations that have not been loaded will be left - * untouched in EEPROM. - * - * pageAddr EEPROM Page address between 0 and EEPROM_SIZE/EEPROM_PAGESIZE - */ -inline void EEPROM_AtomicWritePage(uint8_t pageAddr) { - EEPROM_WaitForNVM(); // wait until NVM not busy - uint16_t address = (uint16_t)(pageAddr * EEPROM_PAGESIZE); - NVM.ADDR0 = address & 0xFF; // set addresses - NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - NVM.CMD = NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc; // erase & write page command - NVM_EXEC(); -} - - -/* - * EEPROM_ErasePage() - Erase EEPROM page. - * - * This function erases one EEPROM page, so that every location reads 0xFF. - * - * pageAddr EEPROM Page address between 0 and EEPROM_SIZE/EEPROM_PAGESIZE - */ -inline void EEPROM_ErasePage(uint8_t pageAddr) { - EEPROM_WaitForNVM(); // wait until NVM not busy - uint16_t address = (uint16_t)(pageAddr * EEPROM_PAGESIZE); - NVM.ADDR0 = address & 0xFF; // set addresses - NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - NVM.CMD = NVM_CMD_ERASE_EEPROM_PAGE_gc; // erase page command - NVM_EXEC_WRAPPER(); -} - - -/* - * EEPROM_SplitWritePage() - Write (without erasing) EEPROM page. - * - * This function writes the contents of an already loaded EEPROM page - * buffer into EEPROM memory. As this is a split write, the page in - * EEPROM will *NOT* be erased before writing. - * - * pageAddr EEPROM Page address between 0 and EEPROM_SIZE/EEPROM_PAGESIZE - */ -inline void EEPROM_SplitWritePage(uint8_t pageAddr) { - EEPROM_WaitForNVM(); // wait until NVM not busy - uint16_t address = (uint16_t)(pageAddr * EEPROM_PAGESIZE); - NVM.ADDR0 = address & 0xFF; // set addresses - NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - NVM.CMD = NVM_CMD_WRITE_EEPROM_PAGE_gc; // split write command - NVM_EXEC_WRAPPER(); -} - - -/// EEPROM_EraseAll() - Erase entire EEPROM memory to 0xFF -inline void EEPROM_EraseAll() { - EEPROM_WaitForNVM(); // wait until NVM not busy - NVM.CMD = NVM_CMD_ERASE_EEPROM_gc; // erase all command - NVM_EXEC_WRAPPER(); -} - diff --git a/src/xmega/xmega_eeprom.h b/src/xmega/xmega_eeprom.h deleted file mode 100644 index 17bb0b8..0000000 --- a/src/xmega/xmega_eeprom.h +++ /dev/null @@ -1,216 +0,0 @@ -/************************************************************************* - * - * XMEGA EEPROM driver header file. - * - * This file contains the function prototypes and enumerator - * definitions for various configuration parameters for the - * XMEGA EEPROM driver. - * - * The driver is not intended for size and/or speed critical code, - * since most functions are just a few lines of code, and the - * function call overhead would decrease code performance. The driver - * is intended for rapid prototyping and documentation purposes for - * getting started with the XMEGA EEPROM module. - * - * For size and/or speed critical code, it is recommended to copy the - * function contents directly into your application instead of making - * a function call. - * - * Notes: - * See AVR1315: Accessing the XMEGA EEPROM + Code eeprom_driver.c /.h - * - * Author: * \author - * Original Author: Atmel Corporation: http://www.atmel.com \n - * Adapted by: Alden S. Hart Jr; 04/02/2010 - * - * Copyright (c) 2008, Atmel Corporation All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * - * 3. The name of ATMEL may not be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED - * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND - * SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY - * DIRECT,INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS - * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, - * STRICT LIABILITY, OR FART (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING - * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - ************************************************************************/ - -#ifndef xmega_eeprom_h -#define xmega_eeprom_h - -#include - -/* Configuration settings */ -// UNCOMMENT FOR TEST ONLY - uses a RAM block to simulate EEPROM -//#define __NNVM // uncomment to use non-non-volatile RAM -#define NNVM_SIZE 2000 // size of emulation RAM block - xmega192/256 has 4096 - -#ifndef MAPPED_EEPROM_START -#define MAPPED_EEPROM_START 0x1000 -#endif //__NNVM - -#define EEPROM_PAGESIZE 32 // if this changes ...change below: -#define EEPROM_BYTE_ADDR_MASK_gm 0x1F // range of valid byte addrs in page -#define EEPROM_ADDR1_MASK_gm 0x0F // EEPROM is 4K = 0x0F, 2K = 0x07 - -#define EEPROM(_pageAddr, _byteAddr) \ - ((uint8_t *) MAPPED_EEPROM_START)[_pageAddr*EEPROM_PAGESIZE + _byteAddr] - -/* function prototypes for TinyG added functions */ -uint16_t EEPROM_WriteString(const uint16_t address, const char *buf, const uint8_t terminate); -uint16_t EEPROM_ReadString(const uint16_t address, char *buf, const uint16_t size); -uint16_t EEPROM_WriteBytes(const uint16_t address, const int8_t *buf, const uint16_t size); -uint16_t EEPROM_ReadBytes(const uint16_t address, int8_t *buf, const uint16_t size); - -//#ifdef __UNIT_TEST_EEPROM -void EEPROM_unit_tests(); -//#endif - -/* Function prototypes for Atmel and Atmel-derived functions */ -uint8_t EEPROM_ReadByte(uint16_t address); -void EEPROM_WriteByte(uint16_t address, uint8_t value); -void EEPROM_WaitForNVM( void ); -void EEPROM_FlushBuffer( void ); -void EEPROM_LoadByte( uint8_t byteAddr, uint8_t value ); -void EEPROM_LoadPage( const uint8_t * values ); -void EEPROM_AtomicWritePage( uint8_t pageAddr ); -void EEPROM_ErasePage( uint8_t pageAddress ); -void EEPROM_SplitWritePage( uint8_t pageAddr ); -void EEPROM_EraseAll( void ); - -/* Some MACRO Definitions */ - -// Note: the ByPage macros rely on pagesize = 32, and are as yet untested -#define EEPROM_ReadChar (char)EEPROM_ReadByte -#define EEPROM_ReadByteByPage(p,b) EEPROM_ReadByte( (p<<5) | (b) ) -#define EEPROM_ReadCharByPage(p,b) (char)EEPROM_ReadByte( (p<<5) | (b) ) -#define EEPROM_WriteByteByPage(p,b,v) EEPROM_ReadByte( ((p<<5) | (b)), v ) - -#ifndef max -#define max(a, b) (((a)>(b))?(a):(b)) -#endif -#ifndef min -#define min(a, b) (((a)<(b))?(a):(b)) -#endif - -/* Enable EEPROM block sleep-when-not-used mode. - * - * This macro enables power reduction mode for EEPROM. It means that - * the EEPROM block is disabled when not used. Note that there will be - * a penalty of 6 CPU cycles if EEPROM is accessed. - */ -#define EEPROM_EnablePowerReduction() ( NVM.CTRLB |= NVM_EPRM_bm ) - -/* Disable EEPROM block sleep-when-not-used mode. - * - * This macro disables power reduction mode for EEPROM. - */ -#define EEPROM_DisablePowerReduction() ( NVM.CTRLB &= ~NVM_EPRM_bm ) - -/* Enable EEPROM mapping into data space. - * - * This macro enables mapping of EEPROM into data space. EEPROM starts at - * EEPROM_START in data memory. Read access can be done similar to ordinary - * SRAM access. - * - * Note: This disables IO-mapped access to EEPROM, although page erase and - * write operations still needs to be done through IO register. - */ -#define EEPROM_EnableMapping() ( NVM.CTRLB |= NVM_EEMAPEN_bm ) - -/* Disable EEPROM mapping into data space. - * - * This macro disables mapping of EEPROM into data space. - * IO mapped access is now enabled. - */ -#define EEPROM_DisableMapping() ( NVM.CTRLB &= ~NVM_EEMAPEN_bm ) - -/* - * NVM_EXEC() - Non-Volatile Memory Execute Command (MACRO) - * - * This macro sets the CCP register before setting the CMDEX bit in the - * NVM.CTRLA register. The CMDEX bit must be set within 4 clock cycles - * after setting the protection byte in the CCP register. - */ -/* -#define NVM_EXEC() asm("push r30" "\n\t" \ - "push r31" "\n\t" \ - "push r16" "\n\t" \ - "push r18" "\n\t" \ - "ldi r30, 0xCB" "\n\t" \ - "ldi r31, 0x01" "\n\t" \ - "ldi r16, 0xD8" "\n\t" \ - "ldi r18, 0x01" "\n\t" \ - "out 0x34, r16" "\n\t" \ - "st Z, r18" "\n\t" \ - "pop r18" "\n\t" \ - "pop r16" "\n\t" \ - "pop r31" "\n\t" \ - "pop r30" "\n\t" \ - ) -*/ -/* ------------------------------------------------------------------------ - * Modified version from jl_1978 - Feb 01, 2010 - 06:30 PM in thread: - * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=printview&t=87793&start=0 - * - * "This enables a high level interrupt when the EEPROM transaction is - * complete which wakes the CPU back up. In your .c eeprom write/erase - * functions, set the flag to enable sleep mode: - * - * SLEEP.CTRL = 1; - * - * Oh yeah, and disable other interrupts. My system only uses low and - * medium interrupts for other tasks. The high interrupt is only used - * for the stuff above." - */ -/* -#define NVM_EXEC() asm("push r30" "\n\t" \ - "push r31" "\n\t" \ - "push r16" "\n\t" \ - "push r18" "\n\t" \ - "ldi r30, 0xCB" "\n\t" \ - "ldi r31, 0x01" "\n\t" \ - "ldi r16, 0xD8" "\n\t" \ - "ldi r18, 0x01" "\n\t" \ - "out 0x34, r16" "\n\t" \ - "st Z, r18" "\n\t" \ - "ldi r30, 0xCD" "\n\t" \ - "ldi r31, 0x01" "\n\t" \ - "ldi r18, 0x0C" "\n\t" \ - "st Z, r18" "\n\t" \ - "sleep" "\n\t" \ - "pop r18" "\n\t" \ - "pop r16" "\n\t" \ - "pop r31" "\n\t" \ - "pop r30" "\n\t" \ - ) - -*/ - -//#define __UNIT_TEST_EEPROM -#ifdef __UNIT_TEST_EEPROM -void EEPROM_unit_tests(); -#define EEPROM_UNITS EEPROM_unit_tests(); -#else -#define EEPROM_UNITS -#endif - -#endif - diff --git a/src/xmega/xmega_init.c b/src/xmega/xmega_init.c deleted file mode 100644 index 0f1e1bc..0000000 --- a/src/xmega/xmega_init.c +++ /dev/null @@ -1,188 +0,0 @@ -/* - * xmega_init.c - general init and support functions for xmega family - * Part of TinyG project - * - * Copyright (c) 2010 - 2013 Alden S. Hart Jr. - * - * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include -#include - -#include "../tinyg.h" -#include "../hardware.h" -#include "xmega_init.h" - -void xmega_init_clocks(); -void CCPWrite(volatile uint8_t * address, uint8_t value); - -/* - * xmega_init() - */ - -void xmega_init() { - xmega_init_clocks(); -} - -/* - * xmega_init_clocks() - * - * This routine is lifted and modified from Boston Android and from - * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=711659 - * Read the above thread to the bottom and you will find: - * - OSC.XOSCCTRL = 0xCB; // 0.4-16 MHz XTAL - 16K CLK Start Up - OSC.CTRL = 0x08; // Enable External Oscillator - while(!testbit(OSC.STATUS,OSC_XOSCRDY_bp)); // wait until crystal stable - OSC.PLLCTRL = 0xC8; // XOSC is PLL Source - 8x Factor (128MHz) - OSC.CTRL = 0x18; // Enable PLL & External Oscillator - // Prescaler A=1, B=2, C=2 - // CLKPER4=128MHz, CLKPER2=64MHZ, CLKPER & CLKCPU = 32MHz - CCPWrite(&CLK.PSCTRL,0x03); - while(!testbit(OSC.STATUS,OSC_PLLRDY_bp)); // wait until PLL stable - CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc); // Switch to PLL clock - */ - -void xmega_init_clocks() -{ -#ifdef __CLOCK_EXTERNAL_8MHZ // external 8 Mhx Xtal with 4x PLL = 32 Mhz - OSC.XOSCCTRL = 0x4B; // 2-9 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup - OSC.CTRL = 0x08; // enable external crystal oscillator - while(!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready - OSC.PLLCTRL = 0xC4; // XOSC is PLL Source; 4x Factor (32 MHz sys clock) - OSC.CTRL = 0x18; // Enable PLL & External Oscillator - while(!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready - CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc);// switch to PLL clock - OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock -#endif - -#ifdef __CLOCK_EXTERNAL_16MHZ // external 16 Mhx Xtal with 2x PLL = 32 Mhz - OSC.XOSCCTRL = 0xCB; // 12-16 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup - OSC.CTRL = 0x08; // enable external crystal oscillator - while(!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready - OSC.PLLCTRL = 0xC2; // XOSC is PLL Source; 2x Factor (32 MHz sys clock) - OSC.CTRL = 0x18; // Enable PLL & External Oscillator - while(!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready - CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc);// switch to PLL clock - OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock -#endif - -#ifdef __CLOCK_INTERNAL_32MHZ // 32 MHz internal clock (Boston Android code) - CCP = CCP_IOREG_gc; // Security Signature to modify clk - OSC.CTRL = OSC_RC32MEN_bm; // enable internal 32MHz oscillator - while(!(OSC.STATUS & OSC_RC32MRDY_bm)); // wait for oscillator ready - CCP = CCP_IOREG_gc; // Security Signature to modify clk - CLK.CTRL = 0x01; // select sysclock 32MHz osc -#endif -} - -/****************************************************************************** - * The following code was excerpted from the Atmel AVR1003 clock driver example - * and carries its copyright: - * - * $Revision: 2771 $ - * $Date: 2009-09-11 11:54:26 +0200 (fr, 11 sep 2009) $ \n - * - * Copyright (c) 2008, Atmel Corporation All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. The name of ATMEL may not be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED - * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND - * SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, - * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR FART - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF - * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SHOE DAMAGE. - *****************************************************************************/ - -/* Macros to protect the code from interrupts */ - -#define AVR_ENTER_CRITICAL_REGION() uint8_t volatile saved_sreg = SREG; cli(); -#define AVR_LEAVE_CRITICAL_REGION() SREG = saved_sreg; - - -/* CCP write helper function written in assembly. - * - * This function is written in assembly because of the time critial - * operation of writing to the registers. - * - * - address A pointer to the address to write to. - * - value The value to put in to the register. - */ - -void CCPWrite( volatile uint8_t * address, uint8_t value ) -{ -#ifdef __ICCAVR__ - - // Store global interrupt setting in scratch register and disable interrupts. - asm("in R1, 0x3F \n" - "cli" - ); - - // Move destination address pointer to Z pointer registers. - asm("movw r30, r16"); -#ifdef RAMPZ - asm("ldi R16, 0 \n" - "out 0x3B, R16" - ); - -#endif - asm("ldi r16, 0xD8 \n" - "out 0x34, r16 \n" -#if (__MEMORY_MODEL__ == 1) - "st Z, r17 \n"); -#elif (__MEMORY_MODEL__ == 2) - "st Z, r18 \n"); -#else /* (__MEMORY_MODEL__ == 3) || (__MEMORY_MODEL__ == 5) */ - "st Z, r19 \n"); -#endif /* __MEMORY_MODEL__ */ - - // Restore global interrupt setting from scratch register. - asm("out 0x3F, R1"); - -#elif defined __GNUC__ - AVR_ENTER_CRITICAL_REGION(); - volatile uint8_t * tmpAddr = address; -#ifdef RAMPZ - RAMPZ = 0; -#endif - asm volatile( - "movw r30, %0" "\n\t" - "ldi r16, %2" "\n\t" - "out %3, r16" "\n\t" - "st Z, %1" "\n\t" - : - : "r" (tmpAddr), "r" (value), "M" (CCP_IOREG_gc), "i" (&CCP) - : "r16", "r30", "r31" - ); - - AVR_LEAVE_CRITICAL_REGION(); -#endif -} - diff --git a/src/xmega/xmega_interrupts.c b/src/xmega/xmega_interrupts.c deleted file mode 100644 index 904b505..0000000 --- a/src/xmega/xmega_interrupts.c +++ /dev/null @@ -1,88 +0,0 @@ -/* This file has been prepared for Doxygen automatic documentation generation.*/ -/*! \file ********************************************************************* - * - * \brief XMEGA PMIC driver header file. - * - * This file contains the function implementations for the XMEGA - * Programmable Multi-level Interrupt Controller driver/example. - * - * The driver is not intended for size and/or speed critical code, since - * most functions are just a few lines of code, and the function call - * overhead would decrease code performance. The driver is intended for - * rapid prototyping and documentation purposes for getting started with - * the XMEGA PMIC module. - * - * For size and/or speed critical code, it is recommended to copy the - * function contents directly into your application instead of making - * a function call. - * - * \par Application note: - * AVR1305: XMEGA Interrupts and the Programmable Multi-level Interrupt Controller - * - * \par Documentation - * For comprehensive code documentation, supported compilers, compiler - * settings and supported devices see readme.html - * - * \author - * Atmel Corporation: http://www.atmel.com \n - * Support email: avr@atmel.com - * - * $Revision: 1569 $ - * $Date: 2008-04-22 13:03:43 +0200 (ti, 22 apr 2008) $ \n - * - * Copyright (c) 2008, Atmel Corporation All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. The name of ATMEL may not be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED - * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND - * SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, - * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF - * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - ******************************************************************************/ -#include "xmega_interrupts.h" - -/*! \brief Move interrupt vector table to boot area. - * - * This function moves the interrupt vector table to boot area. - * The function writes the correct signature to the Configuration - * Change Protection register before writing the CTRL register. Interrupts are - * automatically ignored during the change enable period. - */ -void PMIC_SetVectorLocationToBoot( void ) -{ - uint8_t temp = PMIC.CTRL | PMIC_IVSEL_bm; - CCP = CCP_IOREG_gc; - PMIC.CTRL = temp; -} - -/*! \brief Move interrupt vector table to application area. - * - * This function moves the interrupt vector table to application area. - * The function writes the correct signature to the Configuration - * Change Protection register before writing the CTRL register. Interrupts are - * automatically ignored during the change enable period. - */ -void PMIC_SetVectorLocationToApplication( void ) -{ - uint8_t temp = PMIC.CTRL & ~PMIC_IVSEL_bm; - CCP = CCP_IOREG_gc; - PMIC.CTRL = temp; -} diff --git a/src/xmega/xmega_interrupts.h b/src/xmega/xmega_interrupts.h deleted file mode 100644 index c15ef8f..0000000 --- a/src/xmega/xmega_interrupts.h +++ /dev/null @@ -1,160 +0,0 @@ -/* This file has been prepared for Doxygen automatic documentation generation.*/ -/*! \file ********************************************************************* - * - * \brief XMEGA PMIC driver header file. - * - * This file contains the function prototypes and enumerator definitions - * for various configuration parameters for the XMEGA Programmable Multi-level - * Interrupt Controller driver/example. - * - * The driver is not intended for size and/or speed critical code, since - * most functions are just a few lines of code, and the function call - * overhead would decrease code performance. The driver is intended for - * rapid prototyping and documentation purposes for getting started with - * the XMEGA PMIC module. - * - * For size and/or speed critical code, it is recommended to copy the - * function contents directly into your application instead of making - * a function call. - * - * \par Application note: - * AVR1305: XMEGA Interrupts and the Programmable Multi-level Interrupt Controller - * - * \par Documentation - * For comprehensive code documentation, supported compilers, compiler - * settings and supported devices see readme.html - * - * \author - * Atmel Corporation: http://www.atmel.com \n - * Support email: avr@atmel.com - * - * $Revision: 1569 $ - * $Date: 2008-04-22 13:03:43 +0200 (ti, 22 apr 2008) $ \n - * - * Copyright (c) 2008, Atmel Corporation All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. The name of ATMEL may not be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED - * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND - * SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, - * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF - * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - ******************************************************************************/ - -/* - * Ref: ATMEL AVR10__ app note and code - * Contains some minor mods by ASH to adapt to GCC: search on "(ash mod)" - */ - -#ifndef xmega_interrupts_h -#define xmega_interrupts_h - -#include // Was #include "avr_compiler.h" (ash mod) - - -/* Definitions of macros. */ - -/*! \brief Enable low-level interrupts. */ -#define PMIC_EnableLowLevel() (PMIC.CTRL |= PMIC_LOLVLEN_bm) - - -/*! \brief Disable low-level interrupts. */ -#define PMIC_DisableLowLevel() (PMIC.CTRL &= ~PMIC_LOLVLEN_bm) - - -/*! \brief Enable medium-level interrupts. */ -#define PMIC_EnableMediumLevel() (PMIC.CTRL |= PMIC_MEDLVLEN_bm) - - -/*! \brief Disable medium-level interrupts. */ -#define PMIC_DisableMediumLevel() (PMIC.CTRL &= ~PMIC_MEDLVLEN_bm) - - -/*! \brief Enable high-level interrupts. */ -#define PMIC_EnableHighLevel() (PMIC.CTRL |= PMIC_HILVLEN_bm) - - -/*! \brief Disable high-level interrupts. */ -#define PMIC_DisableHighLevel() (PMIC.CTRL &= ~PMIC_HILVLEN_bm) - - -/*! \brief Enable round-robin scheduling for low-level interrupts. */ -#define PMIC_EnableRoundRobin() (PMIC.CTRL |= PMIC_RREN_bm) - - -/*! \brief Disable round-robin scheduling for low-level interrupts. */ -#define PMIC_DisableRoundRobin() (PMIC.CTRL &= ~PMIC_RREN_bm) - - - -/*! \brief Set interrupt priority for round-robin scheduling. - * - * This macro selects which low-level interrupt has highest priority. - * Use this function together with round-robin scheduling. - * - * \note The INTPRI register wants the vector _number_ (not address) of the lowest - * prioritized interrupt among low-level interrupts. Since vector addresses - * lies on 4-byte boundaries, we divide by 4. - * - * \param _vectorAddress Number between 0 and the maximum vector address for the device. - */ -#define PMIC_SetNextRoundRobinInterrupt(_vectorAddress) (PMIC.INTPRI = (_vectorAddress >> 2) - 1) - - - -/*! \brief Check if a high-level interrupt handler is currently executing. - * - * \return Non-zero if interrupt handler is executing. Zero otherwise. - */ -#define PMIC_IsHighLevelExecuting() (PMIC.STATUS & PMIC_HILVLEX_bm) - - - -/*! \brief Check if a medium-level interrupt handler is currently executing. - * - * \return Non-zero if interrupt handler is executing. Zero otherwise. - */ -#define PMIC_IsMediumLevelExecuting() (PMIC.STATUS & PMIC_MEDLVLEX_bm) - - - -/*! \brief Check if a low-level interrupt handler is currently executing. - * - * \return Non-zero if interrupt handler is executing. Zero otherwise. - */ -#define PMIC_IsLowLevelExecuting() (PMIC.STATUS & PMIC_LOLVLEX_bm) - - - -/*! \brief Check if an NMI handler is currently executing. - * - * \return Non-zero if interrupt handler is executing. Zero otherwise. - */ -#define PMIC_IsNMIExecuting() (PMIC.STATUS & PMIC_NMIEX_bm) - - - -/* Prototype of functions. */ - -void PMIC_SetVectorLocationToBoot( void ); -void PMIC_SetVectorLocationToApplication( void ); - -#endif diff --git a/src/xmega/xmega_rtc.c b/src/xmega/xmega_rtc.c deleted file mode 100644 index 58ef59a..0000000 --- a/src/xmega/xmega_rtc.c +++ /dev/null @@ -1,79 +0,0 @@ -/* - * xmega_rtc.h - real-time counter/clock - * Part of TinyG project - * - * Copyright (c) 2010 - 2013 Alden S. Hart Jr. - * - * 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 OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include -#include - -#include "../tinyg.h" -#include "../config.h" -#include "../switch.h" -#include "xmega_rtc.h" - -rtClock_t rtc; // allocate clock control struct - -/* - * rtc_init() - initialize and start the clock - * - * This routine follows the code in app note AVR1314. - */ - -void rtc_init() -{ - OSC.CTRL |= OSC_RC32KEN_bm; // Turn on internal 32kHz. - do {} while ((OSC.STATUS & OSC_RC32KRDY_bm) == 0); // Wait for 32kHz oscillator to stabilize. - do {} while (RTC.STATUS & RTC_SYNCBUSY_bm); // Wait until RTC is not busy - - CLK.RTCCTRL = CLK_RTCSRC_RCOSC_gc | CLK_RTCEN_bm; // Set internal 32kHz osc as RTC clock source - do {} while (RTC.STATUS & RTC_SYNCBUSY_bm); // Wait until RTC is not busy - - // the following must be in this order or it doesn;t work - RTC.PER = RTC_MILLISECONDS-1; // set overflow period to 10ms - approximate - RTC.CNT = 0; - RTC.COMP = RTC_MILLISECONDS-1; - RTC.CTRL = RTC_PRESCALER_DIV1_gc; // no prescale (1x) - RTC.INTCTRL = RTC_COMPINTLVL; // interrupt on compare - rtc.rtc_ticks = 0; // reset tick counter - rtc.sys_ticks = 0; // reset tick counter - rtc.magic_end = MAGICNUM; -} - -/* - * rtc ISR - * - * It is the responsibility of the callback code to ensure atomicity and volatiles - * are observed correctly as the callback will be run at the interrupt level. - * - * Here's the code in case the main loop (non-interrupt) function needs to - * create a critical region for variables set or used by the callback: - * - * #include "gpio.h" - * #include "xmega_rtc.h" - * - * RTC.INTCTRL = RTC_OVFINTLVL_OFF_gc; // disable interrupt - * blah blah blah critical region - * RTC.INTCTRL = RTC_OVFINTLVL_LO_gc; // enable interrupt - */ - -ISR(RTC_COMP_vect) -{ - rtc.sys_ticks = ++rtc.rtc_ticks*10; // advance both tick counters as appropriate - - // callbacks to whatever you need to happen on each RTC tick go here: - switch_rtc_callback(); // switch debouncing -} -- 2.27.0