From 1d6736cff62ccf5894edb862617553f1835b8049 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 4 Jul 2021 15:39:26 +0200 Subject: [PATCH 01/43] Add modules::math for specialized math functions Currently include reduced-precision 8x16 and 16x16 multiplication functions used by the speed lookup tables. --- src/modules/math.h | 78 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) create mode 100644 src/modules/math.h diff --git a/src/modules/math.h b/src/modules/math.h new file mode 100644 index 0000000..65697fe --- /dev/null +++ b/src/modules/math.h @@ -0,0 +1,78 @@ +#pragma once +#include "../config/config.h" + +namespace modules { + +/// Specialized math operations +namespace math { + +/// (intIn1 * intIn2) >> 8 +static inline uint16_t mulU8X16toH16(const uint8_t charIn1, const uint16_t intIn2) { + uint16_t intRes; +#if !defined(__AVR__) || defined(NO_ASM) + intRes = ((uint32_t)charIn1 * (uint32_t)intIn2) >> 8; +#else + asm volatile( + "clr r26 \n\t" + "mul %A1, %B2 \n\t" + "movw %A0, r0 \n\t" + "mul %A1, %A2 \n\t" + "add %A0, r1 \n\t" + "adc %B0, r26 \n\t" + "lsr r0 \n\t" + "adc %A0, r26 \n\t" + "adc %B0, r26 \n\t" + "clr r1 \n\t" + : "=&r"(intRes) + : "d"(charIn1), "d"(intIn2) + : "r26"); +#endif + return intRes; +} + +/// (longIn1 * longIn2) >> 24 +static inline uint16_t mulU24X24toH16(const uint32_t &longIn1, const uint32_t &longIn2) { + uint16_t intRes; +#if !defined(__AVR__) || defined(NO_ASM) + intRes = ((uint64_t)longIn1 * (uint64_t)longIn2) >> 24; +#else + asm volatile( + "clr r26 \n\t" + "mul %A1, %B2 \n\t" + "mov r27, r1 \n\t" + "mul %B1, %C2 \n\t" + "movw %A0, r0 \n\t" + "mul %C1, %C2 \n\t" + "add %B0, r0 \n\t" + "mul %C1, %B2 \n\t" + "add %A0, r0 \n\t" + "adc %B0, r1 \n\t" + "mul %A1, %C2 \n\t" + "add r27, r0 \n\t" + "adc %A0, r1 \n\t" + "adc %B0, r26 \n\t" + "mul %B1, %B2 \n\t" + "add r27, r0 \n\t" + "adc %A0, r1 \n\t" + "adc %B0, r26 \n\t" + "mul %C1, %A2 \n\t" + "add r27, r0 \n\t" + "adc %A0, r1 \n\t" + "adc %B0, r26 \n\t" + "mul %B1, %A2 \n\t" + "add r27, r1 \n\t" + "adc %A0, r26 \n\t" + "adc %B0, r26 \n\t" + "lsr r27 \n\t" + "adc %A0, r26 \n\t" + "adc %B0, r26 \n\t" + "clr r1 \n\t" + : "=&r"(intRes) + : "d"(longIn1), "d"(longIn2) + : "r26", "r27"); +#endif + return intRes; +} + +} // namespace math +} // namespace modules From 531f60ed278ea3b8398f1ccd56c427362cff7ef0 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 4 Jul 2021 15:41:42 +0200 Subject: [PATCH 02/43] Add modules::speed_table for acceleration lookup tables --- CMakeLists.txt | 1 + src/config/config.h | 1 + src/config/todo.h | 7 + src/modules/speed_table.cpp | 601 ++++++++++++++++++ src/modules/speed_table.h | 58 ++ tests/unit/modules/CMakeLists.txt | 1 + tests/unit/modules/speed_table/CMakeLists.txt | 12 + .../modules/speed_table/test_speed_table.cpp | 110 ++++ 8 files changed, 791 insertions(+) create mode 100644 src/config/todo.h create mode 100644 src/modules/speed_table.cpp create mode 100644 src/modules/speed_table.h create mode 100644 tests/unit/modules/speed_table/CMakeLists.txt create mode 100644 tests/unit/modules/speed_table/test_speed_table.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d98231d..2a11afa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -205,6 +205,7 @@ target_sources( src/modules/selector.cpp src/modules/timebase.cpp src/modules/user_input.cpp + src/modules/speed_table.cpp src/logic/command_base.cpp src/logic/cut_filament.cpp src/logic/eject_filament.cpp diff --git a/src/config/config.h b/src/config/config.h index 44b661d..9c8b55f 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -1,5 +1,6 @@ #pragma once #include +#include "todo.h" /// Wrangler for assorted compile-time configuration and constants. namespace config { diff --git a/src/config/todo.h b/src/config/todo.h new file mode 100644 index 0000000..2dba1e1 --- /dev/null +++ b/src/config/todo.h @@ -0,0 +1,7 @@ +#pragma once + +#ifndef __AVR__ + #define F_CPU 16000000 +#endif + +#define MAX_STEP_FREQUENCY 40000 // Max step frequency diff --git a/src/modules/speed_table.cpp b/src/modules/speed_table.cpp new file mode 100644 index 0000000..f614eb8 --- /dev/null +++ b/src/modules/speed_table.cpp @@ -0,0 +1,601 @@ +#include "speed_table.h" + +namespace modules { +namespace speed_table { + +#if F_CPU == 16000000 + +const uint16_t speed_table_fast[256][2] PROGMEM = { + { 62500, 55556 }, { 6944, 3268 }, { 3676, 1176 }, { 2500, 607 }, { 1893, 369 }, { 1524, 249 }, { 1275, 179 }, { 1096, 135 }, + { 961, 105 }, { 856, 85 }, { 771, 69 }, { 702, 58 }, { 644, 49 }, { 595, 42 }, { 553, 37 }, { 516, 32 }, + { 484, 28 }, { 456, 25 }, { 431, 23 }, { 408, 20 }, { 388, 19 }, { 369, 16 }, { 353, 16 }, { 337, 14 }, + { 323, 13 }, { 310, 11 }, { 299, 11 }, { 288, 11 }, { 277, 9 }, { 268, 9 }, { 259, 8 }, { 251, 8 }, + { 243, 8 }, { 235, 7 }, { 228, 6 }, { 222, 6 }, { 216, 6 }, { 210, 6 }, { 204, 5 }, { 199, 5 }, + { 194, 5 }, { 189, 4 }, { 185, 4 }, { 181, 4 }, { 177, 4 }, { 173, 4 }, { 169, 4 }, { 165, 3 }, + { 162, 3 }, { 159, 4 }, { 155, 3 }, { 152, 3 }, { 149, 2 }, { 147, 3 }, { 144, 3 }, { 141, 2 }, + { 139, 3 }, { 136, 2 }, { 134, 2 }, { 132, 3 }, { 129, 2 }, { 127, 2 }, { 125, 2 }, { 123, 2 }, + { 121, 2 }, { 119, 1 }, { 118, 2 }, { 116, 2 }, { 114, 1 }, { 113, 2 }, { 111, 2 }, { 109, 1 }, + { 108, 2 }, { 106, 1 }, { 105, 2 }, { 103, 1 }, { 102, 1 }, { 101, 1 }, { 100, 2 }, { 98, 1 }, + { 97, 1 }, { 96, 1 }, { 95, 2 }, { 93, 1 }, { 92, 1 }, { 91, 1 }, { 90, 1 }, { 89, 1 }, + { 88, 1 }, { 87, 1 }, { 86, 1 }, { 85, 1 }, { 84, 1 }, { 83, 0 }, { 83, 1 }, { 82, 1 }, + { 81, 1 }, { 80, 1 }, { 79, 1 }, { 78, 0 }, { 78, 1 }, { 77, 1 }, { 76, 1 }, { 75, 0 }, + { 75, 1 }, { 74, 1 }, { 73, 1 }, { 72, 0 }, { 72, 1 }, { 71, 1 }, { 70, 0 }, { 70, 1 }, + { 69, 0 }, { 69, 1 }, { 68, 1 }, { 67, 0 }, { 67, 1 }, { 66, 0 }, { 66, 1 }, { 65, 0 }, + { 65, 1 }, { 64, 1 }, { 63, 0 }, { 63, 1 }, { 62, 0 }, { 62, 1 }, { 61, 0 }, { 61, 1 }, + { 60, 0 }, { 60, 0 }, { 60, 1 }, { 59, 0 }, { 59, 1 }, { 58, 0 }, { 58, 1 }, { 57, 0 }, + { 57, 1 }, { 56, 0 }, { 56, 0 }, { 56, 1 }, { 55, 0 }, { 55, 1 }, { 54, 0 }, { 54, 0 }, + { 54, 1 }, { 53, 0 }, { 53, 0 }, { 53, 1 }, { 52, 0 }, { 52, 0 }, { 52, 1 }, { 51, 0 }, + { 51, 0 }, { 51, 1 }, { 50, 0 }, { 50, 0 }, { 50, 1 }, { 49, 0 }, { 49, 0 }, { 49, 1 }, + { 48, 0 }, { 48, 0 }, { 48, 1 }, { 47, 0 }, { 47, 0 }, { 47, 0 }, { 47, 1 }, { 46, 0 }, + { 46, 0 }, { 46, 1 }, { 45, 0 }, { 45, 0 }, { 45, 0 }, { 45, 1 }, { 44, 0 }, { 44, 0 }, + { 44, 0 }, { 44, 1 }, { 43, 0 }, { 43, 0 }, { 43, 0 }, { 43, 1 }, { 42, 0 }, { 42, 0 }, + { 42, 0 }, { 42, 1 }, { 41, 0 }, { 41, 0 }, { 41, 0 }, { 41, 0 }, { 41, 1 }, { 40, 0 }, + { 40, 0 }, { 40, 0 }, { 40, 0 }, { 40, 1 }, { 39, 0 }, { 39, 0 }, { 39, 0 }, { 39, 0 }, + { 39, 1 }, { 38, 0 }, { 38, 0 }, { 38, 0 }, { 38, 0 }, { 38, 1 }, { 37, 0 }, { 37, 0 }, + { 37, 0 }, { 37, 0 }, { 37, 0 }, { 37, 1 }, { 36, 0 }, { 36, 0 }, { 36, 0 }, { 36, 0 }, + { 36, 1 }, { 35, 0 }, { 35, 0 }, { 35, 0 }, { 35, 0 }, { 35, 0 }, { 35, 0 }, { 35, 1 }, + { 34, 0 }, { 34, 0 }, { 34, 0 }, { 34, 0 }, { 34, 0 }, { 34, 1 }, { 33, 0 }, { 33, 0 }, + { 33, 0 }, { 33, 0 }, { 33, 0 }, { 33, 0 }, { 33, 1 }, { 32, 0 }, { 32, 0 }, { 32, 0 }, + { 32, 0 }, { 32, 0 }, { 32, 0 }, { 32, 0 }, { 32, 1 }, { 31, 0 }, { 31, 0 }, { 31, 0 }, + { 31, 0 }, { 31, 0 }, { 31, 0 }, { 31, 1 }, { 30, 0 }, { 30, 0 }, { 30, 0 }, { 30, 0 } +}; + +const uint16_t speed_table_slow[256][2] PROGMEM = { + { 62500, 12500 }, { 50000, 8334 }, { 41666, 5952 }, { 35714, 4464 }, { 31250, 3473 }, { 27777, 2777 }, { 25000, 2273 }, { 22727, 1894 }, + { 20833, 1603 }, { 19230, 1373 }, { 17857, 1191 }, { 16666, 1041 }, { 15625, 920 }, { 14705, 817 }, { 13888, 731 }, { 13157, 657 }, + { 12500, 596 }, { 11904, 541 }, { 11363, 494 }, { 10869, 453 }, { 10416, 416 }, { 10000, 385 }, { 9615, 356 }, { 9259, 331 }, + { 8928, 308 }, { 8620, 287 }, { 8333, 269 }, { 8064, 252 }, { 7812, 237 }, { 7575, 223 }, { 7352, 210 }, { 7142, 198 }, + { 6944, 188 }, { 6756, 178 }, { 6578, 168 }, { 6410, 160 }, { 6250, 153 }, { 6097, 145 }, { 5952, 139 }, { 5813, 132 }, + { 5681, 126 }, { 5555, 121 }, { 5434, 115 }, { 5319, 111 }, { 5208, 106 }, { 5102, 102 }, { 5000, 99 }, { 4901, 94 }, + { 4807, 91 }, { 4716, 87 }, { 4629, 84 }, { 4545, 81 }, { 4464, 79 }, { 4385, 75 }, { 4310, 73 }, { 4237, 71 }, + { 4166, 68 }, { 4098, 66 }, { 4032, 64 }, { 3968, 62 }, { 3906, 60 }, { 3846, 59 }, { 3787, 56 }, { 3731, 55 }, + { 3676, 53 }, { 3623, 52 }, { 3571, 50 }, { 3521, 49 }, { 3472, 48 }, { 3424, 46 }, { 3378, 45 }, { 3333, 44 }, + { 3289, 43 }, { 3246, 41 }, { 3205, 41 }, { 3164, 39 }, { 3125, 39 }, { 3086, 38 }, { 3048, 36 }, { 3012, 36 }, + { 2976, 35 }, { 2941, 35 }, { 2906, 33 }, { 2873, 33 }, { 2840, 32 }, { 2808, 31 }, { 2777, 30 }, { 2747, 30 }, + { 2717, 29 }, { 2688, 29 }, { 2659, 28 }, { 2631, 27 }, { 2604, 27 }, { 2577, 26 }, { 2551, 26 }, { 2525, 25 }, + { 2500, 25 }, { 2475, 25 }, { 2450, 23 }, { 2427, 24 }, { 2403, 23 }, { 2380, 22 }, { 2358, 22 }, { 2336, 22 }, + { 2314, 21 }, { 2293, 21 }, { 2272, 20 }, { 2252, 20 }, { 2232, 20 }, { 2212, 20 }, { 2192, 19 }, { 2173, 18 }, + { 2155, 19 }, { 2136, 18 }, { 2118, 18 }, { 2100, 17 }, { 2083, 17 }, { 2066, 17 }, { 2049, 17 }, { 2032, 16 }, + { 2016, 16 }, { 2000, 16 }, { 1984, 16 }, { 1968, 15 }, { 1953, 16 }, { 1937, 14 }, { 1923, 15 }, { 1908, 15 }, + { 1893, 14 }, { 1879, 14 }, { 1865, 14 }, { 1851, 13 }, { 1838, 14 }, { 1824, 13 }, { 1811, 13 }, { 1798, 13 }, + { 1785, 12 }, { 1773, 13 }, { 1760, 12 }, { 1748, 12 }, { 1736, 12 }, { 1724, 12 }, { 1712, 12 }, { 1700, 11 }, + { 1689, 12 }, { 1677, 11 }, { 1666, 11 }, { 1655, 11 }, { 1644, 11 }, { 1633, 10 }, { 1623, 11 }, { 1612, 10 }, + { 1602, 10 }, { 1592, 10 }, { 1582, 10 }, { 1572, 10 }, { 1562, 10 }, { 1552, 9 }, { 1543, 10 }, { 1533, 9 }, + { 1524, 9 }, { 1515, 9 }, { 1506, 9 }, { 1497, 9 }, { 1488, 9 }, { 1479, 9 }, { 1470, 9 }, { 1461, 8 }, + { 1453, 8 }, { 1445, 9 }, { 1436, 8 }, { 1428, 8 }, { 1420, 8 }, { 1412, 8 }, { 1404, 8 }, { 1396, 8 }, + { 1388, 7 }, { 1381, 8 }, { 1373, 7 }, { 1366, 8 }, { 1358, 7 }, { 1351, 7 }, { 1344, 8 }, { 1336, 7 }, + { 1329, 7 }, { 1322, 7 }, { 1315, 7 }, { 1308, 6 }, { 1302, 7 }, { 1295, 7 }, { 1288, 6 }, { 1282, 7 }, + { 1275, 6 }, { 1269, 7 }, { 1262, 6 }, { 1256, 6 }, { 1250, 7 }, { 1243, 6 }, { 1237, 6 }, { 1231, 6 }, + { 1225, 6 }, { 1219, 6 }, { 1213, 6 }, { 1207, 6 }, { 1201, 5 }, { 1196, 6 }, { 1190, 6 }, { 1184, 5 }, + { 1179, 6 }, { 1173, 5 }, { 1168, 6 }, { 1162, 5 }, { 1157, 5 }, { 1152, 6 }, { 1146, 5 }, { 1141, 5 }, + { 1136, 5 }, { 1131, 5 }, { 1126, 5 }, { 1121, 5 }, { 1116, 5 }, { 1111, 5 }, { 1106, 5 }, { 1101, 5 }, + { 1096, 5 }, { 1091, 5 }, { 1086, 4 }, { 1082, 5 }, { 1077, 5 }, { 1072, 4 }, { 1068, 5 }, { 1063, 4 }, + { 1059, 5 }, { 1054, 4 }, { 1050, 4 }, { 1046, 5 }, { 1041, 4 }, { 1037, 4 }, { 1033, 5 }, { 1028, 4 }, + { 1024, 4 }, { 1020, 4 }, { 1016, 4 }, { 1012, 4 }, { 1008, 4 }, { 1004, 4 }, { 1000, 4 }, { 996, 4 }, + { 992, 4 }, { 988, 4 }, { 984, 4 }, { 980, 4 }, { 976, 4 }, { 972, 4 }, { 968, 3 }, { 965, 3 } +}; + +#elif F_CPU == 20000000 + +const uint16_t speed_table_fast[256][2] PROGMEM = { + { 62500, 54055 }, + { 8445, 3917 }, + { 4528, 1434 }, + { 3094, 745 }, + { 2349, 456 }, + { 1893, 307 }, + { 1586, 222 }, + { 1364, 167 }, + { 1197, 131 }, + { 1066, 105 }, + { 961, 86 }, + { 875, 72 }, + { 803, 61 }, + { 742, 53 }, + { 689, 45 }, + { 644, 40 }, + { 604, 35 }, + { 569, 32 }, + { 537, 28 }, + { 509, 25 }, + { 484, 23 }, + { 461, 21 }, + { 440, 19 }, + { 421, 17 }, + { 404, 16 }, + { 388, 15 }, + { 373, 14 }, + { 359, 13 }, + { 346, 12 }, + { 334, 11 }, + { 323, 10 }, + { 313, 10 }, + { 303, 9 }, + { 294, 9 }, + { 285, 8 }, + { 277, 7 }, + { 270, 8 }, + { 262, 7 }, + { 255, 6 }, + { 249, 6 }, + { 243, 6 }, + { 237, 6 }, + { 231, 5 }, + { 226, 5 }, + { 221, 5 }, + { 216, 5 }, + { 211, 4 }, + { 207, 5 }, + { 202, 4 }, + { 198, 4 }, + { 194, 4 }, + { 190, 3 }, + { 187, 4 }, + { 183, 3 }, + { 180, 3 }, + { 177, 4 }, + { 173, 3 }, + { 170, 3 }, + { 167, 2 }, + { 165, 3 }, + { 162, 3 }, + { 159, 2 }, + { 157, 3 }, + { 154, 2 }, + { 152, 3 }, + { 149, 2 }, + { 147, 2 }, + { 145, 2 }, + { 143, 2 }, + { 141, 2 }, + { 139, 2 }, + { 137, 2 }, + { 135, 2 }, + { 133, 2 }, + { 131, 2 }, + { 129, 1 }, + { 128, 2 }, + { 126, 2 }, + { 124, 1 }, + { 123, 2 }, + { 121, 1 }, + { 120, 2 }, + { 118, 1 }, + { 117, 1 }, + { 116, 2 }, + { 114, 1 }, + { 113, 1 }, + { 112, 2 }, + { 110, 1 }, + { 109, 1 }, + { 108, 1 }, + { 107, 2 }, + { 105, 1 }, + { 104, 1 }, + { 103, 1 }, + { 102, 1 }, + { 101, 1 }, + { 100, 1 }, + { 99, 1 }, + { 98, 1 }, + { 97, 1 }, + { 96, 1 }, + { 95, 1 }, + { 94, 1 }, + { 93, 1 }, + { 92, 1 }, + { 91, 0 }, + { 91, 1 }, + { 90, 1 }, + { 89, 1 }, + { 88, 1 }, + { 87, 0 }, + { 87, 1 }, + { 86, 1 }, + { 85, 1 }, + { 84, 0 }, + { 84, 1 }, + { 83, 1 }, + { 82, 1 }, + { 81, 0 }, + { 81, 1 }, + { 80, 1 }, + { 79, 0 }, + { 79, 1 }, + { 78, 0 }, + { 78, 1 }, + { 77, 1 }, + { 76, 0 }, + { 76, 1 }, + { 75, 0 }, + { 75, 1 }, + { 74, 1 }, + { 73, 0 }, + { 73, 1 }, + { 72, 0 }, + { 72, 1 }, + { 71, 0 }, + { 71, 1 }, + { 70, 0 }, + { 70, 1 }, + { 69, 0 }, + { 69, 1 }, + { 68, 0 }, + { 68, 1 }, + { 67, 0 }, + { 67, 1 }, + { 66, 0 }, + { 66, 1 }, + { 65, 0 }, + { 65, 0 }, + { 65, 1 }, + { 64, 0 }, + { 64, 1 }, + { 63, 0 }, + { 63, 1 }, + { 62, 0 }, + { 62, 0 }, + { 62, 1 }, + { 61, 0 }, + { 61, 1 }, + { 60, 0 }, + { 60, 0 }, + { 60, 1 }, + { 59, 0 }, + { 59, 0 }, + { 59, 1 }, + { 58, 0 }, + { 58, 0 }, + { 58, 1 }, + { 57, 0 }, + { 57, 0 }, + { 57, 1 }, + { 56, 0 }, + { 56, 0 }, + { 56, 1 }, + { 55, 0 }, + { 55, 0 }, + { 55, 1 }, + { 54, 0 }, + { 54, 0 }, + { 54, 1 }, + { 53, 0 }, + { 53, 0 }, + { 53, 0 }, + { 53, 1 }, + { 52, 0 }, + { 52, 0 }, + { 52, 1 }, + { 51, 0 }, + { 51, 0 }, + { 51, 0 }, + { 51, 1 }, + { 50, 0 }, + { 50, 0 }, + { 50, 0 }, + { 50, 1 }, + { 49, 0 }, + { 49, 0 }, + { 49, 0 }, + { 49, 1 }, + { 48, 0 }, + { 48, 0 }, + { 48, 0 }, + { 48, 1 }, + { 47, 0 }, + { 47, 0 }, + { 47, 0 }, + { 47, 1 }, + { 46, 0 }, + { 46, 0 }, + { 46, 0 }, + { 46, 0 }, + { 46, 1 }, + { 45, 0 }, + { 45, 0 }, + { 45, 0 }, + { 45, 1 }, + { 44, 0 }, + { 44, 0 }, + { 44, 0 }, + { 44, 0 }, + { 44, 1 }, + { 43, 0 }, + { 43, 0 }, + { 43, 0 }, + { 43, 0 }, + { 43, 1 }, + { 42, 0 }, + { 42, 0 }, + { 42, 0 }, + { 42, 0 }, + { 42, 0 }, + { 42, 1 }, + { 41, 0 }, + { 41, 0 }, + { 41, 0 }, + { 41, 0 }, + { 41, 0 }, + { 41, 1 }, + { 40, 0 }, + { 40, 0 }, + { 40, 0 }, + { 40, 0 }, + { 40, 1 }, + { 39, 0 }, + { 39, 0 }, + { 39, 0 }, + { 39, 0 }, + { 39, 0 }, + { 39, 0 }, + { 39, 1 }, + { 38, 0 }, + { 38, 0 }, + { 38, 0 }, + { 38, 0 }, + { 38, 0 }, +}; + +const uint16_t speed_table_slow[256][2] PROGMEM = { + { 62500, 10417 }, + { 52083, 7441 }, + { 44642, 5580 }, + { 39062, 4340 }, + { 34722, 3472 }, + { 31250, 2841 }, + { 28409, 2368 }, + { 26041, 2003 }, + { 24038, 1717 }, + { 22321, 1488 }, + { 20833, 1302 }, + { 19531, 1149 }, + { 18382, 1021 }, + { 17361, 914 }, + { 16447, 822 }, + { 15625, 745 }, + { 14880, 676 }, + { 14204, 618 }, + { 13586, 566 }, + { 13020, 520 }, + { 12500, 481 }, + { 12019, 445 }, + { 11574, 414 }, + { 11160, 385 }, + { 10775, 359 }, + { 10416, 336 }, + { 10080, 315 }, + { 9765, 296 }, + { 9469, 278 }, + { 9191, 263 }, + { 8928, 248 }, + { 8680, 235 }, + { 8445, 222 }, + { 8223, 211 }, + { 8012, 200 }, + { 7812, 191 }, + { 7621, 181 }, + { 7440, 173 }, + { 7267, 165 }, + { 7102, 158 }, + { 6944, 151 }, + { 6793, 145 }, + { 6648, 138 }, + { 6510, 133 }, + { 6377, 127 }, + { 6250, 123 }, + { 6127, 118 }, + { 6009, 113 }, + { 5896, 109 }, + { 5787, 106 }, + { 5681, 101 }, + { 5580, 98 }, + { 5482, 95 }, + { 5387, 91 }, + { 5296, 88 }, + { 5208, 86 }, + { 5122, 82 }, + { 5040, 80 }, + { 4960, 78 }, + { 4882, 75 }, + { 4807, 73 }, + { 4734, 70 }, + { 4664, 69 }, + { 4595, 67 }, + { 4528, 64 }, + { 4464, 63 }, + { 4401, 61 }, + { 4340, 60 }, + { 4280, 58 }, + { 4222, 56 }, + { 4166, 55 }, + { 4111, 53 }, + { 4058, 52 }, + { 4006, 51 }, + { 3955, 49 }, + { 3906, 48 }, + { 3858, 48 }, + { 3810, 45 }, + { 3765, 45 }, + { 3720, 44 }, + { 3676, 43 }, + { 3633, 42 }, + { 3591, 40 }, + { 3551, 40 }, + { 3511, 39 }, + { 3472, 38 }, + { 3434, 38 }, + { 3396, 36 }, + { 3360, 36 }, + { 3324, 35 }, + { 3289, 34 }, + { 3255, 34 }, + { 3221, 33 }, + { 3188, 32 }, + { 3156, 31 }, + { 3125, 31 }, + { 3094, 31 }, + { 3063, 30 }, + { 3033, 29 }, + { 3004, 28 }, + { 2976, 28 }, + { 2948, 28 }, + { 2920, 27 }, + { 2893, 27 }, + { 2866, 26 }, + { 2840, 25 }, + { 2815, 25 }, + { 2790, 25 }, + { 2765, 24 }, + { 2741, 24 }, + { 2717, 24 }, + { 2693, 23 }, + { 2670, 22 }, + { 2648, 22 }, + { 2626, 22 }, + { 2604, 22 }, + { 2582, 21 }, + { 2561, 21 }, + { 2540, 20 }, + { 2520, 20 }, + { 2500, 20 }, + { 2480, 20 }, + { 2460, 19 }, + { 2441, 19 }, + { 2422, 19 }, + { 2403, 18 }, + { 2385, 18 }, + { 2367, 18 }, + { 2349, 17 }, + { 2332, 18 }, + { 2314, 17 }, + { 2297, 16 }, + { 2281, 17 }, + { 2264, 16 }, + { 2248, 16 }, + { 2232, 16 }, + { 2216, 16 }, + { 2200, 15 }, + { 2185, 15 }, + { 2170, 15 }, + { 2155, 15 }, + { 2140, 15 }, + { 2125, 14 }, + { 2111, 14 }, + { 2097, 14 }, + { 2083, 14 }, + { 2069, 14 }, + { 2055, 13 }, + { 2042, 13 }, + { 2029, 13 }, + { 2016, 13 }, + { 2003, 13 }, + { 1990, 13 }, + { 1977, 12 }, + { 1965, 12 }, + { 1953, 13 }, + { 1940, 11 }, + { 1929, 12 }, + { 1917, 12 }, + { 1905, 12 }, + { 1893, 11 }, + { 1882, 11 }, + { 1871, 11 }, + { 1860, 11 }, + { 1849, 11 }, + { 1838, 11 }, + { 1827, 11 }, + { 1816, 10 }, + { 1806, 11 }, + { 1795, 10 }, + { 1785, 10 }, + { 1775, 10 }, + { 1765, 10 }, + { 1755, 10 }, + { 1745, 9 }, + { 1736, 10 }, + { 1726, 9 }, + { 1717, 10 }, + { 1707, 9 }, + { 1698, 9 }, + { 1689, 9 }, + { 1680, 9 }, + { 1671, 9 }, + { 1662, 9 }, + { 1653, 9 }, + { 1644, 8 }, + { 1636, 9 }, + { 1627, 8 }, + { 1619, 9 }, + { 1610, 8 }, + { 1602, 8 }, + { 1594, 8 }, + { 1586, 8 }, + { 1578, 8 }, + { 1570, 8 }, + { 1562, 8 }, + { 1554, 7 }, + { 1547, 8 }, + { 1539, 8 }, + { 1531, 7 }, + { 1524, 8 }, + { 1516, 7 }, + { 1509, 7 }, + { 1502, 7 }, + { 1495, 7 }, + { 1488, 7 }, + { 1481, 7 }, + { 1474, 7 }, + { 1467, 7 }, + { 1460, 7 }, + { 1453, 7 }, + { 1446, 6 }, + { 1440, 7 }, + { 1433, 7 }, + { 1426, 6 }, + { 1420, 6 }, + { 1414, 7 }, + { 1407, 6 }, + { 1401, 6 }, + { 1395, 7 }, + { 1388, 6 }, + { 1382, 6 }, + { 1376, 6 }, + { 1370, 6 }, + { 1364, 6 }, + { 1358, 6 }, + { 1352, 6 }, + { 1346, 5 }, + { 1341, 6 }, + { 1335, 6 }, + { 1329, 5 }, + { 1324, 6 }, + { 1318, 5 }, + { 1313, 6 }, + { 1307, 5 }, + { 1302, 6 }, + { 1296, 5 }, + { 1291, 5 }, + { 1286, 6 }, + { 1280, 5 }, + { 1275, 5 }, + { 1270, 5 }, + { 1265, 5 }, + { 1260, 5 }, + { 1255, 5 }, + { 1250, 5 }, + { 1245, 5 }, + { 1240, 5 }, + { 1235, 5 }, + { 1230, 5 }, + { 1225, 5 }, + { 1220, 5 }, + { 1215, 4 }, + { 1211, 5 }, + { 1206, 5 }, + { 1201, 5 }, +}; + +#endif + +} // namespace speed_table +} // namespace modules diff --git a/src/modules/speed_table.h b/src/modules/speed_table.h new file mode 100644 index 0000000..d5d4692 --- /dev/null +++ b/src/modules/speed_table.h @@ -0,0 +1,58 @@ +#pragma once +#include "../config/config.h" +#include "../hal/progmem.h" +#include "math.h" + +namespace modules { + +/// Speed tables for acceleration calculations +namespace speed_table { + +/// Lookup table for rates equal or higher than 8*256 +extern const uint16_t speed_table_fast[256][2] PROGMEM; + +/// Lookup table for lower step rates +extern const uint16_t speed_table_slow[256][2] PROGMEM; + +/// Calculate the next timer interval and steps according to current step rate +static inline uint16_t calc_timer(uint16_t step_rate, uint8_t &step_loops) { + if (step_rate > MAX_STEP_FREQUENCY) + step_rate = MAX_STEP_FREQUENCY; + if (step_rate > 20000) { // If steprate > 20kHz >> step 4 times + step_rate = (step_rate >> 2) & 0x3fff; + step_loops = 4; + } else if (step_rate > 10000) { // If steprate > 10kHz >> step 2 times + step_rate = (step_rate >> 1) & 0x7fff; + step_loops = 2; + } else { + step_loops = 1; + } + + using modules::math::mulU8X16toH16; + namespace pm = hal::progmem; + + uint16_t timer; // calculated interval + + if (step_rate < (F_CPU / 500000)) + step_rate = (F_CPU / 500000); + step_rate -= (F_CPU / 500000); // Correct for minimal speed + if (step_rate >= (8 * 256)) { // higher step rate + const uint16_t *table_address = &speed_table_fast[(uint8_t)(step_rate >> 8)][0]; + uint8_t tmp_step_rate = (step_rate & 0x00ff); + uint16_t gain = pm::read_word(table_address + 1); + timer = mulU8X16toH16(tmp_step_rate, gain); + timer = pm::read_word(table_address) - timer; + } else { // lower step rates + const uint16_t *table_address = &speed_table_slow[0][0]; + table_address += (step_rate >> 2) & 0xfffe; + timer = pm::read_word(table_address); + timer -= ((pm::read_word(table_address + 1) * (uint8_t)(step_rate & 0x0007)) >> 3); + } + if (timer < 100) { + timer = 100; + } // 20kHz this should never happen + return timer; +} + +} // namespace speed_table +} // namespace modules diff --git a/tests/unit/modules/CMakeLists.txt b/tests/unit/modules/CMakeLists.txt index 442fa8a..ae0026a 100644 --- a/tests/unit/modules/CMakeLists.txt +++ b/tests/unit/modules/CMakeLists.txt @@ -1,3 +1,4 @@ add_subdirectory(buttons) add_subdirectory(leds) add_subdirectory(protocol) +add_subdirectory(speed_table) diff --git a/tests/unit/modules/speed_table/CMakeLists.txt b/tests/unit/modules/speed_table/CMakeLists.txt new file mode 100644 index 0000000..947bfed --- /dev/null +++ b/tests/unit/modules/speed_table/CMakeLists.txt @@ -0,0 +1,12 @@ +# define the test executable +add_executable(speed_table_tests + test_speed_table.cpp + ../../../../src/modules/speed_table.cpp) + +# define required search paths +target_include_directories( + speed_table_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules +) + +# tell build system about the test case +add_catch_test(speed_table_tests) diff --git a/tests/unit/modules/speed_table/test_speed_table.cpp b/tests/unit/modules/speed_table/test_speed_table.cpp new file mode 100644 index 0000000..1fecdc1 --- /dev/null +++ b/tests/unit/modules/speed_table/test_speed_table.cpp @@ -0,0 +1,110 @@ +#include "catch2/catch.hpp" +#include "speed_table.h" +#include + +using Catch::Matchers::Equals; +using namespace modules::speed_table; + +// The following reference values are calculated for 16MHz F_CPU +static_assert(F_CPU == 16000000); + +static const uint16_t reference[][3] = { + { 1, 62500, 1 }, + { 501, 3992, 1 }, + { 1001, 1998, 1 }, + { 1501, 1332, 1 }, + { 2001, 1000, 1 }, + { 2501, 801, 1 }, + { 3001, 668, 1 }, + { 3501, 572, 1 }, + { 4001, 500, 1 }, + { 4501, 444, 1 }, + { 5001, 400, 1 }, + { 5501, 364, 1 }, + { 6001, 333, 1 }, + { 6501, 307, 1 }, + { 7001, 285, 1 }, + { 7501, 266, 1 }, + { 8001, 250, 1 }, + { 8501, 234, 1 }, + { 9001, 222, 1 }, + { 9501, 211, 1 }, + { 10001, 400, 2 }, + { 10501, 381, 2 }, + { 11001, 364, 2 }, + { 11501, 348, 2 }, + { 12001, 333, 2 }, + { 12501, 320, 2 }, + { 13001, 308, 2 }, + { 13501, 297, 2 }, + { 14001, 286, 2 }, + { 14501, 276, 2 }, + { 15001, 267, 2 }, + { 15501, 258, 2 }, + { 16001, 250, 2 }, + { 16501, 243, 2 }, + { 17001, 235, 2 }, + { 17501, 228, 2 }, + { 18001, 222, 2 }, + { 18501, 216, 2 }, + { 19001, 211, 2 }, + { 19501, 205, 2 }, + { 20001, 400, 4 }, + { 20501, 391, 4 }, + { 21001, 381, 4 }, + { 21501, 371, 4 }, + { 22001, 364, 4 }, + { 22501, 356, 4 }, + { 23001, 348, 4 }, + { 23501, 340, 4 }, + { 24001, 333, 4 }, + { 24501, 326, 4 }, + { 25001, 320, 4 }, + { 25501, 312, 4 }, + { 26001, 308, 4 }, + { 26501, 301, 4 }, + { 27001, 297, 4 }, + { 27501, 290, 4 }, + { 28001, 286, 4 }, + { 28501, 280, 4 }, + { 29001, 276, 4 }, + { 29501, 270, 4 }, + { 30001, 267, 4 }, + { 30501, 262, 4 }, + { 31001, 258, 4 }, + { 31501, 254, 4 }, + { 32001, 250, 4 }, + { 32501, 247, 4 }, + { 33001, 243, 4 }, + { 33501, 239, 4 }, + { 34001, 235, 4 }, + { 34501, 231, 4 }, + { 35001, 228, 4 }, + { 35501, 225, 4 }, + { 36001, 222, 4 }, + { 36501, 219, 4 }, + { 37001, 216, 4 }, + { 37501, 214, 4 }, + { 38001, 211, 4 }, + { 38501, 208, 4 }, + { 39001, 205, 4 }, + { 39501, 201, 4 }, +}; + +TEST_CASE("speed_table::calc_timer", "[speed_table]") { + // Check the result values of calc_timer against an AVR reference table + for (unsigned i = 0; i != sizeof(reference) / sizeof(*reference); ++i) { + uint16_t step_rate = reference[i][0]; + uint8_t loops; + uint16_t timer = calc_timer(step_rate, loops); + + // allow +/-1 of difference for rounding between the C and ASM versions + REQUIRE(abs((int)timer - (int)reference[i][1]) <= 1); + + // loops should be exact + REQUIRE(loops == reference[i][2]); + + // show the table + printf("%u %u %u\n", step_rate, timer, loops); + } +} From ef7c77646160a170415ba3ba9125e64a42344181 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 4 Jul 2021 17:46:15 +0200 Subject: [PATCH 03/43] Update Motion for the new TMC2130/pins definitions --- src/modules/motion.cpp | 38 +------------------------- src/modules/motion.h | 33 ++++------------------ src/pins.h | 12 ++++++++ tests/unit/logic/stubs/stub_motion.cpp | 2 +- 4 files changed, 20 insertions(+), 65 deletions(-) diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index 22914f6..783f0d3 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -1,5 +1,4 @@ #include "motion.h" -#include "../hal/shr16.h" namespace modules { namespace motion { @@ -22,7 +21,7 @@ uint16_t Motion::CurrentPos(Axis axis) const { return 0; } void Motion::Home(Axis axis, bool direction) {} -void Motion::SetMode(MotorMode mode) {} +void Motion::SetMode(hal::tmc2130::MotorMode mode) {} void Motion::Step() {} @@ -32,40 +31,5 @@ void Motion::AbortPlannedMoves() {} void ISR() {} -//@@TODO check the directions -void StepDirPins::SetIdlerDirUp() { - hal::shr16::shr16.SetTMCDir(Axis::Idler, true); -} - -void StepDirPins::SetIdlerDirDown() { - hal::shr16::shr16.SetTMCDir(Axis::Idler, false); -} - -void StepDirPins::SetSelectorDirLeft() { - hal::shr16::shr16.SetTMCDir(Axis::Selector, true); -} -void StepDirPins::SetSelectorDirRight() { - hal::shr16::shr16.SetTMCDir(Axis::Selector, false); -} - -void StepDirPins::SetPulleyDirPull() { - hal::shr16::shr16.SetTMCDir(Axis::Pulley, true); -} -void StepDirPins::SetPulleyDirPush() { - hal::shr16::shr16.SetTMCDir(Axis::Pulley, false); -} - -void StepDirPins::StepIdler(uint8_t on) { - // PORTD |= idler_step_pin; -} - -void StepDirPins::StepSelector(uint8_t on) { - // PORTD |= selector_step_pin; -} - -void StepDirPins::StepPulley(uint8_t on) { - // PORTB |= pulley_step_pin; -} - } // namespace motion } // namespace modules diff --git a/src/modules/motion.h b/src/modules/motion.h index 17eefdd..6e82696 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -1,5 +1,10 @@ #pragma once #include +#include "../config/config.h" +#include "../hal/tmc2130.h" +#include "../pins.h" + +namespace modules { /// @@TODO /// Logic of motor handling @@ -20,8 +25,6 @@ /// rotate(speed) /// rotate(speed, angle/steps) /// home? - -namespace modules { namespace motion { enum Axis { @@ -30,35 +33,11 @@ enum Axis { Idler, }; -enum MotorMode { - Stealth, - Normal -}; - enum IdlerMode { Engage, Disengage }; -/// As step and dir pins are randomly scattered on the board for each of the axes/motors -/// it is convenient to make a common interface for them -class StepDirPins { -public: - static void SetIdlerDirUp(); - static void SetIdlerDirDown(); - - static void SetSelectorDirLeft(); - static void SetSelectorDirRight(); - - static void SetPulleyDirPull(); - static void SetPulleyDirPush(); - - static void StepIdler(uint8_t on); - static void StepSelector(uint8_t on); - static void StepPulley(uint8_t on); -}; - -/// @@TODO this is subject of discussion and change in the future class Motion { public: inline constexpr Motion() = default; @@ -96,7 +75,7 @@ public: /// Set mode of TMC/motors operation /// Common for all axes/motors - void SetMode(MotorMode mode); + void SetMode(hal::tmc2130::MotorMode mode); /// State machine doing all the planning and stepping preparation based on received commands void Step(); diff --git a/src/pins.h b/src/pins.h index 5476621..9d8ff75 100644 --- a/src/pins.h +++ b/src/pins.h @@ -13,3 +13,15 @@ static constexpr hal::gpio::GPIO_pin SHR16_CLOCK = { GPIOC, 7 }; ///SHCP static constexpr hal::gpio::GPIO_pin USART_RX = { GPIOD, 2 }; static constexpr hal::gpio::GPIO_pin USART_TX = { GPIOD, 3 }; + +static constexpr hal::gpio::GPIO_pin IDLER_CS_PIN = { GPIOB, 0 }; // TODO +static constexpr hal::gpio::GPIO_pin IDLER_SG_PIN = { GPIOB, 0 }; // TODO +static constexpr hal::gpio::GPIO_pin IDLER_STEP_PIN = { GPIOB, 0 }; // TODO + +static constexpr hal::gpio::GPIO_pin PULLEY_CS_PIN = { GPIOB, 0 }; // TODO +static constexpr hal::gpio::GPIO_pin PULLEY_SG_PIN = { GPIOB, 0 }; // TODO +static constexpr hal::gpio::GPIO_pin PULLEY_STEP_PIN = { GPIOB, 0 }; // TODO + +static constexpr hal::gpio::GPIO_pin SELECTOR_CS_PIN = { GPIOB, 0 }; // TODO +static constexpr hal::gpio::GPIO_pin SELECTOR_SG_PIN = { GPIOB, 0 }; // TODO +static constexpr hal::gpio::GPIO_pin SELECTOR_STEP_PIN = { GPIOB, 0 }; // TODO diff --git a/tests/unit/logic/stubs/stub_motion.cpp b/tests/unit/logic/stubs/stub_motion.cpp index 07e5d50..c053c2b 100644 --- a/tests/unit/logic/stubs/stub_motion.cpp +++ b/tests/unit/logic/stubs/stub_motion.cpp @@ -49,7 +49,7 @@ void Motion::Home(Axis axis, bool direction) { axes[Pulley].homed = true; } -void Motion::SetMode(MotorMode mode) { +void Motion::SetMode(hal::tmc2130::MotorMode mode) { } void Motion::Step() { From de88ed4c6bbfb060a353c21627f6f88490952b3a Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 4 Jul 2021 17:56:09 +0200 Subject: [PATCH 04/43] Add initial Axis configuration static structs --- src/config/axis.h | 18 +++++++ src/config/config.h | 42 ++++++++++++++++ src/modules/motion.h | 50 ++++++++++++++++++- src/modules/speed_table.cpp | 8 +-- src/modules/speed_table.h | 10 ++-- .../modules/speed_table/test_speed_table.cpp | 6 +-- 6 files changed, 121 insertions(+), 13 deletions(-) create mode 100644 src/config/axis.h diff --git a/src/config/axis.h b/src/config/axis.h new file mode 100644 index 0000000..ff5a1be --- /dev/null +++ b/src/config/axis.h @@ -0,0 +1,18 @@ +#pragma once +#include + +namespace config { + +/// Axis configuration data +struct AxisConfig { + bool dirOn; ///< direction ON state (for inversion) + uint8_t uSteps; ///< microstepping [1-32] + bool vSense; ///< vSense scaling + uint8_t iRun; ///< running current + uint8_t iHold; ///< holding current + float scale; ///< Scaling unit (unit/uStepsMaxRes) + float accel; ///< Acceleration (unit/s^2) + float jerk; ///< Jerk (unit/s) +}; + +} // namespace config diff --git a/src/config/config.h b/src/config/config.h index 9c8b55f..c396067 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -1,5 +1,6 @@ #pragma once #include +#include "axis.h" #include "todo.h" /// Wrangler for assorted compile-time configuration and constants. @@ -27,4 +28,45 @@ static constexpr const uint16_t buttonsDebounceMs = 100; static constexpr const uint16_t buttonADCLimits[buttonCount][2] = { { 0, 10 }, { 320, 360 }, { 500, 530 } }; static constexpr const uint8_t buttonsADCIndex = 0; ///< ADC index of buttons input +/// Maximum microstepping resolution. This defines the effective unit of +/// the step intevals on the motion API, independently of the selected +/// microstepping interval. +static constexpr uint8_t uStepMaxRes = 32; + +/// Idler configuration +static constexpr AxisConfig idler = { + .dirOn = true, + .uSteps = 16, + .vSense = false, + .iRun = 20, + .iHold = 20, + .scale = 1., + .accel = 100., + .jerk = 1., +}; + +/// Pulley configuration +static constexpr AxisConfig pulley = { + .dirOn = true, + .uSteps = 16, + .vSense = false, + .iRun = 20, + .iHold = 20, + .scale = 1., + .accel = 100., + .jerk = 1., +}; + +/// Selector configuration +static constexpr AxisConfig selector = { + .dirOn = true, + .uSteps = 16, + .vSense = false, + .iRun = 20, + .iHold = 20, + .scale = 1., + .accel = 100., + .jerk = 1., +}; + } // namespace config diff --git a/src/modules/motion.h b/src/modules/motion.h index 6e82696..25cabe5 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -27,13 +27,59 @@ namespace modules { /// home? namespace motion { -enum Axis { +/// Main axis enumeration +enum Axis : uint8_t { Pulley, Selector, Idler, + _Axis_Last = Idler }; -enum IdlerMode { +static constexpr uint8_t NUM_AXIS = _Axis_Last + 1; + +/// Static axis configuration +struct AxisParams { + char name; + hal::tmc2130::MotorParams params; + hal::tmc2130::MotorCurrents currents; + float scale; + float accel; +}; + +static constexpr AxisParams axisParams[NUM_AXIS] = { + // Idler + { + .name = 'I', + .params = { + .idx = Idler, + .dirOn = config::idler.dirOn, + .csPin = IDLER_CS_PIN, + .stepPin = IDLER_STEP_PIN, + .sgPin = IDLER_SG_PIN, + .uSteps = config::idler.uSteps }, + .currents = { .vSense = config::idler.vSense, .iRun = config::idler.iRun, .iHold = config::idler.iHold }, + .scale = config::idler.scale, + .accel = config::idler.accel, + }, + // Pulley + { + .name = 'P', + .params = { .idx = Pulley, .dirOn = config::pulley.dirOn, .csPin = PULLEY_CS_PIN, .stepPin = PULLEY_STEP_PIN, .sgPin = PULLEY_SG_PIN, .uSteps = config::pulley.uSteps }, + .currents = { .vSense = config::pulley.vSense, .iRun = config::pulley.iRun, .iHold = config::pulley.iHold }, + .scale = config::pulley.scale, + .accel = config::pulley.accel, + }, + // Selector + { + .name = 'S', + .params = { .idx = Selector, .dirOn = config::selector.dirOn, .csPin = SELECTOR_CS_PIN, .stepPin = SELECTOR_STEP_PIN, .sgPin = SELECTOR_SG_PIN, .uSteps = config::selector.uSteps }, + .currents = { .vSense = config::selector.vSense, .iRun = config::selector.iRun, .iHold = config::selector.iHold }, + .scale = config::selector.scale, + .accel = config::selector.accel, + }, +}; + +enum IdlerMode : uint8_t { Engage, Disengage }; diff --git a/src/modules/speed_table.cpp b/src/modules/speed_table.cpp index f614eb8..0da0d24 100644 --- a/src/modules/speed_table.cpp +++ b/src/modules/speed_table.cpp @@ -5,7 +5,7 @@ namespace speed_table { #if F_CPU == 16000000 -const uint16_t speed_table_fast[256][2] PROGMEM = { +const st_timer_t speed_table_fast[256][2] PROGMEM = { { 62500, 55556 }, { 6944, 3268 }, { 3676, 1176 }, { 2500, 607 }, { 1893, 369 }, { 1524, 249 }, { 1275, 179 }, { 1096, 135 }, { 961, 105 }, { 856, 85 }, { 771, 69 }, { 702, 58 }, { 644, 49 }, { 595, 42 }, { 553, 37 }, { 516, 32 }, { 484, 28 }, { 456, 25 }, { 431, 23 }, { 408, 20 }, { 388, 19 }, { 369, 16 }, { 353, 16 }, { 337, 14 }, @@ -40,7 +40,7 @@ const uint16_t speed_table_fast[256][2] PROGMEM = { { 31, 0 }, { 31, 0 }, { 31, 0 }, { 31, 1 }, { 30, 0 }, { 30, 0 }, { 30, 0 }, { 30, 0 } }; -const uint16_t speed_table_slow[256][2] PROGMEM = { +const st_timer_t speed_table_slow[256][2] PROGMEM = { { 62500, 12500 }, { 50000, 8334 }, { 41666, 5952 }, { 35714, 4464 }, { 31250, 3473 }, { 27777, 2777 }, { 25000, 2273 }, { 22727, 1894 }, { 20833, 1603 }, { 19230, 1373 }, { 17857, 1191 }, { 16666, 1041 }, { 15625, 920 }, { 14705, 817 }, { 13888, 731 }, { 13157, 657 }, { 12500, 596 }, { 11904, 541 }, { 11363, 494 }, { 10869, 453 }, { 10416, 416 }, { 10000, 385 }, { 9615, 356 }, { 9259, 331 }, @@ -77,7 +77,7 @@ const uint16_t speed_table_slow[256][2] PROGMEM = { #elif F_CPU == 20000000 -const uint16_t speed_table_fast[256][2] PROGMEM = { +const st_timer_t speed_table_fast[256][2] PROGMEM = { { 62500, 54055 }, { 8445, 3917 }, { 4528, 1434 }, @@ -336,7 +336,7 @@ const uint16_t speed_table_fast[256][2] PROGMEM = { { 38, 0 }, }; -const uint16_t speed_table_slow[256][2] PROGMEM = { +const st_timer_t speed_table_slow[256][2] PROGMEM = { { 62500, 10417 }, { 52083, 7441 }, { 44642, 5580 }, diff --git a/src/modules/speed_table.h b/src/modules/speed_table.h index d5d4692..667c0a2 100644 --- a/src/modules/speed_table.h +++ b/src/modules/speed_table.h @@ -8,14 +8,16 @@ namespace modules { /// Speed tables for acceleration calculations namespace speed_table { +typedef uint16_t st_timer_t; + /// Lookup table for rates equal or higher than 8*256 -extern const uint16_t speed_table_fast[256][2] PROGMEM; +extern const st_timer_t speed_table_fast[256][2] PROGMEM; /// Lookup table for lower step rates -extern const uint16_t speed_table_slow[256][2] PROGMEM; +extern const st_timer_t speed_table_slow[256][2] PROGMEM; /// Calculate the next timer interval and steps according to current step rate -static inline uint16_t calc_timer(uint16_t step_rate, uint8_t &step_loops) { +static inline st_timer_t calc_timer(st_timer_t step_rate, uint8_t &step_loops) { if (step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY; if (step_rate > 20000) { // If steprate > 20kHz >> step 4 times @@ -31,7 +33,7 @@ static inline uint16_t calc_timer(uint16_t step_rate, uint8_t &step_loops) { using modules::math::mulU8X16toH16; namespace pm = hal::progmem; - uint16_t timer; // calculated interval + st_timer_t timer; // calculated interval if (step_rate < (F_CPU / 500000)) step_rate = (F_CPU / 500000); diff --git a/tests/unit/modules/speed_table/test_speed_table.cpp b/tests/unit/modules/speed_table/test_speed_table.cpp index 1fecdc1..21c8801 100644 --- a/tests/unit/modules/speed_table/test_speed_table.cpp +++ b/tests/unit/modules/speed_table/test_speed_table.cpp @@ -8,7 +8,7 @@ using namespace modules::speed_table; // The following reference values are calculated for 16MHz F_CPU static_assert(F_CPU == 16000000); -static const uint16_t reference[][3] = { +static const st_timer_t reference[][3] = { { 1, 62500, 1 }, { 501, 3992, 1 }, { 1001, 1998, 1 }, @@ -94,9 +94,9 @@ static const uint16_t reference[][3] = { TEST_CASE("speed_table::calc_timer", "[speed_table]") { // Check the result values of calc_timer against an AVR reference table for (unsigned i = 0; i != sizeof(reference) / sizeof(*reference); ++i) { - uint16_t step_rate = reference[i][0]; + st_timer_t step_rate = reference[i][0]; uint8_t loops; - uint16_t timer = calc_timer(step_rate, loops); + st_timer_t timer = calc_timer(step_rate, loops); // allow +/-1 of difference for rounding between the C and ASM versions REQUIRE(abs((int)timer - (int)reference[i][1]) <= 1); From 7d2329cf8547fc76e428cf37841e411ee87b2f1a Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 5 Jul 2021 18:13:27 +0200 Subject: [PATCH 05/43] Add src/cmath.h for portability between platforms --- src/cmath.h | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 src/cmath.h diff --git a/src/cmath.h b/src/cmath.h new file mode 100644 index 0000000..bf876b3 --- /dev/null +++ b/src/cmath.h @@ -0,0 +1,17 @@ +// Provide an uniform interface for basic math functions between AVR libc and newer +// standards that support +#pragma once + +#ifndef __AVR__ + #include +#else + + // AVR libc doesn't support cmath + #include + + // Use builtin functions for min/max/abs + #define min(a, b) __builtin_min((a, b)) + #define max(a, b) __builtin_max((a, b)) + #define abs(n) __builtin_abs((n)) + +#endif From cf5be5aade670b10b1a26a3c97dc41de81734aef Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 5 Jul 2021 18:20:50 +0200 Subject: [PATCH 06/43] PulseGen: initial version of the ramp/pulse generator --- src/config/todo.h | 9 +- src/modules/pulse_gen.cpp | 243 ++++++++++++++++++ src/modules/pulse_gen.h | 95 +++++++ tests/unit/modules/CMakeLists.txt | 1 + tests/unit/modules/pulse_gen/CMakeLists.txt | 16 ++ .../unit/modules/pulse_gen/test_pulse_gen.cpp | 35 +++ 6 files changed, 398 insertions(+), 1 deletion(-) create mode 100644 src/modules/pulse_gen.cpp create mode 100644 src/modules/pulse_gen.h create mode 100644 tests/unit/modules/pulse_gen/CMakeLists.txt create mode 100644 tests/unit/modules/pulse_gen/test_pulse_gen.cpp diff --git a/src/config/todo.h b/src/config/todo.h index 2dba1e1..3331837 100644 --- a/src/config/todo.h +++ b/src/config/todo.h @@ -4,4 +4,11 @@ #define F_CPU 16000000 #endif -#define MAX_STEP_FREQUENCY 40000 // Max step frequency +// Max step frequency 40KHz +#define MAX_STEP_FREQUENCY 40000 + +// Minimum stepper rate 120Hz. +#define MINIMAL_STEP_RATE 120 + +// Step frequency divider (influences the speed tables!) +#define STEP_TIMER_DIVIDER 8 diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp new file mode 100644 index 0000000..3196081 --- /dev/null +++ b/src/modules/pulse_gen.cpp @@ -0,0 +1,243 @@ +#include "pulse_gen.h" +using hal::tmc2130::MotorParams; +using hal::tmc2130::TMC2130; +using modules::math::mulU24X24toH16; +using modules::speed_table::calc_timer; + +#include "../cmath.h" + +namespace modules { +namespace pulse_gen { + +PulseGen::PulseGen() { + // Some initial values + position = 0; + acceleration = 1200; + block_buffer_head = block_buffer_tail = 0; + current_block = nullptr; + + // TODO: configuration constants + dropsegments = 5; + + // TODO: base units for the axis + steps_t max_acceleration_units_per_sq_second = 2500; // mm/s2 + axis_steps_per_unit = 100.f; // steps/mm + max_jerk = 10.f; + + // TODO: derived for trapezoid calculations + axis_steps_per_sqr_second = max_acceleration_units_per_sq_second * axis_steps_per_unit; +} + +// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. +void PulseGen::calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed) { + // These two lines are the only floating point calculations performed in this routine. + // initial_rate, final_rate in Hz. + // Minimum stepper rate 120Hz, maximum 40kHz. If the stepper rate goes above 10kHz, + // the stepper interrupt routine groups the pulses by 2 or 4 pulses per interrupt tick. + rate_t initial_rate = ceil(entry_speed * block->speed_factor); // (step/min) + rate_t final_rate = ceil(exit_speed * block->speed_factor); // (step/min) + + // Limit minimal step rate (Otherwise the timer will overflow.) + if (initial_rate < MINIMAL_STEP_RATE) + initial_rate = MINIMAL_STEP_RATE; + if (initial_rate > block->nominal_rate) + initial_rate = block->nominal_rate; + if (final_rate < MINIMAL_STEP_RATE) + final_rate = MINIMAL_STEP_RATE; + if (final_rate > block->nominal_rate) + final_rate = block->nominal_rate; + + rate_t acceleration = block->acceleration_st; + + // Don't allow zero acceleration. + if (acceleration == 0) + acceleration = 1; + + // estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) + // (target_rate*target_rate-initial_rate*initial_rate)/(2.0*acceleration)); + rate_t initial_rate_sqr = initial_rate * initial_rate; + rate_t nominal_rate_sqr = block->nominal_rate * block->nominal_rate; + rate_t final_rate_sqr = final_rate * final_rate; + rate_t acceleration_x2 = acceleration << 1; + // ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration)); + steps_t accelerate_steps = (nominal_rate_sqr - initial_rate_sqr + acceleration_x2 - 1) / acceleration_x2; + // floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration)); + steps_t decelerate_steps = (nominal_rate_sqr - final_rate_sqr) / acceleration_x2; + steps_t accel_decel_steps = accelerate_steps + decelerate_steps; + // Size of Plateau of Nominal Rate. + steps_t plateau_steps = 0; + + // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will + // have to use intersection_distance() to calculate when to abort acceleration and start braking + // in order to reach the final_rate exactly at the end of this block. + if (accel_decel_steps < block->step_event_count) { + plateau_steps = block->step_event_count - accel_decel_steps; + } else { + uint32_t acceleration_x4 = acceleration << 2; + // Avoid negative numbers + if (final_rate_sqr >= initial_rate_sqr) { + // accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count)); + // intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) + // (2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4.0*acceleration); +#if 0 + accelerate_steps = (block->step_event_count >> 1) + (final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1 + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4; +#else + accelerate_steps = final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1; + if (block->step_event_count & 1) + accelerate_steps += acceleration_x2; + accelerate_steps /= acceleration_x4; + accelerate_steps += (block->step_event_count >> 1); +#endif + if (accelerate_steps > block->step_event_count) + accelerate_steps = block->step_event_count; + } else { +#if 0 + decelerate_steps = (block->step_event_count >> 1) + (initial_rate_sqr - final_rate_sqr + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4; +#else + decelerate_steps = initial_rate_sqr - final_rate_sqr; + if (block->step_event_count & 1) + decelerate_steps += acceleration_x2; + decelerate_steps /= acceleration_x4; + decelerate_steps += (block->step_event_count >> 1); +#endif + if (decelerate_steps > block->step_event_count) + decelerate_steps = block->step_event_count; + accelerate_steps = block->step_event_count - decelerate_steps; + } + } + + block->accelerate_until = accelerate_steps; + block->decelerate_after = accelerate_steps + plateau_steps; + block->initial_rate = initial_rate; + block->final_rate = final_rate; +} + +// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in +// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration +// calculation the caller must also provide the physical length of the line in millimeters. +void PulseGen::Move(float x, float feed_rate) { + // Prepare to set up new block + block_t *block = &block_buffer[block_buffer_head]; + + // The target position of the tool in absolute steps + // Calculate target position in absolute steps + long target = lround(x * axis_steps_per_unit); + + block->step_event_count = abs(target - position); + + // Bail if this is a zero-length block + if (block->step_event_count <= dropsegments) + return; + + // Compute direction bits for this block + block->direction = (target < position); + + float delta_mm = (target - position) / axis_steps_per_unit; + block->millimeters = abs(delta_mm); + + float inverse_millimeters = 1.0f / block->millimeters; // Inverse millimeters to remove multiple divides + + // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. + float inverse_second = feed_rate * inverse_millimeters; + + block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 + block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 + + // Compute and limit the acceleration rate for the trapezoid generator. + float steps_per_mm = block->step_event_count / block->millimeters; + + // Acceleration of the segment, in mm/sec^2 + block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 + block->acceleration = acceleration; + block->acceleration_rate = ((float)block->acceleration_st * (float)(16777216.0 / (F_CPU / 8.0))); + + // Precalculate the division, so when all the trapezoids in the planner queue get recalculated, the division is not repeated. + block->speed_factor = block->nominal_rate / block->nominal_speed; + + // TODO: chain moves? + calculate_trapezoid_for_block(block, max_jerk, max_jerk); + + // TODO: Move the buffer head + //block_buffer_head++; +} + +st_timer_t PulseGen::Step(const MotorParams &motorParams) { + if (!current_block) { + // TODO: fetch next block + if (!block_buffer_head) + current_block = &block_buffer[block_buffer_head++]; + if (!current_block) + return 0; + + // Set direction early so that the direction-change delay is accounted for + TMC2130::SetDir(motorParams, current_block->direction); + + // Initializes the trapezoid generator from the current block. + deceleration_time = 0; + acc_step_rate = uint16_t(current_block->initial_rate); + acceleration_time = calc_timer(acc_step_rate, step_loops); + step_events_completed = 0; + + // Set the nominal step loops to zero to indicate, that the timer value is not known yet. + // That means, delay the initialization of nominal step rate and step loops until the steady + // state is reached. + step_loops_nominal = 0; + } + + // Step the motor + for (uint8_t i = 0; i < step_loops; ++i) { + TMC2130::Step(motorParams); + if (++step_events_completed >= current_block->step_event_count) + break; + } + + // Calculate new timer value + // 13.38-14.63us for steady state, + // 25.12us for acceleration / deceleration. + st_timer_t timer; + if (step_events_completed <= current_block->accelerate_until) { + // v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate + acc_step_rate = mulU24X24toH16(acceleration_time, current_block->acceleration_rate); + acc_step_rate += uint16_t(current_block->initial_rate); + // upper limit + if (acc_step_rate > uint16_t(current_block->nominal_rate)) + acc_step_rate = current_block->nominal_rate; + // step_rate to timer interval + timer = calc_timer(acc_step_rate, step_loops); + acceleration_time += timer; + } else if (step_events_completed > current_block->decelerate_after) { + st_timer_t step_rate = mulU24X24toH16(deceleration_time, current_block->acceleration_rate); + + if (step_rate > acc_step_rate) { // Check step_rate stays positive + step_rate = uint16_t(current_block->final_rate); + } else { + step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point. + + // lower limit + if (step_rate < current_block->final_rate) + step_rate = uint16_t(current_block->final_rate); + } + + // Step_rate to timer interval. + timer = calc_timer(step_rate, step_loops); + deceleration_time += timer; + } else { + if (!step_loops_nominal) { + // Calculation of the steady state timer rate has been delayed to the 1st tick of the steady state to lower + // the initial interrupt blocking. + timer_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops); + step_loops_nominal = step_loops; + } + timer = timer_nominal; + } + + // If current block is finished, reset pointer + if (step_events_completed >= current_block->step_event_count) { + current_block = nullptr; + } + + return timer; +} + +} // namespace motor +} // namespace modules diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h new file mode 100644 index 0000000..93c0ef0 --- /dev/null +++ b/src/modules/pulse_gen.h @@ -0,0 +1,95 @@ +#pragma once +#include +#include "speed_table.h" +#include "../hal/tmc2130.h" + +namespace modules { +namespace pulse_gen { + +using speed_table::st_timer_t; +typedef uint32_t steps_t; +typedef uint32_t rate_t; +typedef int32_t pos_t; + +struct block_t { + // Fields used by the bresenham algorithm for tracing the line + // steps_x.y,z, step_event_count, acceleration_rate, direction_bits and active_extruder are set by plan_buffer_line(). + steps_t step_event_count; // The number of step events required to complete this block + rate_t acceleration_rate; // The acceleration rate used for acceleration calculation + bool direction; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + + // accelerate_until and decelerate_after are set by calculate_trapezoid_for_block() and they need to be synchronized with the stepper interrupt controller. + steps_t accelerate_until; // The index of the step event on which to stop acceleration + steps_t decelerate_after; // The index of the step event on which to start decelerating + + // Fields used by the motion planner to manage acceleration + // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/sec for each axis + // The nominal speed for this block in mm/sec. + // This speed may or may not be reached due to the jerk and acceleration limits. + float nominal_speed; + // Entry speed at previous-current junction in mm/sec, respecting the acceleration and jerk limits. + // The entry speed limit of the current block equals the exit speed of the preceding block. + //float entry_speed; + + // The total travel of this block in mm + float millimeters; + // acceleration mm/sec^2 + float acceleration; + + // Settings for the trapezoid generator (runs inside an interrupt handler). + // Changing the following values in the planner needs to be synchronized with the interrupt handler by disabling the interrupts. + rate_t nominal_rate; // The nominal step rate for this block in step_events/sec + rate_t initial_rate; // The jerk-adjusted step rate at start of block + rate_t final_rate; // The minimal rate at exit + rate_t acceleration_st; // acceleration steps/sec^2 + + // Pre-calculated division for the calculate_trapezoid_for_block() routine to run faster. + float speed_factor; +}; + +class PulseGen { +public: + PulseGen(); + + float Acceleration() const { return acceleration; }; + void SetAcceleration(float accel) { acceleration = accel; } + + void Move(float x, float feed_rate); + float Position() const; + bool QueueEmpty() const; + bool Full() const; + + st_timer_t Step(const hal::tmc2130::MotorParams &motorParams); + +private: + //{ units constants + steps_t axis_steps_per_sqr_second; + float axis_steps_per_unit; + float max_jerk; + steps_t dropsegments; // segments are dropped if lower than that + //} + + //{ block buffer + block_t block_buffer[2]; + block_t *current_block; + uint8_t block_buffer_head; + uint8_t block_buffer_tail; + //} + + //{ state + pos_t position; + float acceleration; + + rate_t acceleration_time, deceleration_time; + st_timer_t acc_step_rate; // decelaration start point + uint8_t step_loops; + uint8_t step_loops_nominal; + st_timer_t timer_nominal; + steps_t step_events_completed; + //} + + void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed); +}; + +} // namespace pulse_gen +} // namespace modules diff --git a/tests/unit/modules/CMakeLists.txt b/tests/unit/modules/CMakeLists.txt index ae0026a..6467464 100644 --- a/tests/unit/modules/CMakeLists.txt +++ b/tests/unit/modules/CMakeLists.txt @@ -2,3 +2,4 @@ add_subdirectory(buttons) add_subdirectory(leds) add_subdirectory(protocol) add_subdirectory(speed_table) +add_subdirectory(pulse_gen) diff --git a/tests/unit/modules/pulse_gen/CMakeLists.txt b/tests/unit/modules/pulse_gen/CMakeLists.txt new file mode 100644 index 0000000..8f0bc24 --- /dev/null +++ b/tests/unit/modules/pulse_gen/CMakeLists.txt @@ -0,0 +1,16 @@ +# define the test executable +add_executable(pulse_gen_tests + test_pulse_gen.cpp + ../../../../src/modules/pulse_gen.cpp + ../../../../src/modules/speed_table.cpp + ../stubs/stub_shr16.cpp + ../stubs/stub_gpio.cpp +) + +# define required search paths +target_include_directories( + pulse_gen_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules ${CMAKE_SOURCE_DIR}/src/hal +) + +# tell build system about the test case +add_catch_test(pulse_gen_tests) diff --git a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp new file mode 100644 index 0000000..84b66fd --- /dev/null +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -0,0 +1,35 @@ +#include "catch2/catch.hpp" +#include "pulse_gen.h" +#include "../pins.h" +#include + +using Catch::Matchers::Equals; +using namespace modules::pulse_gen; +using hal::tmc2130::MotorParams; + +TEST_CASE("pulse_gen::basic", "[pulse_gen]") { + MotorParams mp = { + .idx = 0, + .dirOn = config::idler.dirOn, + .csPin = IDLER_CS_PIN, + .stepPin = IDLER_STEP_PIN, + .sgPin = IDLER_SG_PIN, + .uSteps = config::idler.uSteps + }; + + for (int accel = 100; accel <= 5000; accel *= 2) { + PulseGen pg; + pg.SetAcceleration(accel); + pg.Move(100, 100); + + unsigned long ts = 0; + st_timer_t next; + do { + next = pg.Step(mp); + printf("%lu %u\n", ts, next); + ts += next; + } while (next); + + printf("\n\n"); + } +} From 006dfd4abcdb231a2d63d7f03e906eec20348110 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 5 Jul 2021 19:56:14 +0200 Subject: [PATCH 07/43] PulseGen: remove all floating point calculations Work in steps, steps/s, steps/s2 directly. --- src/modules/pulse_gen.cpp | 109 ++++++------------ src/modules/pulse_gen.h | 109 +++++++++--------- .../unit/modules/pulse_gen/test_pulse_gen.cpp | 2 +- 3 files changed, 87 insertions(+), 133 deletions(-) diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index 3196081..500c5a8 100644 --- a/src/modules/pulse_gen.cpp +++ b/src/modules/pulse_gen.cpp @@ -18,24 +18,14 @@ PulseGen::PulseGen() { // TODO: configuration constants dropsegments = 5; - - // TODO: base units for the axis - steps_t max_acceleration_units_per_sq_second = 2500; // mm/s2 - axis_steps_per_unit = 100.f; // steps/mm - max_jerk = 10.f; - - // TODO: derived for trapezoid calculations - axis_steps_per_sqr_second = max_acceleration_units_per_sq_second * axis_steps_per_unit; + max_jerk = 100; } -// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. -void PulseGen::calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed) { - // These two lines are the only floating point calculations performed in this routine. - // initial_rate, final_rate in Hz. +void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t exit_speed) { // Minimum stepper rate 120Hz, maximum 40kHz. If the stepper rate goes above 10kHz, // the stepper interrupt routine groups the pulses by 2 or 4 pulses per interrupt tick. - rate_t initial_rate = ceil(entry_speed * block->speed_factor); // (step/min) - rate_t final_rate = ceil(exit_speed * block->speed_factor); // (step/min) + rate_t initial_rate = entry_speed; + rate_t final_rate = exit_speed; // Limit minimal step rate (Otherwise the timer will overflow.) if (initial_rate < MINIMAL_STEP_RATE) @@ -47,9 +37,8 @@ void PulseGen::calculate_trapezoid_for_block(block_t *block, float entry_speed, if (final_rate > block->nominal_rate) final_rate = block->nominal_rate; - rate_t acceleration = block->acceleration_st; - // Don't allow zero acceleration. + rate_t acceleration = block->acceleration; if (acceleration == 0) acceleration = 1; @@ -70,39 +59,31 @@ void PulseGen::calculate_trapezoid_for_block(block_t *block, float entry_speed, // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will // have to use intersection_distance() to calculate when to abort acceleration and start braking // in order to reach the final_rate exactly at the end of this block. - if (accel_decel_steps < block->step_event_count) { - plateau_steps = block->step_event_count - accel_decel_steps; + if (accel_decel_steps < block->steps) { + plateau_steps = block->steps - accel_decel_steps; } else { uint32_t acceleration_x4 = acceleration << 2; // Avoid negative numbers if (final_rate_sqr >= initial_rate_sqr) { - // accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count)); + // accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->steps)); // intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) // (2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4.0*acceleration); -#if 0 - accelerate_steps = (block->step_event_count >> 1) + (final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1 + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4; -#else accelerate_steps = final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1; - if (block->step_event_count & 1) + if (block->steps & 1) accelerate_steps += acceleration_x2; accelerate_steps /= acceleration_x4; - accelerate_steps += (block->step_event_count >> 1); -#endif - if (accelerate_steps > block->step_event_count) - accelerate_steps = block->step_event_count; + accelerate_steps += (block->steps >> 1); + if (accelerate_steps > block->steps) + accelerate_steps = block->steps; } else { -#if 0 - decelerate_steps = (block->step_event_count >> 1) + (initial_rate_sqr - final_rate_sqr + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4; -#else decelerate_steps = initial_rate_sqr - final_rate_sqr; - if (block->step_event_count & 1) + if (block->steps & 1) decelerate_steps += acceleration_x2; decelerate_steps /= acceleration_x4; - decelerate_steps += (block->step_event_count >> 1); -#endif - if (decelerate_steps > block->step_event_count) - decelerate_steps = block->step_event_count; - accelerate_steps = block->step_event_count - decelerate_steps; + decelerate_steps += (block->steps >> 1); + if (decelerate_steps > block->steps) + decelerate_steps = block->steps; + accelerate_steps = block->steps - decelerate_steps; } } @@ -112,53 +93,31 @@ void PulseGen::calculate_trapezoid_for_block(block_t *block, float entry_speed, block->final_rate = final_rate; } -// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in -// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration -// calculation the caller must also provide the physical length of the line in millimeters. -void PulseGen::Move(float x, float feed_rate) { +void PulseGen::Move(pos_t target, steps_t feed_rate) { // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; - // The target position of the tool in absolute steps - // Calculate target position in absolute steps - long target = lround(x * axis_steps_per_unit); - - block->step_event_count = abs(target - position); + block->steps = abs(target - position); // Bail if this is a zero-length block - if (block->step_event_count <= dropsegments) + if (block->steps <= dropsegments) return; - // Compute direction bits for this block - block->direction = (target < position); + // Direction and speed for this block + block->direction = (target > position); + block->nominal_rate = feed_rate; - float delta_mm = (target - position) / axis_steps_per_unit; - block->millimeters = abs(delta_mm); - - float inverse_millimeters = 1.0f / block->millimeters; // Inverse millimeters to remove multiple divides - - // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. - float inverse_second = feed_rate * inverse_millimeters; - - block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 - block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 - - // Compute and limit the acceleration rate for the trapezoid generator. - float steps_per_mm = block->step_event_count / block->millimeters; - - // Acceleration of the segment, in mm/sec^2 - block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 + // Acceleration of the segment, in steps/sec^2 block->acceleration = acceleration; - block->acceleration_rate = ((float)block->acceleration_st * (float)(16777216.0 / (F_CPU / 8.0))); + block->acceleration_rate = block->acceleration * (rate_t)((float)F_CPU / (F_CPU / STEP_TIMER_DIVIDER)); - // Precalculate the division, so when all the trapezoids in the planner queue get recalculated, the division is not repeated. - block->speed_factor = block->nominal_rate / block->nominal_speed; - - // TODO: chain moves? - calculate_trapezoid_for_block(block, max_jerk, max_jerk); + // Perform the trapezoid calculations + CalculateTrapezoid(block, max_jerk, max_jerk); // TODO: Move the buffer head //block_buffer_head++; + + position = target; } st_timer_t PulseGen::Step(const MotorParams &motorParams) { @@ -176,7 +135,7 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { deceleration_time = 0; acc_step_rate = uint16_t(current_block->initial_rate); acceleration_time = calc_timer(acc_step_rate, step_loops); - step_events_completed = 0; + steps_completed = 0; // Set the nominal step loops to zero to indicate, that the timer value is not known yet. // That means, delay the initialization of nominal step rate and step loops until the steady @@ -187,7 +146,7 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { // Step the motor for (uint8_t i = 0; i < step_loops; ++i) { TMC2130::Step(motorParams); - if (++step_events_completed >= current_block->step_event_count) + if (++steps_completed >= current_block->steps) break; } @@ -195,7 +154,7 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { // 13.38-14.63us for steady state, // 25.12us for acceleration / deceleration. st_timer_t timer; - if (step_events_completed <= current_block->accelerate_until) { + if (steps_completed <= current_block->accelerate_until) { // v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate acc_step_rate = mulU24X24toH16(acceleration_time, current_block->acceleration_rate); acc_step_rate += uint16_t(current_block->initial_rate); @@ -205,7 +164,7 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { // step_rate to timer interval timer = calc_timer(acc_step_rate, step_loops); acceleration_time += timer; - } else if (step_events_completed > current_block->decelerate_after) { + } else if (steps_completed > current_block->decelerate_after) { st_timer_t step_rate = mulU24X24toH16(deceleration_time, current_block->acceleration_rate); if (step_rate > acc_step_rate) { // Check step_rate stays positive @@ -232,7 +191,7 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { } // If current block is finished, reset pointer - if (step_events_completed >= current_block->step_event_count) { + if (steps_completed >= current_block->steps) { current_block = nullptr; } diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index 93c0ef0..11149d1 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -4,91 +4,86 @@ #include "../hal/tmc2130.h" namespace modules { + +/// Acceleration ramp and stepper pulse generator namespace pulse_gen { using speed_table::st_timer_t; -typedef uint32_t steps_t; -typedef uint32_t rate_t; -typedef int32_t pos_t; - -struct block_t { - // Fields used by the bresenham algorithm for tracing the line - // steps_x.y,z, step_event_count, acceleration_rate, direction_bits and active_extruder are set by plan_buffer_line(). - steps_t step_event_count; // The number of step events required to complete this block - rate_t acceleration_rate; // The acceleration rate used for acceleration calculation - bool direction; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) - - // accelerate_until and decelerate_after are set by calculate_trapezoid_for_block() and they need to be synchronized with the stepper interrupt controller. - steps_t accelerate_until; // The index of the step event on which to stop acceleration - steps_t decelerate_after; // The index of the step event on which to start decelerating - - // Fields used by the motion planner to manage acceleration - // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/sec for each axis - // The nominal speed for this block in mm/sec. - // This speed may or may not be reached due to the jerk and acceleration limits. - float nominal_speed; - // Entry speed at previous-current junction in mm/sec, respecting the acceleration and jerk limits. - // The entry speed limit of the current block equals the exit speed of the preceding block. - //float entry_speed; - - // The total travel of this block in mm - float millimeters; - // acceleration mm/sec^2 - float acceleration; - - // Settings for the trapezoid generator (runs inside an interrupt handler). - // Changing the following values in the planner needs to be synchronized with the interrupt handler by disabling the interrupts. - rate_t nominal_rate; // The nominal step rate for this block in step_events/sec - rate_t initial_rate; // The jerk-adjusted step rate at start of block - rate_t final_rate; // The minimal rate at exit - rate_t acceleration_st; // acceleration steps/sec^2 - - // Pre-calculated division for the calculate_trapezoid_for_block() routine to run faster. - float speed_factor; -}; +typedef uint32_t steps_t; ///< Absolute step units +typedef uint32_t rate_t; ///< Type for step rates +typedef int32_t pos_t; ///< Axis position (signed) class PulseGen { public: PulseGen(); - float Acceleration() const { return acceleration; }; - void SetAcceleration(float accel) { acceleration = accel; } + /// @returns the acceleration for the axis + steps_t Acceleration() const { return acceleration; }; - void Move(float x, float feed_rate); - float Position() const; + /// Set acceleration for the axis + void SetAcceleration(steps_t accel) { acceleration = accel; } + + /// Plan a single move (can only be executed when !Full()) + void Move(pos_t x, steps_t feed_rate); + + /// @returns the current position of the axis + pos_t Position() const { return position; } + + /// Set the position of the axis + void SetPosition(pos_t x) { position = x; } + + /// @returns true if all planned moves have been finished bool QueueEmpty() const; + + /// @returns false if new moves can still be planned bool Full() const; + /// Single-step the axis + /// @returns the interval for the next tick st_timer_t Step(const hal::tmc2130::MotorParams &motorParams); private: + /// Motion parameters for the current planned or executing move + struct block_t { + steps_t steps; ///< Step events + bool direction; ///< The direction for this block + + rate_t acceleration_rate; ///< The acceleration rate + steps_t accelerate_until; ///< The index of the step event on which to stop acceleration + steps_t decelerate_after; ///< The index of the step event on which to start decelerating + + // Settings for the trapezoid generator (runs inside an interrupt handler) + rate_t nominal_rate; ///< The nominal step rate for this block in steps/sec + rate_t initial_rate; ///< Rate at start of block + rate_t final_rate; ///< Rate at exit + rate_t acceleration; ///< acceleration steps/sec^2 + }; + //{ units constants - steps_t axis_steps_per_sqr_second; - float axis_steps_per_unit; - float max_jerk; + steps_t max_jerk; steps_t dropsegments; // segments are dropped if lower than that //} - //{ block buffer + // Block buffer parameters block_t block_buffer[2]; block_t *current_block; uint8_t block_buffer_head; uint8_t block_buffer_tail; - //} - //{ state - pos_t position; - float acceleration; + // Axis data + pos_t position; ///< Current axis position + steps_t acceleration; ///< Current axis acceleration + // Step parameters rate_t acceleration_time, deceleration_time; st_timer_t acc_step_rate; // decelaration start point - uint8_t step_loops; - uint8_t step_loops_nominal; - st_timer_t timer_nominal; - steps_t step_events_completed; - //} + uint8_t step_loops; // steps per loop + uint8_t step_loops_nominal; // steps per loop at nominal speed + st_timer_t timer_nominal; // nominal interval + steps_t steps_completed; // steps completed - void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed); + /// Calculate the trapezoid parameters for the block + void CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t exit_speed); }; } // namespace pulse_gen diff --git a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp index 84b66fd..152f77c 100644 --- a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -20,7 +20,7 @@ TEST_CASE("pulse_gen::basic", "[pulse_gen]") { for (int accel = 100; accel <= 5000; accel *= 2) { PulseGen pg; pg.SetAcceleration(accel); - pg.Move(100, 100); + pg.Move(100000, 10000); unsigned long ts = 0; st_timer_t next; From ed04bd02e27dabccf114ee4c10a88865fd1ec369 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 5 Jul 2021 20:31:08 +0200 Subject: [PATCH 08/43] PulseGen/speed_tables: cleanup constants --- src/config/config.h | 10 +++++++- src/config/todo.h | 14 ----------- src/hal/cpu.h | 5 ++++ src/modules/pulse_gen.cpp | 25 +++++++++---------- src/modules/pulse_gen.h | 8 ++---- src/modules/speed_table.h | 13 +++++++--- .../unit/modules/pulse_gen/test_pulse_gen.cpp | 3 +-- 7 files changed, 39 insertions(+), 39 deletions(-) delete mode 100644 src/config/todo.h diff --git a/src/config/config.h b/src/config/config.h index c396067..b5c139c 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -1,7 +1,6 @@ #pragma once #include #include "axis.h" -#include "todo.h" /// Wrangler for assorted compile-time configuration and constants. namespace config { @@ -33,6 +32,15 @@ static constexpr const uint8_t buttonsADCIndex = 0; ///< ADC index of buttons in /// microstepping interval. static constexpr uint8_t uStepMaxRes = 32; +/// Do not plan moves equal or shorter than the requested steps +static constexpr uint8_t dropSegments = 0; + +/// Max step frequency 40KHz +static constexpr uint16_t maxStepFrequency = 40000; + +/// Minimum stepping rate 120Hz +static constexpr uint16_t minStepRate = 120; + /// Idler configuration static constexpr AxisConfig idler = { .dirOn = true, diff --git a/src/config/todo.h b/src/config/todo.h deleted file mode 100644 index 3331837..0000000 --- a/src/config/todo.h +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#ifndef __AVR__ - #define F_CPU 16000000 -#endif - -// Max step frequency 40KHz -#define MAX_STEP_FREQUENCY 40000 - -// Minimum stepper rate 120Hz. -#define MINIMAL_STEP_RATE 120 - -// Step frequency divider (influences the speed tables!) -#define STEP_TIMER_DIVIDER 8 diff --git a/src/hal/cpu.h b/src/hal/cpu.h index 9775408..a1713a1 100644 --- a/src/hal/cpu.h +++ b/src/hal/cpu.h @@ -5,6 +5,11 @@ namespace hal { namespace cpu { +#ifndef F_CPU + /// Main clock frequency + #define F_CPU (16000000ul) +#endif + /// CPU init routines (not really necessary for the AVR) void Init(); diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index 500c5a8..0c94e35 100644 --- a/src/modules/pulse_gen.cpp +++ b/src/modules/pulse_gen.cpp @@ -9,16 +9,15 @@ using modules::speed_table::calc_timer; namespace modules { namespace pulse_gen { -PulseGen::PulseGen() { - // Some initial values +PulseGen::PulseGen(steps_t max_jerk, steps_t acceleration) { + // Axis status position = 0; - acceleration = 1200; + this->max_jerk = max_jerk; + this->acceleration = acceleration; + + // Block buffer block_buffer_head = block_buffer_tail = 0; current_block = nullptr; - - // TODO: configuration constants - dropsegments = 5; - max_jerk = 100; } void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t exit_speed) { @@ -28,12 +27,12 @@ void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t e rate_t final_rate = exit_speed; // Limit minimal step rate (Otherwise the timer will overflow.) - if (initial_rate < MINIMAL_STEP_RATE) - initial_rate = MINIMAL_STEP_RATE; + if (initial_rate < config::minStepRate) + initial_rate = config::minStepRate; if (initial_rate > block->nominal_rate) initial_rate = block->nominal_rate; - if (final_rate < MINIMAL_STEP_RATE) - final_rate = MINIMAL_STEP_RATE; + if (final_rate < config::minStepRate) + final_rate = config::minStepRate; if (final_rate > block->nominal_rate) final_rate = block->nominal_rate; @@ -100,7 +99,7 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) { block->steps = abs(target - position); // Bail if this is a zero-length block - if (block->steps <= dropsegments) + if (block->steps <= config::dropSegments) return; // Direction and speed for this block @@ -109,7 +108,7 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) { // Acceleration of the segment, in steps/sec^2 block->acceleration = acceleration; - block->acceleration_rate = block->acceleration * (rate_t)((float)F_CPU / (F_CPU / STEP_TIMER_DIVIDER)); + block->acceleration_rate = block->acceleration * (rate_t)((float)F_CPU / (F_CPU / speed_table::cpuFrequencyDivider)); // Perform the trapezoid calculations CalculateTrapezoid(block, max_jerk, max_jerk); diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index 11149d1..5485dec 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -15,7 +15,7 @@ typedef int32_t pos_t; ///< Axis position (signed) class PulseGen { public: - PulseGen(); + PulseGen(steps_t max_jerk, steps_t acceleration); /// @returns the acceleration for the axis steps_t Acceleration() const { return acceleration; }; @@ -59,11 +59,6 @@ private: rate_t acceleration; ///< acceleration steps/sec^2 }; - //{ units constants - steps_t max_jerk; - steps_t dropsegments; // segments are dropped if lower than that - //} - // Block buffer parameters block_t block_buffer[2]; block_t *current_block; @@ -72,6 +67,7 @@ private: // Axis data pos_t position; ///< Current axis position + steps_t max_jerk; ///< Axis jerk (could be constant) steps_t acceleration; ///< Current axis acceleration // Step parameters diff --git a/src/modules/speed_table.h b/src/modules/speed_table.h index 667c0a2..6c88cf1 100644 --- a/src/modules/speed_table.h +++ b/src/modules/speed_table.h @@ -1,6 +1,7 @@ #pragma once #include "../config/config.h" #include "../hal/progmem.h" +#include "../hal/cpu.h" #include "math.h" namespace modules { @@ -10,6 +11,11 @@ namespace speed_table { typedef uint16_t st_timer_t; +/// CPU timer frequency divider required for the speed tables +static constexpr uint8_t cpuFrequencyDivider = 8; +static_assert(F_CPU / cpuFrequencyDivider == 2000000, + "speed tables not compatible for the requested frequency"); + /// Lookup table for rates equal or higher than 8*256 extern const st_timer_t speed_table_fast[256][2] PROGMEM; @@ -18,8 +24,8 @@ extern const st_timer_t speed_table_slow[256][2] PROGMEM; /// Calculate the next timer interval and steps according to current step rate static inline st_timer_t calc_timer(st_timer_t step_rate, uint8_t &step_loops) { - if (step_rate > MAX_STEP_FREQUENCY) - step_rate = MAX_STEP_FREQUENCY; + if (step_rate > config::maxStepFrequency) + step_rate = config::maxStepFrequency; if (step_rate > 20000) { // If steprate > 20kHz >> step 4 times step_rate = (step_rate >> 2) & 0x3fff; step_loops = 4; @@ -51,8 +57,9 @@ static inline st_timer_t calc_timer(st_timer_t step_rate, uint8_t &step_loops) { timer -= ((pm::read_word(table_address + 1) * (uint8_t)(step_rate & 0x0007)) >> 3); } if (timer < 100) { + // 20kHz this should never happen timer = 100; - } // 20kHz this should never happen + } return timer; } diff --git a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp index 152f77c..194c685 100644 --- a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -18,8 +18,7 @@ TEST_CASE("pulse_gen::basic", "[pulse_gen]") { }; for (int accel = 100; accel <= 5000; accel *= 2) { - PulseGen pg; - pg.SetAcceleration(accel); + PulseGen pg(10, accel); pg.Move(100000, 10000); unsigned long ts = 0; From 6f518f1ed268ed664409b5e823a805f71abfa1db Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 6 Jul 2021 08:55:02 +0200 Subject: [PATCH 09/43] PulseGen: complete implementation --- src/config/config.h | 3 +++ src/modules/pulse_gen.cpp | 25 ++++++++++++------------- src/modules/pulse_gen.h | 11 ++++++----- 3 files changed, 21 insertions(+), 18 deletions(-) diff --git a/src/config/config.h b/src/config/config.h index b5c139c..66080d9 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -41,6 +41,9 @@ static constexpr uint16_t maxStepFrequency = 40000; /// Minimum stepping rate 120Hz static constexpr uint16_t minStepRate = 120; +/// Size for the motion planner block buffer size +static constexpr uint8_t blockBufferSize = 2; + /// Idler configuration static constexpr AxisConfig idler = { .dirOn = true, diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index 0c94e35..09e4aef 100644 --- a/src/modules/pulse_gen.cpp +++ b/src/modules/pulse_gen.cpp @@ -16,7 +16,6 @@ PulseGen::PulseGen(steps_t max_jerk, steps_t acceleration) { this->acceleration = acceleration; // Block buffer - block_buffer_head = block_buffer_tail = 0; current_block = nullptr; } @@ -94,7 +93,7 @@ void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t e void PulseGen::Move(pos_t target, steps_t feed_rate) { // Prepare to set up new block - block_t *block = &block_buffer[block_buffer_head]; + block_t *block = &block_buffer[block_index.back()]; block->steps = abs(target - position); @@ -113,17 +112,16 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) { // Perform the trapezoid calculations CalculateTrapezoid(block, max_jerk, max_jerk); - // TODO: Move the buffer head - //block_buffer_head++; - + // Move forward + block_index.push(); position = target; } st_timer_t PulseGen::Step(const MotorParams &motorParams) { if (!current_block) { - // TODO: fetch next block - if (!block_buffer_head) - current_block = &block_buffer[block_buffer_head++]; + // fetch next block + if (!block_index.empty()) + current_block = &block_buffer[block_index.front()]; if (!current_block) return 0; @@ -136,9 +134,9 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { acceleration_time = calc_timer(acc_step_rate, step_loops); steps_completed = 0; - // Set the nominal step loops to zero to indicate, that the timer value is not known yet. - // That means, delay the initialization of nominal step rate and step loops until the steady - // state is reached. + // Set the nominal step loops to zero to indicate, that the timer value is not + // known yet. That means, delay the initialization of nominal step rate and step + // loops until the steady state is reached. step_loops_nominal = 0; } @@ -181,8 +179,8 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { deceleration_time += timer; } else { if (!step_loops_nominal) { - // Calculation of the steady state timer rate has been delayed to the 1st tick of the steady state to lower - // the initial interrupt blocking. + // Calculation of the steady state timer rate has been delayed to the 1st tick + // of the steady state to lower the initial interrupt blocking. timer_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops); step_loops_nominal = step_loops; } @@ -192,6 +190,7 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { // If current block is finished, reset pointer if (steps_completed >= current_block->steps) { current_block = nullptr; + block_index.pop(); } return timer; diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index 5485dec..201dd30 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -2,12 +2,14 @@ #include #include "speed_table.h" #include "../hal/tmc2130.h" +#include "../hal/circular_buffer.h" namespace modules { /// Acceleration ramp and stepper pulse generator namespace pulse_gen { +using config::blockBufferSize; using speed_table::st_timer_t; typedef uint32_t steps_t; ///< Absolute step units typedef uint32_t rate_t; ///< Type for step rates @@ -33,10 +35,10 @@ public: void SetPosition(pos_t x) { position = x; } /// @returns true if all planned moves have been finished - bool QueueEmpty() const; + bool QueueEmpty() const { return block_index.empty(); } /// @returns false if new moves can still be planned - bool Full() const; + bool Full() const { return block_index.full(); } /// Single-step the axis /// @returns the interval for the next tick @@ -60,10 +62,9 @@ private: }; // Block buffer parameters - block_t block_buffer[2]; + block_t block_buffer[blockBufferSize]; + CircularIndex block_index; block_t *current_block; - uint8_t block_buffer_head; - uint8_t block_buffer_tail; // Axis data pos_t position; ///< Current axis position From 44a263d3342869c513da3ab2de6935ec47c37835 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 6 Jul 2021 09:00:43 +0200 Subject: [PATCH 10/43] PulseGen: move Step() to header for inlining --- src/modules/pulse_gen.cpp | 85 --------------------------------------- src/modules/pulse_gen.h | 83 +++++++++++++++++++++++++++++++++++++- 2 files changed, 82 insertions(+), 86 deletions(-) diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index 09e4aef..3d38784 100644 --- a/src/modules/pulse_gen.cpp +++ b/src/modules/pulse_gen.cpp @@ -1,10 +1,4 @@ #include "pulse_gen.h" -using hal::tmc2130::MotorParams; -using hal::tmc2130::TMC2130; -using modules::math::mulU24X24toH16; -using modules::speed_table::calc_timer; - -#include "../cmath.h" namespace modules { namespace pulse_gen { @@ -117,84 +111,5 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) { position = target; } -st_timer_t PulseGen::Step(const MotorParams &motorParams) { - if (!current_block) { - // fetch next block - if (!block_index.empty()) - current_block = &block_buffer[block_index.front()]; - if (!current_block) - return 0; - - // Set direction early so that the direction-change delay is accounted for - TMC2130::SetDir(motorParams, current_block->direction); - - // Initializes the trapezoid generator from the current block. - deceleration_time = 0; - acc_step_rate = uint16_t(current_block->initial_rate); - acceleration_time = calc_timer(acc_step_rate, step_loops); - steps_completed = 0; - - // Set the nominal step loops to zero to indicate, that the timer value is not - // known yet. That means, delay the initialization of nominal step rate and step - // loops until the steady state is reached. - step_loops_nominal = 0; - } - - // Step the motor - for (uint8_t i = 0; i < step_loops; ++i) { - TMC2130::Step(motorParams); - if (++steps_completed >= current_block->steps) - break; - } - - // Calculate new timer value - // 13.38-14.63us for steady state, - // 25.12us for acceleration / deceleration. - st_timer_t timer; - if (steps_completed <= current_block->accelerate_until) { - // v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate - acc_step_rate = mulU24X24toH16(acceleration_time, current_block->acceleration_rate); - acc_step_rate += uint16_t(current_block->initial_rate); - // upper limit - if (acc_step_rate > uint16_t(current_block->nominal_rate)) - acc_step_rate = current_block->nominal_rate; - // step_rate to timer interval - timer = calc_timer(acc_step_rate, step_loops); - acceleration_time += timer; - } else if (steps_completed > current_block->decelerate_after) { - st_timer_t step_rate = mulU24X24toH16(deceleration_time, current_block->acceleration_rate); - - if (step_rate > acc_step_rate) { // Check step_rate stays positive - step_rate = uint16_t(current_block->final_rate); - } else { - step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point. - - // lower limit - if (step_rate < current_block->final_rate) - step_rate = uint16_t(current_block->final_rate); - } - - // Step_rate to timer interval. - timer = calc_timer(step_rate, step_loops); - deceleration_time += timer; - } else { - if (!step_loops_nominal) { - // Calculation of the steady state timer rate has been delayed to the 1st tick - // of the steady state to lower the initial interrupt blocking. - timer_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops); - step_loops_nominal = step_loops; - } - timer = timer_nominal; - } - - // If current block is finished, reset pointer - if (steps_completed >= current_block->steps) { - current_block = nullptr; - block_index.pop(); - } - - return timer; -} - } // namespace motor } // namespace modules diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index 201dd30..fb143f8 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -3,6 +3,7 @@ #include "speed_table.h" #include "../hal/tmc2130.h" #include "../hal/circular_buffer.h" +#include "../cmath.h" namespace modules { @@ -10,6 +11,9 @@ namespace modules { namespace pulse_gen { using config::blockBufferSize; +using hal::tmc2130::TMC2130; +using math::mulU24X24toH16; +using speed_table::calc_timer; using speed_table::st_timer_t; typedef uint32_t steps_t; ///< Absolute step units typedef uint32_t rate_t; ///< Type for step rates @@ -42,7 +46,84 @@ public: /// Single-step the axis /// @returns the interval for the next tick - st_timer_t Step(const hal::tmc2130::MotorParams &motorParams); + inline st_timer_t Step(const hal::tmc2130::MotorParams &motorParams) { + if (!current_block) { + // fetch next block + if (!block_index.empty()) + current_block = &block_buffer[block_index.front()]; + if (!current_block) + return 0; + + // Set direction early so that the direction-change delay is accounted for + TMC2130::SetDir(motorParams, current_block->direction); + + // Initializes the trapezoid generator from the current block. + deceleration_time = 0; + acc_step_rate = uint16_t(current_block->initial_rate); + acceleration_time = calc_timer(acc_step_rate, step_loops); + steps_completed = 0; + + // Set the nominal step loops to zero to indicate, that the timer value is not + // known yet. That means, delay the initialization of nominal step rate and step + // loops until the steady state is reached. + step_loops_nominal = 0; + } + + // Step the motor + for (uint8_t i = 0; i < step_loops; ++i) { + TMC2130::Step(motorParams); + if (++steps_completed >= current_block->steps) + break; + } + + // Calculate new timer value + // 13.38-14.63us for steady state, + // 25.12us for acceleration / deceleration. + st_timer_t timer; + if (steps_completed <= current_block->accelerate_until) { + // v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate + acc_step_rate = mulU24X24toH16(acceleration_time, current_block->acceleration_rate); + acc_step_rate += uint16_t(current_block->initial_rate); + // upper limit + if (acc_step_rate > uint16_t(current_block->nominal_rate)) + acc_step_rate = current_block->nominal_rate; + // step_rate to timer interval + timer = calc_timer(acc_step_rate, step_loops); + acceleration_time += timer; + } else if (steps_completed > current_block->decelerate_after) { + st_timer_t step_rate = mulU24X24toH16(deceleration_time, current_block->acceleration_rate); + + if (step_rate > acc_step_rate) { // Check step_rate stays positive + step_rate = uint16_t(current_block->final_rate); + } else { + step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point. + + // lower limit + if (step_rate < current_block->final_rate) + step_rate = uint16_t(current_block->final_rate); + } + + // Step_rate to timer interval. + timer = calc_timer(step_rate, step_loops); + deceleration_time += timer; + } else { + if (!step_loops_nominal) { + // Calculation of the steady state timer rate has been delayed to the 1st tick + // of the steady state to lower the initial interrupt blocking. + timer_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops); + step_loops_nominal = step_loops; + } + timer = timer_nominal; + } + + // If current block is finished, reset pointer + if (steps_completed >= current_block->steps) { + current_block = nullptr; + block_index.pop(); + } + + return timer; + } private: /// Motion parameters for the current planned or executing move From d7874d5336877efefffde50d74167ab06d65c767 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 6 Jul 2021 09:12:47 +0200 Subject: [PATCH 11/43] PulseGen: implement AbortPlannedMoves --- src/modules/pulse_gen.cpp | 18 +++++++++++++++++- src/modules/pulse_gen.h | 3 +++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index 3d38784..2cea500 100644 --- a/src/modules/pulse_gen.cpp +++ b/src/modules/pulse_gen.cpp @@ -96,7 +96,7 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) { return; // Direction and speed for this block - block->direction = (target > position); + block->direction = (target >= position); block->nominal_rate = feed_rate; // Acceleration of the segment, in steps/sec^2 @@ -111,5 +111,21 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) { position = target; } +void PulseGen::AbortPlannedMoves() { + if (!current_block) + return; + + // update position + steps_t steps_missing = (current_block->steps - steps_completed); + if (current_block->direction) + position -= steps_missing; + else + position += steps_missing; + + // destroy the block + current_block = nullptr; + block_index.pop(); +} + } // namespace motor } // namespace modules diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index fb143f8..03d02c3 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -32,6 +32,9 @@ public: /// Plan a single move (can only be executed when !Full()) void Move(pos_t x, steps_t feed_rate); + /// stop whatever moves are being done + void AbortPlannedMoves(); + /// @returns the current position of the axis pos_t Position() const { return position; } From e990b703e30210a3576941c95dd598c4916b9ac5 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 6 Jul 2021 10:55:40 +0200 Subject: [PATCH 12/43] CMake: reformat --- CMakeLists.txt | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a11afa..9c1e668 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -166,12 +166,16 @@ if(CMAKE_CROSSCOMPILING) # produce ASM listing add_custom_command( - TARGET firmware POST_BUILD COMMAND ${CMAKE_OBJDUMP} -CSd firmware > firmware.asm + TARGET firmware + POST_BUILD + COMMAND ${CMAKE_OBJDUMP} -CSd firmware > firmware.asm ) # inform about the firmware's size in terminal add_custom_command( - TARGET firmware POST_BUILD COMMAND ${CMAKE_SIZE_UTIL} -C --mcu=atmega32u4 firmware + TARGET firmware + POST_BUILD + COMMAND ${CMAKE_SIZE_UTIL} -C --mcu=atmega32u4 firmware ) report_size(firmware) From d87db1ff76cbd5f06dabc340540f4e7679983207 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 6 Jul 2021 10:57:27 +0200 Subject: [PATCH 13/43] Revised WIP for the Motion API - Remove the combined PlanMove(a,b,c,rate) call. If we allow the units of the various motors to be changed at compile time, the unit of rate can vary between axes. - Build PlanMove on top of the absolute PlanMoveTo. - Add required stubs for TMC2130. - Allow each axis mode to be set independently, since we have this feature for free anyway. - Rework internals to use PulseGen data types and structs. --- CMakeLists.txt | 2 + src/config/axis.h | 6 +- src/config/config.h | 18 +-- src/hal/avr/tmc2130.cpp | 13 ++ src/logic/eject_filament.cpp | 2 +- src/logic/feed_to_bondtech.cpp | 2 +- src/logic/feed_to_finda.cpp | 4 +- src/modules/motion.cpp | 24 ++-- src/modules/motion.h | 121 ++++++++++-------- src/modules/pulse_gen.h | 1 - tests/unit/logic/cut_filament/CMakeLists.txt | 2 + .../unit/logic/eject_filament/CMakeLists.txt | 3 + .../logic/feed_to_bondtech/CMakeLists.txt | 2 + tests/unit/logic/feed_to_finda/CMakeLists.txt | 2 + tests/unit/logic/load_filament/CMakeLists.txt | 2 + tests/unit/logic/stubs/stub_motion.cpp | 15 +-- tests/unit/logic/stubs/stub_motion.h | 4 +- tests/unit/logic/tool_change/CMakeLists.txt | 2 + .../unit/logic/unload_filament/CMakeLists.txt | 2 + .../unit/logic/unload_to_finda/CMakeLists.txt | 2 + .../unit/modules/pulse_gen/test_pulse_gen.cpp | 1 + tests/unit/modules/stubs/stub_tmc2130.cpp | 12 ++ 22 files changed, 150 insertions(+), 92 deletions(-) create mode 100644 src/hal/avr/tmc2130.cpp create mode 100644 tests/unit/modules/stubs/stub_tmc2130.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9c1e668..ecca89e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -195,6 +195,7 @@ target_sources( src/hal/avr/usart.cpp src/hal/avr/shr16.cpp src/hal/avr/eeprom.cpp + src/hal/avr/tmc2130.cpp src/hal/adc.cpp src/modules/protocol.cpp src/modules/buttons.cpp @@ -205,6 +206,7 @@ target_sources( src/modules/idler.cpp src/modules/leds.cpp src/modules/motion.cpp + src/modules/pulse_gen.cpp src/modules/permanent_storage.cpp src/modules/selector.cpp src/modules/timebase.cpp diff --git a/src/config/axis.h b/src/config/axis.h index ff5a1be..6ca96a8 100644 --- a/src/config/axis.h +++ b/src/config/axis.h @@ -10,9 +10,9 @@ struct AxisConfig { bool vSense; ///< vSense scaling uint8_t iRun; ///< running current uint8_t iHold; ///< holding current - float scale; ///< Scaling unit (unit/uStepsMaxRes) - float accel; ///< Acceleration (unit/s^2) - float jerk; ///< Jerk (unit/s) + uint16_t accel; ///< Acceleration (unit/s^2) + uint16_t jerk; ///< Jerk (unit/s) + bool stealth; ///< Default to Stealth mode }; } // namespace config diff --git a/src/config/config.h b/src/config/config.h index 66080d9..73aa8ae 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -51,9 +51,9 @@ static constexpr AxisConfig idler = { .vSense = false, .iRun = 20, .iHold = 20, - .scale = 1., - .accel = 100., - .jerk = 1., + .accel = 100, + .jerk = 10, + .stealth = false, }; /// Pulley configuration @@ -63,9 +63,9 @@ static constexpr AxisConfig pulley = { .vSense = false, .iRun = 20, .iHold = 20, - .scale = 1., - .accel = 100., - .jerk = 1., + .accel = 100, + .jerk = 10, + .stealth = false, }; /// Selector configuration @@ -75,9 +75,9 @@ static constexpr AxisConfig selector = { .vSense = false, .iRun = 20, .iHold = 20, - .scale = 1., - .accel = 100., - .jerk = 1., + .accel = 100, + .jerk = 1, + .stealth = false }; } // namespace config diff --git a/src/hal/avr/tmc2130.cpp b/src/hal/avr/tmc2130.cpp new file mode 100644 index 0000000..dd088be --- /dev/null +++ b/src/hal/avr/tmc2130.cpp @@ -0,0 +1,13 @@ +#include "../tmc2130.h" + +namespace hal { +namespace tmc2130 { + +TMC2130::TMC2130(const MotorParams ¶ms, + const MotorCurrents ¤ts, + MotorMode mode) { + // TODO +} + +} // namespace tmc2130 +} // namespace hal diff --git a/src/logic/eject_filament.cpp b/src/logic/eject_filament.cpp index 3db91d9..67e64d4 100644 --- a/src/logic/eject_filament.cpp +++ b/src/logic/eject_filament.cpp @@ -50,7 +50,7 @@ bool EjectFilament::Step() { if (mm::motion.QueueEmpty()) { // selector parked aside state = ProgressCode::EjectingFilament; mm::motion.InitAxis(mm::Pulley); - mm::motion.PlanMove(ejectSteps, 0, 0, 1500, 0, 0); + mm::motion.PlanMove(mm::Pulley, ejectSteps, 1500); } break; case ProgressCode::EjectingFilament: diff --git a/src/logic/feed_to_bondtech.cpp b/src/logic/feed_to_bondtech.cpp index c492867..af2b77b 100644 --- a/src/logic/feed_to_bondtech.cpp +++ b/src/logic/feed_to_bondtech.cpp @@ -30,7 +30,7 @@ bool FeedToBondtech::Step() { if (mi::idler.Engaged()) { state = PushingFilament; ml::leds.SetMode(mg::globals.ActiveSlot(), ml::Color::green, ml::blink0); - mm::motion.PlanMove(steps, 0, 0, 4500, 0, 0); //@@TODO constants - there was some strange acceleration sequence in the original FW, + mm::motion.PlanMove(mm::Pulley, steps, 4500); //@@TODO constants - there was some strange acceleration sequence in the original FW, // we can probably hand over some array of constants for hand-tuned acceleration + leverage some smoothing in the stepper as well } return false; diff --git a/src/logic/feed_to_finda.cpp b/src/logic/feed_to_finda.cpp index d7c400f..d557b70 100644 --- a/src/logic/feed_to_finda.cpp +++ b/src/logic/feed_to_finda.cpp @@ -32,7 +32,7 @@ bool FeedToFinda::Step() { if (mi::idler.Engaged() && ms::selector.Slot() == mg::globals.ActiveSlot()) { state = PushingFilament; ml::leds.SetMode(mg::globals.ActiveSlot(), ml::Color::green, ml::blink0); - mm::motion.PlanMove(feedPhaseLimited ? 1500 : 32767, 0, 0, 4000, 0, 0); //@@TODO constants + mm::motion.PlanMove(mm::Pulley, feedPhaseLimited ? 1500 : 32767, 4000); //@@TODO constants mu::userInput.Clear(); // remove all buffered events if any just before we wait for some input } return false; @@ -41,7 +41,7 @@ bool FeedToFinda::Step() { mm::motion.AbortPlannedMoves(); // stop pushing filament // FINDA triggered - that means it works and detected the filament tip state = UnloadBackToPTFE; - mm::motion.PlanMove(-600, 0, 0, 4000, 0, 0); //@@TODO constants + mm::motion.PlanMove(mm::Pulley, -600, 4000); //@@TODO constants } else if (mm::motion.QueueEmpty()) { // all moves have been finished and FINDA didn't switch on state = Failed; // @@TODO - shall we disengage the idler? diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index 783f0d3..98bc703 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -13,22 +13,28 @@ bool Motion::StallGuard(Axis axis) { return false; } void Motion::ClearStallGuardFlag(Axis axis) {} -void Motion::PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed) {} +void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) {} -void Motion::PlanMove(Axis axis, int16_t delta, uint16_t feedrate) {} - -uint16_t Motion::CurrentPos(Axis axis) const { return 0; } +pos_t Motion::CurrentPos(Axis axis) const { return axisData[axis].ctrl.Position(); } void Motion::Home(Axis axis, bool direction) {} -void Motion::SetMode(hal::tmc2130::MotorMode mode) {} +void Motion::SetMode(Axis axis, MotorMode mode) {} + +bool Motion::QueueEmpty() const { + for (uint8_t i = 0; i != NUM_AXIS; ++i) + if (!axisData[i].ctrl.QueueEmpty()) + return false; + return true; +} + +void Motion::AbortPlannedMoves() { + for (uint8_t i = 0; i != NUM_AXIS; ++i) + axisData[i].ctrl.AbortPlannedMoves(); +} void Motion::Step() {} -bool Motion::QueueEmpty() const { return false; } - -void Motion::AbortPlannedMoves() {} - void ISR() {} } // namespace motion diff --git a/src/modules/motion.h b/src/modules/motion.h index 25cabe5..6af6313 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -1,32 +1,18 @@ #pragma once -#include -#include "../config/config.h" -#include "../hal/tmc2130.h" #include "../pins.h" +#include "pulse_gen.h" namespace modules { /// @@TODO /// Logic of motor handling /// Ideally enable stepping of motors under ISR (all timers have higher priority than serial) - -/// input: -/// motor, direction, speed (step rate), may be acceleration if necessary (not sure) -/// enable/disable motor current -/// stealth/normal - -/// Motors: -/// idler -/// selector -/// pulley - -/// Operations: -/// setDir(); -/// rotate(speed) -/// rotate(speed, angle/steps) -/// home? namespace motion { +using namespace hal::tmc2130; +using pulse_gen::pos_t; +using pulse_gen::steps_t; + /// Main axis enumeration enum Axis : uint8_t { Pulley, @@ -37,28 +23,29 @@ enum Axis : uint8_t { static constexpr uint8_t NUM_AXIS = _Axis_Last + 1; -/// Static axis configuration struct AxisParams { char name; - hal::tmc2130::MotorParams params; - hal::tmc2130::MotorCurrents currents; - float scale; - float accel; + MotorParams params; + MotorCurrents currents; + MotorMode mode; + steps_t jerk; + steps_t accel; }; +/// Return the default motor mode for an Axis +static constexpr MotorMode DefaultMotorMode(const config::AxisConfig &axis) { + return axis.stealth ? MotorMode::Stealth : MotorMode::Normal; +} + +/// Static axis configuration static constexpr AxisParams axisParams[NUM_AXIS] = { // Idler { .name = 'I', - .params = { - .idx = Idler, - .dirOn = config::idler.dirOn, - .csPin = IDLER_CS_PIN, - .stepPin = IDLER_STEP_PIN, - .sgPin = IDLER_SG_PIN, - .uSteps = config::idler.uSteps }, + .params = { .idx = Idler, .dirOn = config::idler.dirOn, .csPin = IDLER_CS_PIN, .stepPin = IDLER_STEP_PIN, .sgPin = IDLER_SG_PIN, .uSteps = config::idler.uSteps }, .currents = { .vSense = config::idler.vSense, .iRun = config::idler.iRun, .iHold = config::idler.iHold }, - .scale = config::idler.scale, + .mode = DefaultMotorMode(config::idler), + .jerk = config::idler.jerk, .accel = config::idler.accel, }, // Pulley @@ -66,7 +53,8 @@ static constexpr AxisParams axisParams[NUM_AXIS] = { .name = 'P', .params = { .idx = Pulley, .dirOn = config::pulley.dirOn, .csPin = PULLEY_CS_PIN, .stepPin = PULLEY_STEP_PIN, .sgPin = PULLEY_SG_PIN, .uSteps = config::pulley.uSteps }, .currents = { .vSense = config::pulley.vSense, .iRun = config::pulley.iRun, .iHold = config::pulley.iHold }, - .scale = config::pulley.scale, + .mode = DefaultMotorMode(config::pulley), + .jerk = config::pulley.jerk, .accel = config::pulley.accel, }, // Selector @@ -74,22 +62,19 @@ static constexpr AxisParams axisParams[NUM_AXIS] = { .name = 'S', .params = { .idx = Selector, .dirOn = config::selector.dirOn, .csPin = SELECTOR_CS_PIN, .stepPin = SELECTOR_STEP_PIN, .sgPin = SELECTOR_SG_PIN, .uSteps = config::selector.uSteps }, .currents = { .vSense = config::selector.vSense, .iRun = config::selector.iRun, .iHold = config::selector.iHold }, - .scale = config::selector.scale, + .mode = DefaultMotorMode(config::selector), + .jerk = config::selector.jerk, .accel = config::selector.accel, }, }; -enum IdlerMode : uint8_t { - Engage, - Disengage -}; - class Motion { public: inline constexpr Motion() = default; - /// Init axis driver - @@TODO this should be probably hidden somewhere deeper ... something should manage the axes and their state - /// especially when the TMC may get randomly reset (deinited) + /// Init axis driver - @@TODO this should be probably hidden + /// somewhere deeper ... something should manage the axes and their + /// state especially when the TMC may get randomly reset (deinited) void InitAxis(Axis axis); /// Disable axis motor @@ -101,27 +86,36 @@ public: /// clear stall guard flag reported on an axis void ClearStallGuardFlag(Axis axis); - /// Enqueue move of a specific motor/axis into planner buffer - /// @param pulley, idler, selector - target coords - void PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed); + /// Enqueue a single axis move in steps starting and ending at zero speed with maximum feedrate + /// @param axis axis affected + /// @param pos target position + /// @param feedrate maximum feedrate + void PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate); /// Enqueue a single axis move in steps starting and ending at zero speed with maximum feedrate /// @param axis axis affected - /// @param delta number of steps in either direction - /// @param feedrate maximum feedrate/speed after acceleration - void PlanMove(Axis axis, int16_t delta, uint16_t feedrate); + /// @param delta relative to current position + /// @param feedrate maximum feedrate + void PlanMove(Axis axis, pos_t delta, steps_t feedrate) { + PlanMoveTo(axis, CurrentPos(axis) + delta, feedrate); + } /// @returns current position of an axis /// @param axis axis affected - uint16_t CurrentPos(Axis axis) const; + pos_t CurrentPos(Axis axis) const; + + /// Set acceleration for the selected axis + /// @param axis axis affected + /// @param accel acceleration + void SetAcceleration(Axis axis, steps_t accel) { + axisData[axis].ctrl.SetAcceleration(accel); + } /// Enqueue performing of homing of an axis - /// @@TODO void Home(Axis axis, bool direction); /// Set mode of TMC/motors operation - /// Common for all axes/motors - void SetMode(hal::tmc2130::MotorMode mode); + void SetMode(Axis axis, MotorMode mode); /// State machine doing all the planning and stepping preparation based on received commands void Step(); @@ -132,9 +126,30 @@ public: /// stop whatever moves are being done void AbortPlannedMoves(); - /// probably higher-level operations knowing the semantic meaning of axes - private: + struct AxisData { + TMC2130 drv; ///< Motor driver + pulse_gen::PulseGen ctrl; ///< Motor controller + }; + + /// Dynamic axis data + AxisData axisData[NUM_AXIS] = { + // Idler + { + .drv = { axisParams[Idler].params, axisParams[Idler].currents, axisParams[Idler].mode }, + .ctrl = { axisParams[Idler].jerk, axisParams[Idler].accel }, + }, + // Pulley + { + .drv = { axisParams[Pulley].params, axisParams[Pulley].currents, axisParams[Pulley].mode }, + .ctrl = { axisParams[Pulley].jerk, axisParams[Pulley].accel }, + }, + // Selector + { + .drv = { axisParams[Selector].params, axisParams[Selector].currents, axisParams[Selector].mode }, + .ctrl = { axisParams[Selector].jerk, axisParams[Selector].accel }, + } + }; }; /// ISR stepping routine diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index 03d02c3..f65268a 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -1,5 +1,4 @@ #pragma once -#include #include "speed_table.h" #include "../hal/tmc2130.h" #include "../hal/circular_buffer.h" diff --git a/tests/unit/logic/cut_filament/CMakeLists.txt b/tests/unit/logic/cut_filament/CMakeLists.txt index 07d0990..d0e21e7 100644 --- a/tests/unit/logic/cut_filament/CMakeLists.txt +++ b/tests/unit/logic/cut_filament/CMakeLists.txt @@ -15,11 +15,13 @@ add_executable( ../../../../src/modules/permanent_storage.cpp ../../../../src/modules/selector.cpp ../../../../src/modules/user_input.cpp + ../../../../src/modules/pulse_gen.cpp ../../modules/stubs/stub_adc.cpp ../../modules/stubs/stub_eeprom.cpp ../../modules/stubs/stub_gpio.cpp ../../modules/stubs/stub_shr16.cpp ../../modules/stubs/stub_timebase.cpp + ../../modules/stubs/stub_tmc2130.cpp ../stubs/main_loop_stub.cpp ../stubs/stub_motion.cpp test_cut_filament.cpp diff --git a/tests/unit/logic/eject_filament/CMakeLists.txt b/tests/unit/logic/eject_filament/CMakeLists.txt index 40f277d..f658cca 100644 --- a/tests/unit/logic/eject_filament/CMakeLists.txt +++ b/tests/unit/logic/eject_filament/CMakeLists.txt @@ -15,11 +15,14 @@ add_executable( ../../../../src/modules/permanent_storage.cpp ../../../../src/modules/selector.cpp ../../../../src/modules/user_input.cpp + ../../../../src/modules/pulse_gen.cpp ../../modules/stubs/stub_adc.cpp ../../modules/stubs/stub_eeprom.cpp ../../modules/stubs/stub_gpio.cpp ../../modules/stubs/stub_shr16.cpp ../../modules/stubs/stub_timebase.cpp + ../../modules/stubs/stub_gpio.cpp + ../../modules/stubs/stub_tmc2130.cpp ../stubs/main_loop_stub.cpp ../stubs/stub_motion.cpp test_eject_filament.cpp diff --git a/tests/unit/logic/feed_to_bondtech/CMakeLists.txt b/tests/unit/logic/feed_to_bondtech/CMakeLists.txt index d727aeb..8a78a59 100644 --- a/tests/unit/logic/feed_to_bondtech/CMakeLists.txt +++ b/tests/unit/logic/feed_to_bondtech/CMakeLists.txt @@ -12,11 +12,13 @@ add_executable( ../../../../src/modules/permanent_storage.cpp ../../../../src/modules/selector.cpp ../../../../src/modules/user_input.cpp + ../../../../src/modules/pulse_gen.cpp ../../modules/stubs/stub_adc.cpp ../../modules/stubs/stub_eeprom.cpp ../../modules/stubs/stub_gpio.cpp ../../modules/stubs/stub_shr16.cpp ../../modules/stubs/stub_timebase.cpp + ../../modules/stubs/stub_tmc2130.cpp ../stubs/main_loop_stub.cpp ../stubs/stub_motion.cpp test_feed_to_bondtech.cpp diff --git a/tests/unit/logic/feed_to_finda/CMakeLists.txt b/tests/unit/logic/feed_to_finda/CMakeLists.txt index 1965ccd..a036a9c 100644 --- a/tests/unit/logic/feed_to_finda/CMakeLists.txt +++ b/tests/unit/logic/feed_to_finda/CMakeLists.txt @@ -12,11 +12,13 @@ add_executable( ../../../../src/modules/permanent_storage.cpp ../../../../src/modules/selector.cpp ../../../../src/modules/user_input.cpp + ../../../../src/modules/pulse_gen.cpp ../../modules/stubs/stub_adc.cpp ../../modules/stubs/stub_eeprom.cpp ../../modules/stubs/stub_gpio.cpp ../../modules/stubs/stub_shr16.cpp ../../modules/stubs/stub_timebase.cpp + ../../modules/stubs/stub_tmc2130.cpp ../stubs/main_loop_stub.cpp ../stubs/stub_motion.cpp test_feed_to_finda.cpp diff --git a/tests/unit/logic/load_filament/CMakeLists.txt b/tests/unit/logic/load_filament/CMakeLists.txt index 22619d3..562e58f 100644 --- a/tests/unit/logic/load_filament/CMakeLists.txt +++ b/tests/unit/logic/load_filament/CMakeLists.txt @@ -14,11 +14,13 @@ add_executable( ../../../../src/modules/permanent_storage.cpp ../../../../src/modules/selector.cpp ../../../../src/modules/user_input.cpp + ../../../../src/modules/pulse_gen.cpp ../../modules/stubs/stub_adc.cpp ../../modules/stubs/stub_eeprom.cpp ../../modules/stubs/stub_gpio.cpp ../../modules/stubs/stub_shr16.cpp ../../modules/stubs/stub_timebase.cpp + ../../modules/stubs/stub_tmc2130.cpp ../stubs/main_loop_stub.cpp ../stubs/stub_motion.cpp test_load_filament.cpp diff --git a/tests/unit/logic/stubs/stub_motion.cpp b/tests/unit/logic/stubs/stub_motion.cpp index c053c2b..bad3a51 100644 --- a/tests/unit/logic/stubs/stub_motion.cpp +++ b/tests/unit/logic/stubs/stub_motion.cpp @@ -30,18 +30,11 @@ void Motion::ClearStallGuardFlag(Axis axis) { axes[axis].stallGuard = false; } -void Motion::PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed) { - axes[Pulley].targetPos = axes[Pulley].pos + pulley; - axes[Idler].targetPos = axes[Idler].pos + idler; - axes[Selector].targetPos = axes[Selector].pos + selector; - // speeds and feedrates are not simulated yet +void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) { + axes[axis].targetPos = pos; } -void Motion::PlanMove(Axis axis, int16_t delta, uint16_t feedrate) { - axes[axis].targetPos = axes[axis].pos + delta; -} - -uint16_t Motion::CurrentPos(Axis axis) const { +pos_t Motion::CurrentPos(Axis axis) const { return axes[axis].pos; } @@ -49,7 +42,7 @@ void Motion::Home(Axis axis, bool direction) { axes[Pulley].homed = true; } -void Motion::SetMode(hal::tmc2130::MotorMode mode) { +void Motion::SetMode(Axis axis, hal::tmc2130::MotorMode mode) { } void Motion::Step() { diff --git a/tests/unit/logic/stubs/stub_motion.h b/tests/unit/logic/stubs/stub_motion.h index bc1a814..60d44eb 100644 --- a/tests/unit/logic/stubs/stub_motion.h +++ b/tests/unit/logic/stubs/stub_motion.h @@ -5,8 +5,8 @@ namespace modules { namespace motion { struct AxisSim { - int32_t pos; - int32_t targetPos; + pos_t pos; + pos_t targetPos; bool enabled; bool homed; bool stallGuard; diff --git a/tests/unit/logic/tool_change/CMakeLists.txt b/tests/unit/logic/tool_change/CMakeLists.txt index 00c6c5e..c01d6ec 100644 --- a/tests/unit/logic/tool_change/CMakeLists.txt +++ b/tests/unit/logic/tool_change/CMakeLists.txt @@ -17,11 +17,13 @@ add_executable( ../../../../src/modules/permanent_storage.cpp ../../../../src/modules/selector.cpp ../../../../src/modules/user_input.cpp + ../../../../src/modules/pulse_gen.cpp ../../modules/stubs/stub_adc.cpp ../../modules/stubs/stub_eeprom.cpp ../../modules/stubs/stub_gpio.cpp ../../modules/stubs/stub_shr16.cpp ../../modules/stubs/stub_timebase.cpp + ../../modules/stubs/stub_tmc2130.cpp ../stubs/main_loop_stub.cpp ../stubs/stub_motion.cpp test_tool_change.cpp diff --git a/tests/unit/logic/unload_filament/CMakeLists.txt b/tests/unit/logic/unload_filament/CMakeLists.txt index 0b4cc25..255a006 100644 --- a/tests/unit/logic/unload_filament/CMakeLists.txt +++ b/tests/unit/logic/unload_filament/CMakeLists.txt @@ -14,11 +14,13 @@ add_executable( ../../../../src/modules/permanent_storage.cpp ../../../../src/modules/selector.cpp ../../../../src/modules/user_input.cpp + ../../../../src/modules/pulse_gen.cpp ../../modules/stubs/stub_adc.cpp ../../modules/stubs/stub_eeprom.cpp ../../modules/stubs/stub_gpio.cpp ../../modules/stubs/stub_shr16.cpp ../../modules/stubs/stub_timebase.cpp + ../../modules/stubs/stub_tmc2130.cpp ../stubs/main_loop_stub.cpp ../stubs/stub_motion.cpp test_unload_filament.cpp diff --git a/tests/unit/logic/unload_to_finda/CMakeLists.txt b/tests/unit/logic/unload_to_finda/CMakeLists.txt index c0a215a..e2a3ceb 100644 --- a/tests/unit/logic/unload_to_finda/CMakeLists.txt +++ b/tests/unit/logic/unload_to_finda/CMakeLists.txt @@ -12,11 +12,13 @@ add_executable( ../../../../src/modules/permanent_storage.cpp ../../../../src/modules/selector.cpp ../../../../src/modules/user_input.cpp + ../../../../src/modules/pulse_gen.cpp ../../modules/stubs/stub_adc.cpp ../../modules/stubs/stub_eeprom.cpp ../../modules/stubs/stub_gpio.cpp ../../modules/stubs/stub_shr16.cpp ../../modules/stubs/stub_timebase.cpp + ../../modules/stubs/stub_tmc2130.cpp ../stubs/main_loop_stub.cpp ../stubs/stub_motion.cpp test_unload_to_finda.cpp diff --git a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp index 194c685..c0b2deb 100644 --- a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -8,6 +8,7 @@ using namespace modules::pulse_gen; using hal::tmc2130::MotorParams; TEST_CASE("pulse_gen::basic", "[pulse_gen]") { + // TODO: woefully incomplete MotorParams mp = { .idx = 0, .dirOn = config::idler.dirOn, diff --git a/tests/unit/modules/stubs/stub_tmc2130.cpp b/tests/unit/modules/stubs/stub_tmc2130.cpp new file mode 100644 index 0000000..7ab2e00 --- /dev/null +++ b/tests/unit/modules/stubs/stub_tmc2130.cpp @@ -0,0 +1,12 @@ +#include "tmc2130.h" + +namespace hal { +namespace tmc2130 { + +TMC2130::TMC2130(const MotorParams ¶ms, + const MotorCurrents ¤ts, + MotorMode mode) { +} + +} // namespace tmc2130 +} // namespace hal From 16cf02726ba3dbcfab29b178ab555fdf2a43d055 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 6 Jul 2021 15:54:27 +0200 Subject: [PATCH 14/43] test_speed_table: improve static assertion Test against the resulting timer speed, not against the CPU clock --- tests/unit/modules/speed_table/test_speed_table.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tests/unit/modules/speed_table/test_speed_table.cpp b/tests/unit/modules/speed_table/test_speed_table.cpp index 21c8801..1531849 100644 --- a/tests/unit/modules/speed_table/test_speed_table.cpp +++ b/tests/unit/modules/speed_table/test_speed_table.cpp @@ -5,8 +5,9 @@ using Catch::Matchers::Equals; using namespace modules::speed_table; -// The following reference values are calculated for 16MHz F_CPU -static_assert(F_CPU == 16000000); +// The following reference values are calculated for 2MHz timer +static_assert(F_CPU / cpuFrequencyDivider == 2000000, + "speed tables not compatible for the requested frequency"); static const st_timer_t reference[][3] = { { 1, 62500, 1 }, From bd8ae6221128d4faeeb68a90062c9d1b60e73b4a Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 6 Jul 2021 16:09:05 +0200 Subject: [PATCH 15/43] PulseGen: cleanup AxisData initialization --- src/modules/motion.h | 26 +++++++++++--------------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/src/modules/motion.h b/src/modules/motion.h index 6af6313..95e1a07 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -132,23 +132,19 @@ private: pulse_gen::PulseGen ctrl; ///< Motor controller }; + /// Helper to initialize AxisData members + static AxisData DataForAxis(Axis axis) { + return { + .drv = { axisParams[axis].params, axisParams[axis].currents, axisParams[axis].mode }, + .ctrl = { axisParams[axis].jerk, axisParams[axis].accel }, + }; + } + /// Dynamic axis data AxisData axisData[NUM_AXIS] = { - // Idler - { - .drv = { axisParams[Idler].params, axisParams[Idler].currents, axisParams[Idler].mode }, - .ctrl = { axisParams[Idler].jerk, axisParams[Idler].accel }, - }, - // Pulley - { - .drv = { axisParams[Pulley].params, axisParams[Pulley].currents, axisParams[Pulley].mode }, - .ctrl = { axisParams[Pulley].jerk, axisParams[Pulley].accel }, - }, - // Selector - { - .drv = { axisParams[Selector].params, axisParams[Selector].currents, axisParams[Selector].mode }, - .ctrl = { axisParams[Selector].jerk, axisParams[Selector].accel }, - } + DataForAxis(Idler), + DataForAxis(Pulley), + DataForAxis(Selector), }; }; From 94f11642e091da55dddb9674df79a0785f3a8325 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 6 Jul 2021 16:10:16 +0200 Subject: [PATCH 16/43] PulseGen: perform an extra queue check in Move() Since scheduling a move on a block which is being executed will jolt the motors, be extra-safe and perform an extra lower-level check before committing even if the caller is responsible. Return the status, which can be useful to build a simple busy loop. --- src/modules/pulse_gen.cpp | 14 +++++++++----- src/modules/pulse_gen.h | 5 +++-- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index 2cea500..6fe2ba1 100644 --- a/src/modules/pulse_gen.cpp +++ b/src/modules/pulse_gen.cpp @@ -85,15 +85,18 @@ void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t e block->final_rate = final_rate; } -void PulseGen::Move(pos_t target, steps_t feed_rate) { +bool PulseGen::Move(pos_t target, steps_t feed_rate) { // Prepare to set up new block + if (block_index.full()) + return false; block_t *block = &block_buffer[block_index.back()]; - block->steps = abs(target - position); - // Bail if this is a zero-length block - if (block->steps <= config::dropSegments) - return; + block->steps = abs(target - position); + if (block->steps <= config::dropSegments) { + // behave as-if the block as been scheduled + return true; + } // Direction and speed for this block block->direction = (target >= position); @@ -109,6 +112,7 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) { // Move forward block_index.push(); position = target; + return true; } void PulseGen::AbortPlannedMoves() { diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index f65268a..219acbd 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -28,8 +28,9 @@ public: /// Set acceleration for the axis void SetAcceleration(steps_t accel) { acceleration = accel; } - /// Plan a single move (can only be executed when !Full()) - void Move(pos_t x, steps_t feed_rate); + /// Plan a single move (can only be executed when not Full()) + /// @returns True if the move has been planned + bool Move(pos_t x, steps_t feed_rate); /// stop whatever moves are being done void AbortPlannedMoves(); From 2875e61484fa1e5913c26ead2643cb809c51f607 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 6 Jul 2021 16:12:44 +0200 Subject: [PATCH 17/43] Motion: allow to query the buffer per-axis We might want to schedule new moves while a single motor is moving. Allow to do that by introducing per-axis query functions. The main QueueEmpty() and Full() still function as before: - Call QueueEmpty() to wait for all moves to finish. - Use !Full() to know that a Plan() move will never be discarded. --- src/modules/motion.h | 24 +++++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/src/modules/motion.h b/src/modules/motion.h index 95e1a07..ae56536 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -86,13 +86,15 @@ public: /// clear stall guard flag reported on an axis void ClearStallGuardFlag(Axis axis); - /// Enqueue a single axis move in steps starting and ending at zero speed with maximum feedrate + /// Enqueue a single axis move in steps starting and ending at zero speed with maximum + /// feedrate. Moves can only be enqueued if the axis is not Full(). /// @param axis axis affected /// @param pos target position /// @param feedrate maximum feedrate void PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate); - /// Enqueue a single axis move in steps starting and ending at zero speed with maximum feedrate + /// Enqueue a single axis move in steps starting and ending at zero speed with maximum + /// feedrate. Moves can only be enqueued if the axis is not Full(). /// @param axis axis affected /// @param delta relative to current position /// @param feedrate maximum feedrate @@ -120,9 +122,25 @@ public: /// State machine doing all the planning and stepping preparation based on received commands void Step(); - /// @returns true if all planned moves have been finished + /// @returns true if all planned moves have been finished for all axes bool QueueEmpty() const; + /// @returns true if all planned moves have been finished for one axis + /// @param axis requested + bool QueueEmpty(Axis axis) const { return axisData[axis].ctrl.QueueEmpty(); } + + /// @returns false if new moves can still be planned for _any_ axis + bool Full() const { + for (uint8_t i = 0; i != NUM_AXIS; ++i) + if (axisData[i].ctrl.Full()) + return true; + return false; + } + + /// @returns false if new moves can still be planned for one axis + /// @param axis axis requested + bool Full(Axis axis) const { return axisData[axis].ctrl.Full(); } + /// stop whatever moves are being done void AbortPlannedMoves(); From be30314634dd8591aa6224d255eba05cd12f6087 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 6 Jul 2021 16:44:20 +0200 Subject: [PATCH 18/43] Motion: initialize axes in the same order as the enumeration --- src/modules/motion.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/motion.h b/src/modules/motion.h index ae56536..0c7f0e5 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -39,15 +39,6 @@ static constexpr MotorMode DefaultMotorMode(const config::AxisConfig &axis) { /// Static axis configuration static constexpr AxisParams axisParams[NUM_AXIS] = { - // Idler - { - .name = 'I', - .params = { .idx = Idler, .dirOn = config::idler.dirOn, .csPin = IDLER_CS_PIN, .stepPin = IDLER_STEP_PIN, .sgPin = IDLER_SG_PIN, .uSteps = config::idler.uSteps }, - .currents = { .vSense = config::idler.vSense, .iRun = config::idler.iRun, .iHold = config::idler.iHold }, - .mode = DefaultMotorMode(config::idler), - .jerk = config::idler.jerk, - .accel = config::idler.accel, - }, // Pulley { .name = 'P', @@ -66,6 +57,15 @@ static constexpr AxisParams axisParams[NUM_AXIS] = { .jerk = config::selector.jerk, .accel = config::selector.accel, }, + // Idler + { + .name = 'I', + .params = { .idx = Idler, .dirOn = config::idler.dirOn, .csPin = IDLER_CS_PIN, .stepPin = IDLER_STEP_PIN, .sgPin = IDLER_SG_PIN, .uSteps = config::idler.uSteps }, + .currents = { .vSense = config::idler.vSense, .iRun = config::idler.iRun, .iHold = config::idler.iHold }, + .mode = DefaultMotorMode(config::idler), + .jerk = config::idler.jerk, + .accel = config::idler.accel, + }, }; class Motion { @@ -160,9 +160,9 @@ private: /// Dynamic axis data AxisData axisData[NUM_AXIS] = { - DataForAxis(Idler), DataForAxis(Pulley), DataForAxis(Selector), + DataForAxis(Idler), }; }; From 1ff9b81630a4670488c204bb7584b21235fd346c Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Wed, 7 Jul 2021 16:35:50 +0200 Subject: [PATCH 19/43] Rename PulseGen::Move to PlanMoveTo Make Motion and PulseGen intentionally very similar. --- src/modules/pulse_gen.cpp | 2 +- src/modules/pulse_gen.h | 9 ++++++--- tests/unit/modules/pulse_gen/test_pulse_gen.cpp | 2 +- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index 6fe2ba1..a06400d 100644 --- a/src/modules/pulse_gen.cpp +++ b/src/modules/pulse_gen.cpp @@ -85,7 +85,7 @@ void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t e block->final_rate = final_rate; } -bool PulseGen::Move(pos_t target, steps_t feed_rate) { +bool PulseGen::PlanMoveTo(pos_t target, steps_t feed_rate) { // Prepare to set up new block if (block_index.full()) return false; diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index 219acbd..7c6edb5 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -28,9 +28,12 @@ public: /// Set acceleration for the axis void SetAcceleration(steps_t accel) { acceleration = accel; } - /// Plan a single move (can only be executed when not Full()) - /// @returns True if the move has been planned - bool Move(pos_t x, steps_t feed_rate); + /// Enqueue a single move in steps starting and ending at zero speed with maximum + /// feedrate. Moves can only be enqueued if the axis is not Full(). + /// @param pos target position + /// @param feedrate maximum feedrate + /// @returns true if the move has been enqueued + bool PlanMoveTo(pos_t pos, steps_t feedrate); /// stop whatever moves are being done void AbortPlannedMoves(); diff --git a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp index c0b2deb..c6cba8d 100644 --- a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -20,7 +20,7 @@ TEST_CASE("pulse_gen::basic", "[pulse_gen]") { for (int accel = 100; accel <= 5000; accel *= 2) { PulseGen pg(10, accel); - pg.Move(100000, 10000); + pg.PlanMoveTo(100000, 10000); unsigned long ts = 0; st_timer_t next; From 32e09afd4c4875ff337bfde558c5fca4eaa05c2d Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 11 Jul 2021 20:23:21 +0200 Subject: [PATCH 20/43] Fix return value of gpio::ReadPin The cast to Level is incorrect, since the expression returns either 0 or a positive value if the pin is set. The value is directly assigned to the underlying uint8_t, meaning that Level::high won't match for levels greater than 0. Return a boolean and cast that to Level instead. --- src/hal/gpio.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hal/gpio.h b/src/hal/gpio.h index e29546d..3d45e88 100644 --- a/src/hal/gpio.h +++ b/src/hal/gpio.h @@ -57,7 +57,7 @@ __attribute__((always_inline)) inline void WritePin(const GPIO_pin portPin, Leve } __attribute__((always_inline)) inline Level ReadPin(const GPIO_pin portPin) { - return (Level)(portPin.port->PINx & (1 << portPin.pin)); + return (Level)((portPin.port->PINx & (1 << portPin.pin)) != 0); } __attribute__((always_inline)) inline void TogglePin(const GPIO_pin portPin) { From b01a819644b5c5483d7d0be230b765a784e4a363 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 11 Jul 2021 20:25:52 +0200 Subject: [PATCH 21/43] Make Read/Write/Toggle pin functional for testing Make ReadPin return the last value set by WritePin for proper testing. Add a slow-path to TogglePin that goes through a read-write cycle. This is useful both for testing and for platforms that don't have an efficient toggle like AVR. --- src/hal/gpio.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/hal/gpio.h b/src/hal/gpio.h index 3d45e88..df8da66 100644 --- a/src/hal/gpio.h +++ b/src/hal/gpio.h @@ -57,11 +57,21 @@ __attribute__((always_inline)) inline void WritePin(const GPIO_pin portPin, Leve } __attribute__((always_inline)) inline Level ReadPin(const GPIO_pin portPin) { +#ifdef __AVR__ return (Level)((portPin.port->PINx & (1 << portPin.pin)) != 0); +#else + // Return the value modified by WritePin + return (Level)((portPin.port->PORTx & (1 << portPin.pin)) != 0); +#endif } __attribute__((always_inline)) inline void TogglePin(const GPIO_pin portPin) { +#ifdef __AVR__ + // Optimized path for AVR, resulting in a pin toggle portPin.port->PINx |= (1 << portPin.pin); +#else + WritePin(portPin, (Level)(ReadPin(portPin) != Level::high)); +#endif } __attribute__((always_inline)) inline void Init(const GPIO_pin portPin, GPIO_InitTypeDef GPIO_Init) { From 5250cfd4feadc07b0d640507b31aea5ff746a4f1 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 11 Jul 2021 20:33:59 +0200 Subject: [PATCH 22/43] Implement the SHR16::SetTMCDir stub for testing --- tests/unit/modules/stubs/stub_shr16.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/tests/unit/modules/stubs/stub_shr16.cpp b/tests/unit/modules/stubs/stub_shr16.cpp index 45bcf30..97d179f 100644 --- a/tests/unit/modules/stubs/stub_shr16.cpp +++ b/tests/unit/modules/stubs/stub_shr16.cpp @@ -6,20 +6,29 @@ namespace shr16 { SHR16 shr16; uint16_t shr16_v_copy; +uint8_t shr16_tmc_dir; void SHR16::Init() { shr16_v_copy = 0; + shr16_tmc_dir = 0; } void SHR16::SetLED(uint16_t led) { shr16_v_copy = ((led & 0x00ff) << 8) | ((led & 0x0300) >> 2); } + void SHR16::SetTMCEnabled(uint8_t index, bool ena) { // do nothing right now } + void SHR16::SetTMCDir(uint8_t index, bool dir) { - // do nothing right now + // this is using another array for testing convenience + if (dir) + shr16_tmc_dir |= (1 << index); + else + shr16_tmc_dir &= ~(1 << index); } + void SHR16::Write(uint16_t v) { // do nothing right now } From 0c47d8f0d19f73e8c6c920ea6a76095ffc3be92d Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 11 Jul 2021 20:52:27 +0200 Subject: [PATCH 23/43] PulseGen: add comprehensive tests (no acceleration yet) --- .../unit/modules/pulse_gen/test_pulse_gen.cpp | 212 +++++++++++++++++- 1 file changed, 211 insertions(+), 1 deletion(-) diff --git a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp index c6cba8d..f3dd5f0 100644 --- a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -5,10 +5,33 @@ using Catch::Matchers::Equals; using namespace modules::pulse_gen; +using hal::gpio::Level; +using hal::gpio::ReadPin; using hal::tmc2130::MotorParams; +namespace hal { +namespace shr16 { +extern uint8_t shr16_tmc_dir; +} // namespace shr16 +} // namespace hal + +// Conveniently read the direction set into lower-level the shift register +bool getTMCDir(const MotorParams &mp) { + return (hal::shr16::shr16_tmc_dir & (1 << mp.idx)) ^ mp.dirOn; +} + +// Perform Step() until the move is completed, returning the number of steps performed. +// Ensure the move doesn't run forever, making the test fail reliably. +ssize_t stepUntilDone(PulseGen &pg, const MotorParams &mp, size_t maxSteps = 100000) { + for (size_t i = 0; i != maxSteps; ++i) + if (!pg.Step(mp)) + return i; + + // number of steps exceeded + return -1; +} + TEST_CASE("pulse_gen::basic", "[pulse_gen]") { - // TODO: woefully incomplete MotorParams mp = { .idx = 0, .dirOn = config::idler.dirOn, @@ -18,6 +41,193 @@ TEST_CASE("pulse_gen::basic", "[pulse_gen]") { .uSteps = config::idler.uSteps }; + PulseGen pg(10, 100); + + // perform a simple move + REQUIRE(pg.Position() == 0); + pg.PlanMoveTo(10, 1); + REQUIRE(stepUntilDone(pg, mp) == 10); + REQUIRE(pg.Position() == 10); + + // return to zero + pg.PlanMoveTo(0, 1); + REQUIRE(stepUntilDone(pg, mp) == 10); + REQUIRE(pg.Position() == 0); + + // don't move + pg.PlanMoveTo(0, 1); + REQUIRE(stepUntilDone(pg, mp) == 0); + REQUIRE(pg.Position() == 0); +} + +TEST_CASE("pulse_gen::step_dir", "[pulse_gen]") { + MotorParams mp = { + .idx = 0, + .dirOn = config::idler.dirOn, + .csPin = IDLER_CS_PIN, + .stepPin = IDLER_STEP_PIN, + .sgPin = IDLER_SG_PIN, + .uSteps = config::idler.uSteps + }; + + PulseGen pg(10, 100); + + // perform a forward move + REQUIRE(pg.Position() == 0); + pg.PlanMoveTo(10, 10); + REQUIRE(stepUntilDone(pg, mp) == 10); + REQUIRE(pg.Position() == 10); + + // check underlying driver direction + REQUIRE(getTMCDir(mp)); + + // move in reverse + pg.PlanMoveTo(0, 10); + REQUIRE(stepUntilDone(pg, mp) == 10); + REQUIRE(pg.Position() == 0); + + // check underlying driver direction + REQUIRE(!getTMCDir(mp)); + + // forward again (should match initial state) + pg.PlanMoveTo(5, 10); + CHECK(stepUntilDone(pg, mp) == 5); + REQUIRE(getTMCDir(mp)); +} + +TEST_CASE("pulse_gen::step_count", "[pulse_gen]") { + MotorParams mp = { + .idx = 0, + .dirOn = config::idler.dirOn, + .csPin = IDLER_CS_PIN, + .stepPin = IDLER_STEP_PIN, + .sgPin = IDLER_SG_PIN, + .uSteps = config::idler.uSteps + }; + + PulseGen pg(10, 100); + + // step manually, ensuring each step is accounted for + REQUIRE(pg.Position() == 0); + pg.PlanMoveTo(10, 10); + bool st = ReadPin(IDLER_STEP_PIN) == Level::high; + for (size_t i = 0; i != 10; ++i) { + REQUIRE(pg.Step(mp) > 0); + bool newSt = ReadPin(IDLER_STEP_PIN) == Level::high; + + // Assuming DEDGE each step should toggle the pin + REQUIRE(newSt != st); + st = newSt; + } + + // there should be one extra timer event to ensure smooth + // transition between multiple blocks + REQUIRE(pg.Step(mp) == 0); + + // no pin or position change + REQUIRE(st == (ReadPin(IDLER_STEP_PIN) == Level::high)); + REQUIRE(pg.Position() == 10); +} + +TEST_CASE("pulse_gen::queue_size", "[pulse_gen]") { + PulseGen pg(10, 100); + + // queue should start empty + REQUIRE(pg.QueueEmpty()); + + // queue the first move + CHECK(pg.PlanMoveTo(10, 1)); + REQUIRE(!pg.QueueEmpty()); + + // queue a second move to fill the queue + CHECK(pg.PlanMoveTo(15, 1)); + REQUIRE(pg.Full()); + + // further enqueuing should fail + REQUIRE(!pg.PlanMoveTo(20, 1)); +} + +TEST_CASE("pulse_gen::queue_dropsegments", "[pulse_gen]") { + PulseGen pg(10, 100); + + // queue should start empty + REQUIRE(pg.QueueEmpty()); + REQUIRE(pg.Position() == 0); + + // ensure we can enqueue a zero-lenght move successfully + REQUIRE(pg.PlanMoveTo(0, 1)); + + // however the move shouldn't result in a block entry + REQUIRE(pg.QueueEmpty()); +} + +TEST_CASE("pulse_gen::queue_step", "[pulse_gen]") { + MotorParams mp = { + .idx = 0, + .dirOn = config::idler.dirOn, + .csPin = IDLER_CS_PIN, + .stepPin = IDLER_STEP_PIN, + .sgPin = IDLER_SG_PIN, + .uSteps = config::idler.uSteps + }; + + PulseGen pg(10, 100); + + // queue should start empty + REQUIRE(pg.QueueEmpty()); + + // enqueue two moves + REQUIRE(pg.PlanMoveTo(15, 1)); + REQUIRE(pg.PlanMoveTo(5, 1)); + + // check for a total lenght of 25 steps (15+(15-5)) + REQUIRE(stepUntilDone(pg, mp) == 25); + + // check final position + REQUIRE(pg.Position() == 5); +} + +TEST_CASE("pulse_gen::queue_abort", "[pulse_gen]") { + MotorParams mp = { + .idx = 0, + .dirOn = config::idler.dirOn, + .csPin = IDLER_CS_PIN, + .stepPin = IDLER_STEP_PIN, + .sgPin = IDLER_SG_PIN, + .uSteps = config::idler.uSteps + }; + + PulseGen pg(10, 100); + + // queue should start empty + REQUIRE(pg.QueueEmpty()); + + // enqueue a move and step halfway through + REQUIRE(pg.PlanMoveTo(10, 1)); + REQUIRE(stepUntilDone(pg, mp, 5) == -1); + + // abort the queue + pg.AbortPlannedMoves(); + REQUIRE(pg.QueueEmpty()); + + // step shouldn't perform extra moves and return a zero timer + bool st = ReadPin(IDLER_STEP_PIN) == Level::high; + REQUIRE(pg.Step(mp) == 0); + REQUIRE(st == (ReadPin(IDLER_STEP_PIN) == Level::high)); + REQUIRE(pg.Position() == 5); +} + +TEST_CASE("pulse_gen::accel_ramp", "[pulse_gen]") { + MotorParams mp = { + .idx = 0, + .dirOn = config::idler.dirOn, + .csPin = IDLER_CS_PIN, + .stepPin = IDLER_STEP_PIN, + .sgPin = IDLER_SG_PIN, + .uSteps = config::idler.uSteps + }; + + // TODO: output ramps still to be checked for (int accel = 100; accel <= 5000; accel *= 2) { PulseGen pg(10, accel); pg.PlanMoveTo(100000, 10000); From 066aab7adce4d542e88aa43cce13b6ab5226200f Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 11 Jul 2021 21:17:08 +0200 Subject: [PATCH 24/43] Move cpuFrequencyDivider to config::stepTimerFrequencyDivider --- src/config/config.h | 3 +++ src/modules/pulse_gen.cpp | 2 +- src/modules/speed_table.h | 3 +-- tests/unit/modules/speed_table/test_speed_table.cpp | 2 +- 4 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/config/config.h b/src/config/config.h index 73aa8ae..0239c4d 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -44,6 +44,9 @@ static constexpr uint16_t minStepRate = 120; /// Size for the motion planner block buffer size static constexpr uint8_t blockBufferSize = 2; +/// Step timer frequency divider (F = F_CPU / divider) +static constexpr uint8_t stepTimerFrequencyDivider = 8; + /// Idler configuration static constexpr AxisConfig idler = { .dirOn = true, diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index a06400d..0c09573 100644 --- a/src/modules/pulse_gen.cpp +++ b/src/modules/pulse_gen.cpp @@ -104,7 +104,7 @@ bool PulseGen::PlanMoveTo(pos_t target, steps_t feed_rate) { // Acceleration of the segment, in steps/sec^2 block->acceleration = acceleration; - block->acceleration_rate = block->acceleration * (rate_t)((float)F_CPU / (F_CPU / speed_table::cpuFrequencyDivider)); + block->acceleration_rate = block->acceleration * (rate_t)((float)F_CPU / (F_CPU / config::stepTimerFrequencyDivider)); // Perform the trapezoid calculations CalculateTrapezoid(block, max_jerk, max_jerk); diff --git a/src/modules/speed_table.h b/src/modules/speed_table.h index 6c88cf1..028baaa 100644 --- a/src/modules/speed_table.h +++ b/src/modules/speed_table.h @@ -12,8 +12,7 @@ namespace speed_table { typedef uint16_t st_timer_t; /// CPU timer frequency divider required for the speed tables -static constexpr uint8_t cpuFrequencyDivider = 8; -static_assert(F_CPU / cpuFrequencyDivider == 2000000, +static_assert(F_CPU / config::stepTimerFrequencyDivider == 2000000, "speed tables not compatible for the requested frequency"); /// Lookup table for rates equal or higher than 8*256 diff --git a/tests/unit/modules/speed_table/test_speed_table.cpp b/tests/unit/modules/speed_table/test_speed_table.cpp index 1531849..f0223e8 100644 --- a/tests/unit/modules/speed_table/test_speed_table.cpp +++ b/tests/unit/modules/speed_table/test_speed_table.cpp @@ -6,7 +6,7 @@ using Catch::Matchers::Equals; using namespace modules::speed_table; // The following reference values are calculated for 2MHz timer -static_assert(F_CPU / cpuFrequencyDivider == 2000000, +static_assert(F_CPU / config::stepTimerFrequencyDivider == 2000000, "speed tables not compatible for the requested frequency"); static const st_timer_t reference[][3] = { From 7337e977657b380e4aefacb85e1657fd4584b050 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 11 Jul 2021 21:38:01 +0200 Subject: [PATCH 25/43] Rename Motion::DisableAxis to Disable Implement Motion::SetEnabled (for symmetry with TMC2130::SetEnabled). Rename DisableAxis to Disable and use the new SetEnabled. This makes the member names more consistent. --- src/logic/eject_filament.cpp | 2 +- src/logic/load_filament.cpp | 2 +- src/logic/unload_filament.cpp | 2 +- src/modules/idler.cpp | 2 +- src/modules/motion.cpp | 5 ++++- src/modules/motion.h | 6 +++++- src/modules/selector.cpp | 2 +- tests/unit/logic/stubs/stub_motion.cpp | 4 ++-- 8 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/logic/eject_filament.cpp b/src/logic/eject_filament.cpp index 67e64d4..82e7dbd 100644 --- a/src/logic/eject_filament.cpp +++ b/src/logic/eject_filament.cpp @@ -61,7 +61,7 @@ bool EjectFilament::Step() { break; case ProgressCode::DisengagingIdler: if (!mi::idler.Engaged()) { // idler disengaged - mm::motion.DisableAxis(mm::Pulley); + mm::motion.Disable(mm::Pulley); state = ProgressCode::OK; } break; diff --git a/src/logic/load_filament.cpp b/src/logic/load_filament.cpp index c5b3331..121bde9 100644 --- a/src/logic/load_filament.cpp +++ b/src/logic/load_filament.cpp @@ -59,7 +59,7 @@ bool LoadFilament::Step() { case FeedToBondtech::Failed: case FeedToBondtech::OK: - mm::motion.DisableAxis(mm::Pulley); + mm::motion.Disable(mm::Pulley); mi::idler.Disengage(); state = ProgressCode::DisengagingIdler; } diff --git a/src/logic/unload_filament.cpp b/src/logic/unload_filament.cpp index 7be6110..3e32394 100644 --- a/src/logic/unload_filament.cpp +++ b/src/logic/unload_filament.cpp @@ -63,7 +63,7 @@ bool UnloadFilament::Step() { case ProgressCode::FinishingMoves: if (mm::motion.QueueEmpty()) { state = ProgressCode::OK; - mm::motion.DisableAxis(mm::Pulley); + mm::motion.Disable(mm::Pulley); mg::globals.SetFilamentLoaded(false); // filament unloaded ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on); } diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp index a23e1fa..6be1be4 100644 --- a/src/modules/idler.cpp +++ b/src/modules/idler.cpp @@ -66,7 +66,7 @@ bool Idler::Step() { currentSlot = plannedSlot; if (!Engaged()) // turn off power into the Idler motor when disengaged (no force necessary) - mm::motion.DisableAxis(mm::Idler); + mm::motion.Disable(mm::Idler); return true; case Failed: diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index 98bc703..e590e0a 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -7,7 +7,10 @@ Motion motion; void Motion::InitAxis(Axis axis) {} -void Motion::DisableAxis(Axis axis) {} +void Motion::SetEnabled(Axis axis, bool enabled) { + axisData[axis].drv.SetEnabled(axisParams[axis].params, enabled); + axisData[axis].enabled = enabled; +} bool Motion::StallGuard(Axis axis) { return false; } diff --git a/src/modules/motion.h b/src/modules/motion.h index 0c7f0e5..8b0ef1a 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -77,8 +77,11 @@ public: /// state especially when the TMC may get randomly reset (deinited) void InitAxis(Axis axis); + /// Set axis power status + void SetEnabled(Axis axis, bool enabled); + /// Disable axis motor - void DisableAxis(Axis axis); + void Disable(Axis axis) { SetEnabled(axis, false); } /// @returns true if a stall guard event occurred recently on the axis bool StallGuard(Axis axis); @@ -148,6 +151,7 @@ private: struct AxisData { TMC2130 drv; ///< Motor driver pulse_gen::PulseGen ctrl; ///< Motor controller + bool enabled; ///< Axis enabled }; /// Helper to initialize AxisData members diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp index fe07788..ed0f8a6 100644 --- a/src/modules/selector.cpp +++ b/src/modules/selector.cpp @@ -44,7 +44,7 @@ bool Selector::Step() { return false; case Ready: currentSlot = plannedSlot; - mm::motion.DisableAxis(mm::Selector); // turn off selector motor's power every time + mm::motion.Disable(mm::Selector); // turn off selector motor's power every time return true; case Failed: default: diff --git a/tests/unit/logic/stubs/stub_motion.cpp b/tests/unit/logic/stubs/stub_motion.cpp index bad3a51..f7cd8e3 100644 --- a/tests/unit/logic/stubs/stub_motion.cpp +++ b/tests/unit/logic/stubs/stub_motion.cpp @@ -18,8 +18,8 @@ void Motion::InitAxis(Axis axis) { axes[axis].enabled = true; } -void Motion::DisableAxis(Axis axis) { - axes[axis].enabled = false; +void Motion::SetEnabled(Axis axis, bool enabled) { + axes[axis].enabled = enabled; } bool Motion::StallGuard(Axis axis) { From e53a91d56313eb0320e1d0f7a7d88ca316c9bb13 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 11 Jul 2021 22:07:23 +0200 Subject: [PATCH 26/43] Implement more methods in Motion, remove Motion::Full() Motion::Full() (without a specific axis) is counter-productive. When planning new moves the axis needs to be known beforehand, so it might be as well be given to Full() to check the proper queue. --- src/modules/motion.cpp | 33 ++++++++++++++++++++++++++------- src/modules/motion.h | 32 ++++++++++++++------------------ 2 files changed, 40 insertions(+), 25 deletions(-) diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index e590e0a..fcbe784 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -5,6 +5,7 @@ namespace motion { Motion motion; +// TODO: not implemented void Motion::InitAxis(Axis axis) {} void Motion::SetEnabled(Axis axis, bool enabled) { @@ -12,17 +13,35 @@ void Motion::SetEnabled(Axis axis, bool enabled) { axisData[axis].enabled = enabled; } -bool Motion::StallGuard(Axis axis) { return false; } +void Motion::SetMode(Axis axis, MotorMode mode) { + for (uint8_t i = 0; i != NUM_AXIS; ++i) + axisData[axis].drv.SetMode(mode); +} -void Motion::ClearStallGuardFlag(Axis axis) {} +// TODO: not implemented +bool Motion::StallGuard(Axis axis) { + return false; +} -void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) {} +// TODO: not implemented +void Motion::ClearStallGuardFlag(Axis axis) { +} -pos_t Motion::CurrentPos(Axis axis) const { return axisData[axis].ctrl.Position(); } +// TODO: not implemented +void Motion::Home(Axis axis, bool direction) { +} -void Motion::Home(Axis axis, bool direction) {} +void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) { + if (axisData[axis].ctrl.PlanMoveTo(pos, feedrate)) { + // move was queued, prepare the axis + if (!axisData[axis].enabled) + SetEnabled(axis, true); + } +} -void Motion::SetMode(Axis axis, MotorMode mode) {} +pos_t Motion::CurrentPos(Axis axis) const { + return axisData[axis].ctrl.Position(); +} bool Motion::QueueEmpty() const { for (uint8_t i = 0; i != NUM_AXIS; ++i) @@ -36,7 +55,7 @@ void Motion::AbortPlannedMoves() { axisData[i].ctrl.AbortPlannedMoves(); } -void Motion::Step() {} +st_timer_t Motion::Step() {} void ISR() {} diff --git a/src/modules/motion.h b/src/modules/motion.h index 8b0ef1a..0b78929 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -77,18 +77,27 @@ public: /// state especially when the TMC may get randomly reset (deinited) void InitAxis(Axis axis); - /// Set axis power status + /// Set axis power status. One must manually ensure no moves are currently being + /// performed by calling QueueEmpty(). void SetEnabled(Axis axis, bool enabled); - /// Disable axis motor + /// Disable axis motor. One must manually ensure no moves are currently being + /// performed by calling QueueEmpty(). void Disable(Axis axis) { SetEnabled(axis, false); } + /// Set mode of TMC/motors operation. One must manually ensure no moves are currently + /// being performed by calling QueueEmpty(). + void SetMode(Axis axis, MotorMode mode); + /// @returns true if a stall guard event occurred recently on the axis bool StallGuard(Axis axis); /// clear stall guard flag reported on an axis void ClearStallGuardFlag(Axis axis); + /// Enqueue performing of homing of an axis + void Home(Axis axis, bool direction); + /// Enqueue a single axis move in steps starting and ending at zero speed with maximum /// feedrate. Moves can only be enqueued if the axis is not Full(). /// @param axis axis affected @@ -116,14 +125,9 @@ public: axisData[axis].ctrl.SetAcceleration(accel); } - /// Enqueue performing of homing of an axis - void Home(Axis axis, bool direction); - - /// Set mode of TMC/motors operation - void SetMode(Axis axis, MotorMode mode); - - /// State machine doing all the planning and stepping preparation based on received commands - void Step(); + /// State machine doing all the planning and stepping. Called by the stepping ISR. + /// @returns the interval for the next tick + st_timer_t Step(); /// @returns true if all planned moves have been finished for all axes bool QueueEmpty() const; @@ -132,14 +136,6 @@ public: /// @param axis requested bool QueueEmpty(Axis axis) const { return axisData[axis].ctrl.QueueEmpty(); } - /// @returns false if new moves can still be planned for _any_ axis - bool Full() const { - for (uint8_t i = 0; i != NUM_AXIS; ++i) - if (axisData[i].ctrl.Full()) - return true; - return false; - } - /// @returns false if new moves can still be planned for one axis /// @param axis axis requested bool Full(Axis axis) const { return axisData[axis].ctrl.Full(); } From 9e935f6a074d41534122b9e23bb5b8777f712741 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 11 Jul 2021 22:17:04 +0200 Subject: [PATCH 27/43] Comment Typo --- tests/unit/modules/pulse_gen/test_pulse_gen.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp index f3dd5f0..208a1a6 100644 --- a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -15,7 +15,7 @@ extern uint8_t shr16_tmc_dir; } // namespace shr16 } // namespace hal -// Conveniently read the direction set into lower-level the shift register +// Conveniently read the direction set into the lower-level shift register bool getTMCDir(const MotorParams &mp) { return (hal::shr16::shr16_tmc_dir & (1 << mp.idx)) ^ mp.dirOn; } From 9b77623be1c932d8e2eda93a22be56da3949377c Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 11 Jul 2021 22:19:44 +0200 Subject: [PATCH 28/43] PulseGen: ensure Abort calculates the current remainder correctly Instead of stepping halfway, step ~1/3 of the way through. This ensures we can check if the steps performed is correct due to the internal step subtraction. --- tests/unit/modules/pulse_gen/test_pulse_gen.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp index 208a1a6..3a2b105 100644 --- a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -202,9 +202,9 @@ TEST_CASE("pulse_gen::queue_abort", "[pulse_gen]") { // queue should start empty REQUIRE(pg.QueueEmpty()); - // enqueue a move and step halfway through + // enqueue a move and step ~1/3 through REQUIRE(pg.PlanMoveTo(10, 1)); - REQUIRE(stepUntilDone(pg, mp, 5) == -1); + REQUIRE(stepUntilDone(pg, mp, 3) == -1); // abort the queue pg.AbortPlannedMoves(); @@ -214,7 +214,9 @@ TEST_CASE("pulse_gen::queue_abort", "[pulse_gen]") { bool st = ReadPin(IDLER_STEP_PIN) == Level::high; REQUIRE(pg.Step(mp) == 0); REQUIRE(st == (ReadPin(IDLER_STEP_PIN) == Level::high)); - REQUIRE(pg.Position() == 5); + + // check that the aborted position matches + REQUIRE(pg.Position() == 3); } TEST_CASE("pulse_gen::accel_ramp", "[pulse_gen]") { From 9b0dd2d63337de31141f65d4edfe2aa26255a874 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 12 Jul 2021 09:58:53 +0200 Subject: [PATCH 29/43] Motion: fix new Step() prototype --- src/modules/motion.cpp | 4 +++- src/modules/motion.h | 1 + tests/unit/logic/stubs/stub_motion.cpp | 3 ++- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index fcbe784..a902f7a 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -55,7 +55,9 @@ void Motion::AbortPlannedMoves() { axisData[i].ctrl.AbortPlannedMoves(); } -st_timer_t Motion::Step() {} +st_timer_t Motion::Step() { + return 0; +} void ISR() {} diff --git a/src/modules/motion.h b/src/modules/motion.h index 0b78929..fcc3132 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -11,6 +11,7 @@ namespace motion { using namespace hal::tmc2130; using pulse_gen::pos_t; +using pulse_gen::st_timer_t; using pulse_gen::steps_t; /// Main axis enumeration diff --git a/tests/unit/logic/stubs/stub_motion.cpp b/tests/unit/logic/stubs/stub_motion.cpp index f7cd8e3..48fdf5f 100644 --- a/tests/unit/logic/stubs/stub_motion.cpp +++ b/tests/unit/logic/stubs/stub_motion.cpp @@ -45,13 +45,14 @@ void Motion::Home(Axis axis, bool direction) { void Motion::SetMode(Axis axis, hal::tmc2130::MotorMode mode) { } -void Motion::Step() { +st_timer_t Motion::Step() { for (uint8_t i = 0; i < 3; ++i) { if (axes[i].pos != axes[i].targetPos) { int8_t dirInc = (axes[i].pos < axes[i].targetPos) ? 1 : -1; axes[i].pos += dirInc; } } + return 0; } bool Motion::QueueEmpty() const { From a5a91cbaa86667f3159f4c7476a969064d5cd5b9 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 12 Jul 2021 11:11:49 +0200 Subject: [PATCH 30/43] tests: remove relative cmake paths in pulse_gen/speed_table --- tests/unit/modules/pulse_gen/CMakeLists.txt | 14 ++++++-------- tests/unit/modules/speed_table/CMakeLists.txt | 10 ++++------ 2 files changed, 10 insertions(+), 14 deletions(-) diff --git a/tests/unit/modules/pulse_gen/CMakeLists.txt b/tests/unit/modules/pulse_gen/CMakeLists.txt index 8f0bc24..bc2cbf0 100644 --- a/tests/unit/modules/pulse_gen/CMakeLists.txt +++ b/tests/unit/modules/pulse_gen/CMakeLists.txt @@ -1,16 +1,14 @@ # define the test executable -add_executable(pulse_gen_tests - test_pulse_gen.cpp - ../../../../src/modules/pulse_gen.cpp - ../../../../src/modules/speed_table.cpp - ../stubs/stub_shr16.cpp - ../stubs/stub_gpio.cpp -) +add_executable( + pulse_gen_tests + ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp ${CMAKE_SOURCE_DIR}/src/modules/speed_table.cpp + ${MODULES_STUBS_DIR}/stub_gpio.cpp ${MODULES_STUBS_DIR}/stub_shr16.cpp test_pulse_gen.cpp + ) # define required search paths target_include_directories( pulse_gen_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules ${CMAKE_SOURCE_DIR}/src/hal -) + ) # tell build system about the test case add_catch_test(pulse_gen_tests) diff --git a/tests/unit/modules/speed_table/CMakeLists.txt b/tests/unit/modules/speed_table/CMakeLists.txt index 947bfed..858f68f 100644 --- a/tests/unit/modules/speed_table/CMakeLists.txt +++ b/tests/unit/modules/speed_table/CMakeLists.txt @@ -1,12 +1,10 @@ # define the test executable -add_executable(speed_table_tests - test_speed_table.cpp - ../../../../src/modules/speed_table.cpp) +add_executable( + speed_table_tests ${CMAKE_SOURCE_DIR}/src/modules/speed_table.cpp test_speed_table.cpp + ) # define required search paths -target_include_directories( - speed_table_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules -) +target_include_directories(speed_table_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules) # tell build system about the test case add_catch_test(speed_table_tests) From 16e7f62aee79934b0dc27a1ea5dfce99c3cf41da Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 00:48:44 +0200 Subject: [PATCH 31/43] PulseGen: introduce CurPosition() for testing CurPosition() returns the live axis position, which in this implementation is inherently expensive to compute. This shouldn't be required for the MMU, but it /will/ come in handy to check for the axis position/s in Motion tests. --- src/modules/pulse_gen.cpp | 38 +++++++++++------ src/modules/pulse_gen.h | 25 +++++++++-- .../unit/modules/pulse_gen/test_pulse_gen.cpp | 42 ++++++++++++++++++- 3 files changed, 89 insertions(+), 16 deletions(-) diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index 0c09573..b8e51be 100644 --- a/src/modules/pulse_gen.cpp +++ b/src/modules/pulse_gen.cpp @@ -115,20 +115,34 @@ bool PulseGen::PlanMoveTo(pos_t target, steps_t feed_rate) { return true; } +pos_t PulseGen::CurPosition() const { + pos_t cur_pos = position; + circular_index_t iter = block_index; + + // if we have a live block remove the partial offset + if (current_block) { + cur_pos -= CurBlockShift(); + iter.pop(); + } + + // rollback remaining blocks + while (!iter.empty()) { + cur_pos -= BlockShift(&block_buffer[iter.front()]); + iter.pop(); + } + + return cur_pos; +} + void PulseGen::AbortPlannedMoves() { - if (!current_block) - return; + // always update to effective position + position = CurPosition(); - // update position - steps_t steps_missing = (current_block->steps - steps_completed); - if (current_block->direction) - position -= steps_missing; - else - position += steps_missing; - - // destroy the block - current_block = nullptr; - block_index.pop(); + // destroy the current block + if (current_block) { + current_block = nullptr; + block_index.pop(); + } } } // namespace motor diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index 7c6edb5..b3ca9a5 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -14,6 +14,7 @@ using hal::tmc2130::TMC2130; using math::mulU24X24toH16; using speed_table::calc_timer; using speed_table::st_timer_t; +typedef CircularIndex circular_index_t; typedef uint32_t steps_t; ///< Absolute step units typedef uint32_t rate_t; ///< Type for step rates typedef int32_t pos_t; ///< Axis position (signed) @@ -35,13 +36,20 @@ public: /// @returns true if the move has been enqueued bool PlanMoveTo(pos_t pos, steps_t feedrate); - /// stop whatever moves are being done + /// Stop whatever moves are being done void AbortPlannedMoves(); - /// @returns the current position of the axis + /// @returns the position of the axis at the end of all moves pos_t Position() const { return position; } + /// Fetch the current position of the axis while stepping. This function is expensive! + /// It's necessary only in exceptional cases. For regular usage, Position() should + /// probably be used instead. + /// @returns the current position of the axis + pos_t CurPosition() const; + /// Set the position of the axis + /// Should only be called when the queue is empty. void SetPosition(pos_t x) { position = x; } /// @returns true if all planned moves have been finished @@ -150,7 +158,7 @@ private: // Block buffer parameters block_t block_buffer[blockBufferSize]; - CircularIndex block_index; + circular_index_t block_index; block_t *current_block; // Axis data @@ -168,6 +176,17 @@ private: /// Calculate the trapezoid parameters for the block void CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t exit_speed); + + /// Return the axis shift introduced by the current (live) block + inline pos_t CurBlockShift() const { + steps_t steps_missing = (current_block->steps - steps_completed); + return current_block->direction ? steps_missing : -steps_missing; + } + + /// Return the axis shift introduced by the specified full block + static inline pos_t BlockShift(const block_t *block) { + return block->direction ? block->steps : -block->steps; + } }; } // namespace pulse_gen diff --git a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp index 3a2b105..e62a9fb 100644 --- a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -112,10 +112,14 @@ TEST_CASE("pulse_gen::step_count", "[pulse_gen]") { pg.PlanMoveTo(10, 10); bool st = ReadPin(IDLER_STEP_PIN) == Level::high; for (size_t i = 0; i != 10; ++i) { + // check current axis position + REQUIRE((pos_t)i == pg.CurPosition()); + + // perform the step REQUIRE(pg.Step(mp) > 0); bool newSt = ReadPin(IDLER_STEP_PIN) == Level::high; - // Assuming DEDGE each step should toggle the pin + // assuming DEDGE each step should toggle the pin REQUIRE(newSt != st); st = newSt; } @@ -129,6 +133,42 @@ TEST_CASE("pulse_gen::step_count", "[pulse_gen]") { REQUIRE(pg.Position() == 10); } +TEST_CASE("pulse_gen::queue_position", "[pulse_gen]") { + MotorParams mp = { + .idx = 0, + .dirOn = config::idler.dirOn, + .csPin = IDLER_CS_PIN, + .stepPin = IDLER_STEP_PIN, + .sgPin = IDLER_SG_PIN, + .uSteps = config::idler.uSteps + }; + + PulseGen pg(10, 100); + + // enqueue two moves, observing Position and CurPosition. + REQUIRE(pg.Position() == 0); + REQUIRE(pg.CurPosition() == 0); + + // while enqueuing Position should move but CurPosition should not + pg.PlanMoveTo(10, 10); + REQUIRE(pg.Position() == 10); + REQUIRE(pg.CurPosition() == 0); + + pg.PlanMoveTo(15, 10); + REQUIRE(pg.Position() == 15); + REQUIRE(pg.CurPosition() == 0); + + // step through the moves manually, cycling through two blocks + for (size_t i = 0; i != 15; ++i) { + REQUIRE((pos_t)i == pg.CurPosition()); + REQUIRE(pg.Position() == 15); + pg.Step(mp); + } + + // the final positions should match + REQUIRE(pg.CurPosition() == pg.Position()); +} + TEST_CASE("pulse_gen::queue_size", "[pulse_gen]") { PulseGen pg(10, 100); From 9bb1bf4a53534e1c3564f7ad18a596a22193445a Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 00:54:03 +0200 Subject: [PATCH 32/43] PulseGen: typos in comments --- src/modules/pulse_gen.cpp | 2 +- src/modules/pulse_gen.h | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index b8e51be..449ff50 100644 --- a/src/modules/pulse_gen.cpp +++ b/src/modules/pulse_gen.cpp @@ -94,7 +94,7 @@ bool PulseGen::PlanMoveTo(pos_t target, steps_t feed_rate) { // Bail if this is a zero-length block block->steps = abs(target - position); if (block->steps <= config::dropSegments) { - // behave as-if the block as been scheduled + // behave as-if the block has been scheduled return true; } diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index b3ca9a5..37554f1 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -77,9 +77,9 @@ public: acceleration_time = calc_timer(acc_step_rate, step_loops); steps_completed = 0; - // Set the nominal step loops to zero to indicate, that the timer value is not - // known yet. That means, delay the initialization of nominal step rate and step - // loops until the steady state is reached. + // Set the nominal step loops to zero to indicate that the timer value is not + // known yet. That means, delay the initialization of nominal step rate and + // step loops until the steady state is reached. step_loops_nominal = 0; } From 98845008aa098393d419640270d5f7fb5a858ce5 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 01:03:03 +0200 Subject: [PATCH 33/43] Rename Motion::CurrentPos to Position() This matches PulseGen::Position() and avoids confusion around the term "current": Position() returns the head position in the queue, not the "live" axis position. We have PulseGen::CurPosition() now for this purpose, although we don't expose it to Motion yet. --- src/modules/idler.cpp | 4 ++-- src/modules/motion.cpp | 2 +- src/modules/motion.h | 6 +++--- src/modules/selector.cpp | 2 +- tests/unit/logic/stubs/stub_motion.cpp | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp index 6be1be4..b04dd4b 100644 --- a/src/modules/idler.cpp +++ b/src/modules/idler.cpp @@ -22,7 +22,7 @@ bool Idler::Disengage() { mm::motion.InitAxis(mm::Idler); // plan move to idle position - mm::motion.PlanMove(mm::Idler, config::idlerSlotPositions[IdleSlotIndex()] - mm::motion.CurrentPos(mm::Idler), 1000); // @@TODO + mm::motion.PlanMove(mm::Idler, config::idlerSlotPositions[IdleSlotIndex()] - mm::motion.Position(mm::Idler), 1000); // @@TODO state = Moving; return true; } @@ -38,7 +38,7 @@ bool Idler::Engage(uint8_t slot) { return true; mm::motion.InitAxis(mm::Idler); - mm::motion.PlanMove(mm::Idler, config::idlerSlotPositions[slot] - mm::motion.CurrentPos(mm::Idler), 1000); // @@TODO + mm::motion.PlanMove(mm::Idler, config::idlerSlotPositions[slot] - mm::motion.Position(mm::Idler), 1000); // @@TODO state = Moving; return true; } diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index a902f7a..1380be7 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -39,7 +39,7 @@ void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) { } } -pos_t Motion::CurrentPos(Axis axis) const { +pos_t Motion::Position(Axis axis) const { return axisData[axis].ctrl.Position(); } diff --git a/src/modules/motion.h b/src/modules/motion.h index fcc3132..1937ef3 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -112,12 +112,12 @@ public: /// @param delta relative to current position /// @param feedrate maximum feedrate void PlanMove(Axis axis, pos_t delta, steps_t feedrate) { - PlanMoveTo(axis, CurrentPos(axis) + delta, feedrate); + PlanMoveTo(axis, Position(axis) + delta, feedrate); } - /// @returns current position of an axis + /// @returns head position of an axis (last enqueued position) /// @param axis axis affected - pos_t CurrentPos(Axis axis) const; + pos_t Position(Axis axis) const; /// Set acceleration for the selected axis /// @param axis axis affected diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp index ed0f8a6..67605e0 100644 --- a/src/modules/selector.cpp +++ b/src/modules/selector.cpp @@ -21,7 +21,7 @@ bool Selector::MoveToSlot(uint8_t slot) { return true; mm::motion.InitAxis(mm::Selector); - mm::motion.PlanMove(mm::Selector, config::selectorSlotPositions[slot] - mm::motion.CurrentPos(mm::Selector), 1000); // @@TODO + mm::motion.PlanMove(mm::Selector, config::selectorSlotPositions[slot] - mm::motion.Position(mm::Selector), 1000); // @@TODO state = Moving; return true; } diff --git a/tests/unit/logic/stubs/stub_motion.cpp b/tests/unit/logic/stubs/stub_motion.cpp index 48fdf5f..b22ec88 100644 --- a/tests/unit/logic/stubs/stub_motion.cpp +++ b/tests/unit/logic/stubs/stub_motion.cpp @@ -34,7 +34,7 @@ void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) { axes[axis].targetPos = pos; } -pos_t Motion::CurrentPos(Axis axis) const { +pos_t Motion::Position(Axis axis) const { return axes[axis].pos; } From fcb1b0c85df1a095fbdcf201f79e08ed433b5299 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 01:08:33 +0200 Subject: [PATCH 34/43] cmath.h: convert AVR min/max/abs into templates At least for min/max this will ensure types for both arguments are the same. This should be a little bit closer to the definition that simply overloads these functions instead. --- src/cmath.h | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/src/cmath.h b/src/cmath.h index bf876b3..ffd6128 100644 --- a/src/cmath.h +++ b/src/cmath.h @@ -3,15 +3,26 @@ #pragma once #ifndef __AVR__ - #include +#include #else - // AVR libc doesn't support cmath - #include +// AVR libc doesn't support cmath +#include - // Use builtin functions for min/max/abs - #define min(a, b) __builtin_min((a, b)) - #define max(a, b) __builtin_max((a, b)) - #define abs(n) __builtin_abs((n)) +// Use builtin functions for min/max/abs +template +static inline const T min(T a, T b) { + return __builtin_min((a, b)); +} + +template +static inline const T max(T a, T b) { + return __builtin_max((a, b)); +} + +template +static inline const T abs(T n) { + return __builtin_abs((n)); +} #endif From c7459df05e5ddfb7f3d52bb985bcf6b42c06bae1 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 15:10:47 +0200 Subject: [PATCH 35/43] PulseGen: improve comments. --- src/modules/pulse_gen.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index 37554f1..85c88f3 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -14,6 +14,7 @@ using hal::tmc2130::TMC2130; using math::mulU24X24toH16; using speed_table::calc_timer; using speed_table::st_timer_t; + typedef CircularIndex circular_index_t; typedef uint32_t steps_t; ///< Absolute step units typedef uint32_t rate_t; ///< Type for step rates @@ -39,7 +40,7 @@ public: /// Stop whatever moves are being done void AbortPlannedMoves(); - /// @returns the position of the axis at the end of all moves + /// @returns the head position of the axis at the end of all moves pos_t Position() const { return position; } /// Fetch the current position of the axis while stepping. This function is expensive! From d9d769721f97174b1c160bbf78e0eb51d0db7f03 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 15:24:43 +0200 Subject: [PATCH 36/43] hal/cpu: reformat --- src/hal/cpu.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/hal/cpu.h b/src/hal/cpu.h index a1713a1..ff73d0b 100644 --- a/src/hal/cpu.h +++ b/src/hal/cpu.h @@ -6,8 +6,8 @@ namespace hal { namespace cpu { #ifndef F_CPU - /// Main clock frequency - #define F_CPU (16000000ul) +/// Main clock frequency +#define F_CPU (16000000ul) #endif /// CPU init routines (not really necessary for the AVR) From 4fd22f0656c793f6e22d568cd1b156625ad4b74b Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 16:16:56 +0200 Subject: [PATCH 37/43] Motion: add initial test infrastructure and stubs --- tests/unit/modules/CMakeLists.txt | 1 + tests/unit/modules/motion/CMakeLists.txt | 19 +++++++++++++++++++ tests/unit/modules/motion/test_motion.cpp | 8 ++++++++ tests/unit/modules/stubs/stub_tmc2130.cpp | 5 +++++ 4 files changed, 33 insertions(+) create mode 100644 tests/unit/modules/motion/CMakeLists.txt create mode 100644 tests/unit/modules/motion/test_motion.cpp diff --git a/tests/unit/modules/CMakeLists.txt b/tests/unit/modules/CMakeLists.txt index 6467464..3dfd031 100644 --- a/tests/unit/modules/CMakeLists.txt +++ b/tests/unit/modules/CMakeLists.txt @@ -3,3 +3,4 @@ add_subdirectory(leds) add_subdirectory(protocol) add_subdirectory(speed_table) add_subdirectory(pulse_gen) +add_subdirectory(motion) diff --git a/tests/unit/modules/motion/CMakeLists.txt b/tests/unit/modules/motion/CMakeLists.txt new file mode 100644 index 0000000..626a338 --- /dev/null +++ b/tests/unit/modules/motion/CMakeLists.txt @@ -0,0 +1,19 @@ +# define the test executable +add_executable( + motion_tests + ${CMAKE_SOURCE_DIR}/src/modules/motion.cpp + ${CMAKE_SOURCE_DIR}/src/modules/speed_table.cpp + ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp + ${MODULES_STUBS_DIR}/stub_gpio.cpp + ${MODULES_STUBS_DIR}/stub_shr16.cpp + ${MODULES_STUBS_DIR}/stub_tmc2130.cpp + test_motion.cpp + ) + +# define required search paths +target_include_directories( + motion_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules ${CMAKE_SOURCE_DIR}/src/hal + ) + +# tell build system about the test case +add_catch_test(motion_tests) diff --git a/tests/unit/modules/motion/test_motion.cpp b/tests/unit/modules/motion/test_motion.cpp new file mode 100644 index 0000000..f6def34 --- /dev/null +++ b/tests/unit/modules/motion/test_motion.cpp @@ -0,0 +1,8 @@ +#include "catch2/catch.hpp" +#include "motion.h" + +using modules::motion::motion; + +TEST_CASE("motion::basic", "[motion]") { + // TODO +} diff --git a/tests/unit/modules/stubs/stub_tmc2130.cpp b/tests/unit/modules/stubs/stub_tmc2130.cpp index 7ab2e00..0182c69 100644 --- a/tests/unit/modules/stubs/stub_tmc2130.cpp +++ b/tests/unit/modules/stubs/stub_tmc2130.cpp @@ -6,6 +6,11 @@ namespace tmc2130 { TMC2130::TMC2130(const MotorParams ¶ms, const MotorCurrents ¤ts, MotorMode mode) { + // TODO +} + +void TMC2130::SetMode(MotorMode mode) { + // TODO } } // namespace tmc2130 From d0581bf49485cebff947a90e30e02c27fc995eac Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 17:43:50 +0200 Subject: [PATCH 38/43] Motion: Add more methods and tests - Add SetPosition/CurPosition (not only) for testing - Implement some real tests --- src/config/config.h | 2 +- src/modules/motion.h | 30 +++++- src/modules/pulse_gen.h | 1 + tests/unit/modules/motion/test_motion.cpp | 123 +++++++++++++++++++++- 4 files changed, 151 insertions(+), 5 deletions(-) diff --git a/src/config/config.h b/src/config/config.h index 98e5524..b1c247f 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -79,7 +79,7 @@ static constexpr AxisConfig selector = { .iRun = 20, .iHold = 20, .accel = 100, - .jerk = 1, + .jerk = 10, .stealth = false }; diff --git a/src/modules/motion.h b/src/modules/motion.h index 1937ef3..6dc99db 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -119,6 +119,25 @@ public: /// @param axis axis affected pos_t Position(Axis axis) const; + /// Fetch the current position of the axis while stepping. This function is expensive! + /// It's necessary only in exceptional cases. For regular usage, Position() should + /// probably be used instead. + /// @param axis axis affected + /// @returns the current position of the axis + pos_t CurPosition(Axis axis) const { return axisData[axis].ctrl.CurPosition(); } + + /// Set the position of an axis. Should only be called when the queue is empty. + /// @param axis axis affected + /// @param x position to set + void SetPosition(Axis axis, pos_t x) { axisData[axis].ctrl.SetPosition(x); } + + /// Get current acceleration for the selected axis + /// @param axis axis affected + /// @returns acceleration + steps_t Acceleration(Axis axis) { + return axisData[axis].ctrl.Acceleration(); + } + /// Set acceleration for the selected axis /// @param axis axis affected /// @param accel acceleration @@ -154,8 +173,15 @@ private: /// Helper to initialize AxisData members static AxisData DataForAxis(Axis axis) { return { - .drv = { axisParams[axis].params, axisParams[axis].currents, axisParams[axis].mode }, - .ctrl = { axisParams[axis].jerk, axisParams[axis].accel }, + .drv = { + axisParams[axis].params, + axisParams[axis].currents, + axisParams[axis].mode, + }, + .ctrl = { + axisParams[axis].jerk, + axisParams[axis].accel, + }, }; } diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index 85c88f3..ecf9fbc 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -51,6 +51,7 @@ public: /// Set the position of the axis /// Should only be called when the queue is empty. + /// @param x position to set void SetPosition(pos_t x) { position = x; } /// @returns true if all planned moves have been finished diff --git a/tests/unit/modules/motion/test_motion.cpp b/tests/unit/modules/motion/test_motion.cpp index f6def34..c73c246 100644 --- a/tests/unit/modules/motion/test_motion.cpp +++ b/tests/unit/modules/motion/test_motion.cpp @@ -1,8 +1,127 @@ #include "catch2/catch.hpp" #include "motion.h" -using modules::motion::motion; +using namespace modules::motion; + +// Perform Step() until all moves are completed, returning the number of steps performed. +// Ensure the move doesn't run forever, making the test fail reliably. +ssize_t stepUntilDone(size_t maxSteps = 100000) { + for (size_t i = 0; i != maxSteps; ++i) + if (!motion.Step()) + return i; + + // number of steps exceeded + return -1; +} TEST_CASE("motion::basic", "[motion]") { - // TODO + // initial state + REQUIRE(motion.QueueEmpty()); + REQUIRE(motion.Position(Idler) == 0); + + // enqueue a single move + motion.PlanMoveTo(Idler, 10, 1); + REQUIRE(!motion.QueueEmpty()); + + // perform the move + REQUIRE(stepUntilDone() == 10); + REQUIRE(motion.QueueEmpty()); + + // check positions + REQUIRE(motion.Position(Idler) == 10); +} + +TEST_CASE("motion::dual_move_fwd", "[motion]") { + // check for configuration values that we cannot change but should match for this test + // to function as expected (maybe this should be a static_assert?) + REQUIRE(config::idler.jerk == config::selector.jerk); + + // enqueue moves on two axes + REQUIRE(motion.QueueEmpty()); + + // ensure the same acceleration is set on both + motion.SetAcceleration(Idler, motion.Acceleration(Selector)); + REQUIRE(motion.Acceleration(Idler) == motion.Acceleration(Selector)); + + // plan two moves at the same speed and acceleration + motion.PlanMoveTo(Idler, 10, 1); + motion.PlanMoveTo(Selector, 10, 1); + + // perform the move, which should be perfectly merged + REQUIRE(stepUntilDone() == 10); + REQUIRE(motion.QueueEmpty()); + + // check for final axis positions + REQUIRE(motion.Position(Idler) == 10); + REQUIRE(motion.Position(Selector) == 10); +} + +TEST_CASE("motion::dual_move_inv", "[motion]") { + // check for configuration values that we cannot change but should match for this test + // to function as expected (maybe this should be a static_assert?) + REQUIRE(config::idler.jerk == config::selector.jerk); + + // enqueue moves on two axes + REQUIRE(motion.QueueEmpty()); + + // ensure the same acceleration is set on both + motion.SetAcceleration(Idler, motion.Acceleration(Selector)); + REQUIRE(motion.Acceleration(Idler) == motion.Acceleration(Selector)); + + // set two different starting points + motion.SetPosition(Idler, 0); + motion.SetPosition(Selector, 5); + + // plan two moves at the same speed and acceleration, like in the previous + // test this should *also* reduce to the same steps being performed + motion.PlanMove(Idler, 10, 1); + motion.PlanMove(Selector, -10, 1); + + // perform the move, which should be perfectly merged + REQUIRE(stepUntilDone() == 10); + REQUIRE(motion.QueueEmpty()); + + // check for final axis positions + REQUIRE(motion.Position(Idler) == 10); + REQUIRE(motion.Position(Selector) == -5); +} + +TEST_CASE("motion::dual_move_complex", "[motion]") { + // enqueue two completely different moves on two axes + REQUIRE(motion.QueueEmpty()); + + // set custom acceleration values + motion.SetAcceleration(Idler, 10); + motion.SetAcceleration(Selector, 20); + + // plan two moves at the same speed and acceleration, like in the previous + // test: this should *also* reduce to the same steps being performed + motion.PlanMoveTo(Idler, 10, 1); + motion.PlanMoveTo(Selector, 10, 1); + + // perform the move, which should take less iterations than the sum of both + REQUIRE(stepUntilDone(20)); + REQUIRE(motion.QueueEmpty()); + + // check for final axis positions + REQUIRE(motion.Position(Idler) == 10); + REQUIRE(motion.Position(Selector) == 10); +} + +TEST_CASE("motion::triple_move", "[motion]") { + // that that we can move three axes at the same time + motion.PlanMoveTo(Idler, 10, 1); + motion.PlanMoveTo(Selector, 20, 1); + motion.PlanMoveTo(Pulley, 30, 1); + + // perform the move with a maximum step limit + REQUIRE(stepUntilDone(10 + 20 + 30)); + + // check queue status + REQUIRE(motion.QueueEmpty()); + + // check for final axis positions + REQUIRE(motion.Position(Idler) == 10); + REQUIRE(motion.Position(Selector) == 20); + REQUIRE(motion.Position(Pulley) == 30); } From 324fb92fd53221236f727fd238f720cdabdbac4f Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 19:21:13 +0200 Subject: [PATCH 39/43] Motion: implement timer multiplexing --- src/config/config.h | 4 +++ src/modules/motion.cpp | 24 ++++++++++++- src/modules/motion.h | 1 + tests/unit/modules/motion/test_motion.cpp | 43 +++++++++++++++++++++++ 4 files changed, 71 insertions(+), 1 deletion(-) diff --git a/src/config/config.h b/src/config/config.h index b1c247f..3875d94 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -47,6 +47,10 @@ static constexpr uint8_t blockBufferSize = 2; /// Step timer frequency divider (F = F_CPU / divider) static constexpr uint8_t stepTimerFrequencyDivider = 8; +/// Smallest stepping ISR scheduling slice (T = F_CPU / divider * quantum) +/// 16 = 8us (25us is the max frequency interval per maxStepFrequency) +static constexpr uint8_t stepTimerQuantum = 16; + /// Idler configuration static constexpr AxisConfig idler = { .dirOn = true, diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index 1380be7..26386fd 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -56,7 +56,29 @@ void Motion::AbortPlannedMoves() { } st_timer_t Motion::Step() { - return 0; + st_timer_t timers[NUM_AXIS]; + + // step and calculate interval for each new move + for (uint8_t i = 0; i != NUM_AXIS; ++i) { + timers[i] = axisData[i].residual; + if (timers[i] <= config::stepTimerQuantum) { + timers[i] += axisData[i].ctrl.Step(axisParams[i].params); + } + } + + // plan next closest interval + st_timer_t next = timers[0]; + for (uint8_t i = 1; i != NUM_AXIS; ++i) { + if (timers[i] && (!next || timers[i] < next)) + next = timers[i]; + } + + // update residuals + for (uint8_t i = 0; i != NUM_AXIS; ++i) { + axisData[i].residual = (timers[i] ? timers[i] - next : 0); + } + + return next; } void ISR() {} diff --git a/src/modules/motion.h b/src/modules/motion.h index 6dc99db..301a4c9 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -168,6 +168,7 @@ private: TMC2130 drv; ///< Motor driver pulse_gen::PulseGen ctrl; ///< Motor controller bool enabled; ///< Axis enabled + st_timer_t residual; ///< Axis timer residual }; /// Helper to initialize AxisData members diff --git a/tests/unit/modules/motion/test_motion.cpp b/tests/unit/modules/motion/test_motion.cpp index c73c246..1702651 100644 --- a/tests/unit/modules/motion/test_motion.cpp +++ b/tests/unit/modules/motion/test_motion.cpp @@ -125,3 +125,46 @@ TEST_CASE("motion::triple_move", "[motion]") { REQUIRE(motion.Position(Selector) == 20); REQUIRE(motion.Position(Pulley) == 30); } + +TEST_CASE("motion::dual_move_ramp", "[motion]") { + // TODO: output ramps still to be checked + const int idlerSteps = 100; + const int selectorSteps = 80; + const int maxFeedRate = 1000; + + for (int accel = 2000; accel <= 50000; accel *= 2) { + REQUIRE(motion.QueueEmpty()); + + // first axis using nominal values + motion.SetPosition(Idler, 0); + motion.SetAcceleration(Idler, accel); + motion.PlanMoveTo(Idler, idlerSteps, maxFeedRate); + + // second axis finishes slightly sooner at triple acceleration to maximize the + // aliasing effects + motion.SetPosition(Selector, 0); + motion.SetAcceleration(Selector, accel * 3); + motion.PlanMoveTo(Selector, selectorSteps, maxFeedRate); + + // step and output time, interval and positions + unsigned long ts = 0; + st_timer_t next; + do { + next = motion.Step(); + pos_t pos_idler = motion.CurPosition(Idler); + pos_t pos_selector = motion.CurPosition(Selector); + + printf("%lu %u %d %d\n", ts, next, pos_idler, pos_selector); + + ts += next; + } while (next); + printf("\n\n"); + + // check queue status + REQUIRE(motion.QueueEmpty()); + + // check final position + REQUIRE(motion.Position(Idler) == idlerSteps); + REQUIRE(motion.Position(Selector) == selectorSteps); + } +} From eec95d6fb744703acdbc2cd93a3aa908fab1875e Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 19:40:00 +0200 Subject: [PATCH 40/43] Motion: add assertions for stepTimerQuantum --- src/modules/motion.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/motion.h b/src/modules/motion.h index 301a4c9..7a864e1 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -14,6 +14,12 @@ using pulse_gen::pos_t; using pulse_gen::st_timer_t; using pulse_gen::steps_t; +// Check for configuration invariants +static_assert( + (1. / (F_CPU / config::stepTimerFrequencyDivider) * config::stepTimerQuantum) + < (1. / config::maxStepFrequency / 2), + "stepTimerQuantum must be smaller than the maximal stepping frequency interval"); + /// Main axis enumeration enum Axis : uint8_t { Pulley, From f28567a0515ab270864a659fa68ab92b41f8e2f6 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 20:21:28 +0200 Subject: [PATCH 41/43] Motion tests: improve comments --- tests/unit/modules/motion/test_motion.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/tests/unit/modules/motion/test_motion.cpp b/tests/unit/modules/motion/test_motion.cpp index 1702651..d241b83 100644 --- a/tests/unit/modules/motion/test_motion.cpp +++ b/tests/unit/modules/motion/test_motion.cpp @@ -72,7 +72,7 @@ TEST_CASE("motion::dual_move_inv", "[motion]") { motion.SetPosition(Idler, 0); motion.SetPosition(Selector, 5); - // plan two moves at the same speed and acceleration, like in the previous + // plan two moves at the same speed and acceleration: like in the previous // test this should *also* reduce to the same steps being performed motion.PlanMove(Idler, 10, 1); motion.PlanMove(Selector, -10, 1); @@ -94,8 +94,7 @@ TEST_CASE("motion::dual_move_complex", "[motion]") { motion.SetAcceleration(Idler, 10); motion.SetAcceleration(Selector, 20); - // plan two moves at the same speed and acceleration, like in the previous - // test: this should *also* reduce to the same steps being performed + // plan two moves with difference accelerations motion.PlanMoveTo(Idler, 10, 1); motion.PlanMoveTo(Selector, 10, 1); @@ -109,7 +108,7 @@ TEST_CASE("motion::dual_move_complex", "[motion]") { } TEST_CASE("motion::triple_move", "[motion]") { - // that that we can move three axes at the same time + // check that we can move three axes at the same time motion.PlanMoveTo(Idler, 10, 1); motion.PlanMoveTo(Selector, 20, 1); motion.PlanMoveTo(Pulley, 30, 1); From 8f0732a0ccdb132907dd5b5a00da47a89ccfd29a Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 20:36:43 +0200 Subject: [PATCH 42/43] Motion: initial StallGuard support Add the code in the right position to sample StallGuard and set/reset the trigger flag. --- src/modules/motion.cpp | 19 ++++++++++++++++--- src/modules/motion.h | 2 ++ 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index 26386fd..36ca904 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -11,6 +11,12 @@ void Motion::InitAxis(Axis axis) {} void Motion::SetEnabled(Axis axis, bool enabled) { axisData[axis].drv.SetEnabled(axisParams[axis].params, enabled); axisData[axis].enabled = enabled; + + if (!axisData[axis].enabled) { + // axis is powered off, clear internal StallGuard counters + axisData[axis].stall_trig = false; + axisData[axis].stall_cnt = 0; + } } void Motion::SetMode(Axis axis, MotorMode mode) { @@ -18,13 +24,12 @@ void Motion::SetMode(Axis axis, MotorMode mode) { axisData[axis].drv.SetMode(mode); } -// TODO: not implemented bool Motion::StallGuard(Axis axis) { - return false; + return axisData[axis].stall_trig; } -// TODO: not implemented void Motion::ClearStallGuardFlag(Axis axis) { + axisData[axis].stall_trig = false; } // TODO: not implemented @@ -63,6 +68,14 @@ st_timer_t Motion::Step() { timers[i] = axisData[i].residual; if (timers[i] <= config::stepTimerQuantum) { timers[i] += axisData[i].ctrl.Step(axisParams[i].params); + + // axis has been moved, sample StallGuard + if (hal::tmc2130::TMC2130::Stall(axisParams[i].params)) { + // TODO: on the MK3 a stall is marked as such as 1/2 of a full step is + // lost: this is too simplistic for production + ++axisData[i].stall_cnt; + axisData[i].stall_trig = true; + } } } diff --git a/src/modules/motion.h b/src/modules/motion.h index 7a864e1..2c0b311 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -175,6 +175,8 @@ private: pulse_gen::PulseGen ctrl; ///< Motor controller bool enabled; ///< Axis enabled st_timer_t residual; ///< Axis timer residual + uint8_t stall_cnt; ///< Underlying StallGuard lost ustep count + bool stall_trig; ///< StallGuard trigger flag }; /// Helper to initialize AxisData members From 4160d019be1625e5f2541a6b6072a56403dec840 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 20:44:09 +0200 Subject: [PATCH 43/43] Motion: implement InitAxis --- src/modules/motion.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index 36ca904..b358149 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -5,8 +5,14 @@ namespace motion { Motion motion; -// TODO: not implemented -void Motion::InitAxis(Axis axis) {} +void Motion::InitAxis(Axis axis) { + for (uint8_t i = 0; i != NUM_AXIS; ++i) { + // disable the axis and re-init the driver: this will clear the internal + // StallGuard data as a result without special handling + Disable(axis); + axisData[axis].drv.Init(axisParams[axis].params); + } +} void Motion::SetEnabled(Axis axis, bool enabled) { axisData[axis].drv.SetEnabled(axisParams[axis].params, enabled);