diff --git a/CMakeLists.txt b/CMakeLists.txt index 1595cc5..80fddab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -143,12 +143,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) diff --git a/src/cmath.h b/src/cmath.h new file mode 100644 index 0000000..ffd6128 --- /dev/null +++ b/src/cmath.h @@ -0,0 +1,28 @@ +// 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 +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 diff --git a/src/config/axis.h b/src/config/axis.h new file mode 100644 index 0000000..6ca96a8 --- /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 + 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 11c0399..3875d94 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -1,5 +1,6 @@ #pragma once #include +#include "axis.h" /// Wrangler for assorted compile-time configuration and constants. namespace config { @@ -26,4 +27,64 @@ static constexpr const uint16_t buttonsDebounceMs = 100; static constexpr const uint16_t buttonADCLimits[buttonCount][2] = { { 0, 50 }, { 80, 100 }, { 160, 180 } }; static constexpr const uint8_t buttonsADCIndex = 5; ///< 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; + +/// 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; + +/// 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; + +/// 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, + .uSteps = 16, + .vSense = false, + .iRun = 20, + .iHold = 20, + .accel = 100, + .jerk = 10, + .stealth = false, +}; + +/// Pulley configuration +static constexpr AxisConfig pulley = { + .dirOn = true, + .uSteps = 16, + .vSense = false, + .iRun = 20, + .iHold = 20, + .accel = 100, + .jerk = 10, + .stealth = false, +}; + +/// Selector configuration +static constexpr AxisConfig selector = { + .dirOn = true, + .uSteps = 16, + .vSense = false, + .iRun = 20, + .iHold = 20, + .accel = 100, + .jerk = 10, + .stealth = false +}; + } // namespace config diff --git a/src/hal/CMakeLists.txt b/src/hal/CMakeLists.txt index c417e72..1776def 100644 --- a/src/hal/CMakeLists.txt +++ b/src/hal/CMakeLists.txt @@ -1,9 +1,3 @@ target_sources( - firmware - PRIVATE - avr/cpu.cpp - avr/usart.cpp - avr/shr16.cpp - avr/eeprom.cpp - adc.cpp -) + firmware PRIVATE avr/cpu.cpp avr/usart.cpp avr/shr16.cpp avr/eeprom.cpp avr/tmc2130.cpp adc.cpp + ) 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/hal/cpu.h b/src/hal/cpu.h index 9775408..ff73d0b 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/logic/eject_filament.cpp b/src/logic/eject_filament.cpp index 3db91d9..82e7dbd 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: @@ -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/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/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/CMakeLists.txt b/src/modules/CMakeLists.txt index 8e3f3e1..c653a1a 100644 --- a/src/modules/CMakeLists.txt +++ b/src/modules/CMakeLists.txt @@ -1,17 +1,17 @@ target_sources( firmware - PRIVATE - protocol.cpp - buttons.cpp - debouncer.cpp - finda.cpp - fsensor.cpp - globals.cpp - idler.cpp - leds.cpp - motion.cpp - permanent_storage.cpp - selector.cpp - timebase.cpp - user_input.cpp -) + PRIVATE protocol.cpp + buttons.cpp + debouncer.cpp + finda.cpp + fsensor.cpp + globals.cpp + idler.cpp + leds.cpp + motion.cpp + permanent_storage.cpp + selector.cpp + timebase.cpp + user_input.cpp + pulse_gen.cpp + ) diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp index a23e1fa..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; } @@ -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/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 diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index 22914f6..b358149 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -1,71 +1,106 @@ #include "motion.h" -#include "../hal/shr16.h" namespace modules { namespace motion { Motion motion; -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::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; } + if (!axisData[axis].enabled) { + // axis is powered off, clear internal StallGuard counters + axisData[axis].stall_trig = false; + axisData[axis].stall_cnt = 0; + } +} -void Motion::ClearStallGuardFlag(Axis axis) {} +void Motion::SetMode(Axis axis, MotorMode mode) { + for (uint8_t i = 0; i != NUM_AXIS; ++i) + axisData[axis].drv.SetMode(mode); +} -void Motion::PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed) {} +bool Motion::StallGuard(Axis axis) { + return axisData[axis].stall_trig; +} -void Motion::PlanMove(Axis axis, int16_t delta, uint16_t feedrate) {} +void Motion::ClearStallGuardFlag(Axis axis) { + axisData[axis].stall_trig = false; +} -uint16_t Motion::CurrentPos(Axis axis) const { return 0; } +// 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(MotorMode mode) {} +pos_t Motion::Position(Axis axis) const { + return axisData[axis].ctrl.Position(); +} -void Motion::Step() {} +bool Motion::QueueEmpty() const { + for (uint8_t i = 0; i != NUM_AXIS; ++i) + if (!axisData[i].ctrl.QueueEmpty()) + return false; + return true; +} -bool Motion::QueueEmpty() const { return false; } +void Motion::AbortPlannedMoves() { + for (uint8_t i = 0; i != NUM_AXIS; ++i) + axisData[i].ctrl.AbortPlannedMoves(); +} -void Motion::AbortPlannedMoves() {} +st_timer_t Motion::Step() { + 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); + + // 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; + } + } + } + + // 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() {} -//@@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..2c0b311 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -1,74 +1,100 @@ #pragma once -#include +#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 modules { namespace motion { -enum Axis { +using namespace hal::tmc2130; +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, Selector, Idler, + _Axis_Last = Idler }; -enum MotorMode { - Stealth, - Normal +static constexpr uint8_t NUM_AXIS = _Axis_Last + 1; + +struct AxisParams { + char name; + MotorParams params; + MotorCurrents currents; + MotorMode mode; + steps_t jerk; + steps_t accel; }; -enum IdlerMode { - Engage, - Disengage +/// 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] = { + // 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 }, + .mode = DefaultMotorMode(config::pulley), + .jerk = config::pulley.jerk, + .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 }, + .mode = DefaultMotorMode(config::selector), + .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, + }, }; -/// 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; - /// 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 - void DisableAxis(Axis axis); + /// 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. 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); @@ -76,40 +102,104 @@ 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 delta number of steps in either direction - /// @param feedrate maximum feedrate/speed after acceleration - void PlanMove(Axis axis, int16_t delta, uint16_t feedrate); - - /// @returns current position of an axis - /// @param axis axis affected - uint16_t CurrentPos(Axis axis) const; - /// 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(MotorMode mode); + /// 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); - /// State machine doing all the planning and stepping preparation based on received commands - void Step(); + /// 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 + void PlanMove(Axis axis, pos_t delta, steps_t feedrate) { + PlanMoveTo(axis, Position(axis) + delta, feedrate); + } - /// @returns true if all planned moves have been finished + /// @returns head position of an axis (last enqueued position) + /// @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 + void SetAcceleration(Axis axis, steps_t accel) { + axisData[axis].ctrl.SetAcceleration(accel); + } + + /// 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; + /// @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 one axis + /// @param axis axis requested + bool Full(Axis axis) const { return axisData[axis].ctrl.Full(); } + /// 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 + 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 + 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] = { + DataForAxis(Pulley), + DataForAxis(Selector), + DataForAxis(Idler), + }; }; /// ISR stepping routine diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp new file mode 100644 index 0000000..449ff50 --- /dev/null +++ b/src/modules/pulse_gen.cpp @@ -0,0 +1,149 @@ +#include "pulse_gen.h" + +namespace modules { +namespace pulse_gen { + +PulseGen::PulseGen(steps_t max_jerk, steps_t acceleration) { + // Axis status + position = 0; + this->max_jerk = max_jerk; + this->acceleration = acceleration; + + // Block buffer + current_block = nullptr; +} + +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 = entry_speed; + rate_t final_rate = exit_speed; + + // Limit minimal step rate (Otherwise the timer will overflow.) + if (initial_rate < config::minStepRate) + initial_rate = config::minStepRate; + if (initial_rate > block->nominal_rate) + initial_rate = block->nominal_rate; + if (final_rate < config::minStepRate) + final_rate = config::minStepRate; + if (final_rate > block->nominal_rate) + final_rate = block->nominal_rate; + + // Don't allow zero acceleration. + rate_t acceleration = block->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->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->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); + accelerate_steps = final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1; + if (block->steps & 1) + accelerate_steps += acceleration_x2; + accelerate_steps /= acceleration_x4; + accelerate_steps += (block->steps >> 1); + if (accelerate_steps > block->steps) + accelerate_steps = block->steps; + } else { + decelerate_steps = initial_rate_sqr - final_rate_sqr; + if (block->steps & 1) + decelerate_steps += acceleration_x2; + decelerate_steps /= acceleration_x4; + decelerate_steps += (block->steps >> 1); + if (decelerate_steps > block->steps) + decelerate_steps = block->steps; + accelerate_steps = block->steps - decelerate_steps; + } + } + + block->accelerate_until = accelerate_steps; + block->decelerate_after = accelerate_steps + plateau_steps; + block->initial_rate = initial_rate; + block->final_rate = final_rate; +} + +bool PulseGen::PlanMoveTo(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()]; + + // Bail if this is a zero-length block + block->steps = abs(target - position); + if (block->steps <= config::dropSegments) { + // behave as-if the block has been scheduled + return true; + } + + // Direction and speed for this block + block->direction = (target >= position); + block->nominal_rate = 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 / config::stepTimerFrequencyDivider)); + + // Perform the trapezoid calculations + CalculateTrapezoid(block, max_jerk, max_jerk); + + // Move forward + block_index.push(); + position = target; + 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() { + // always update to effective position + position = CurPosition(); + + // destroy the current block + if (current_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 new file mode 100644 index 0000000..ecf9fbc --- /dev/null +++ b/src/modules/pulse_gen.h @@ -0,0 +1,195 @@ +#pragma once +#include "speed_table.h" +#include "../hal/tmc2130.h" +#include "../hal/circular_buffer.h" +#include "../cmath.h" + +namespace modules { + +/// Acceleration ramp and stepper pulse generator +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 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) + +class PulseGen { +public: + PulseGen(steps_t max_jerk, steps_t acceleration); + + /// @returns the acceleration for the axis + steps_t Acceleration() const { return acceleration; }; + + /// Set acceleration for the axis + void SetAcceleration(steps_t accel) { acceleration = accel; } + + /// 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(); + + /// @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! + /// 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. + /// @param x position to set + void SetPosition(pos_t x) { position = x; } + + /// @returns true if all planned moves have been finished + bool QueueEmpty() const { return block_index.empty(); } + + /// @returns false if new moves can still be planned + bool Full() const { return block_index.full(); } + + /// Single-step the axis + /// @returns the interval for the next tick + 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 + 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 + }; + + // Block buffer parameters + block_t block_buffer[blockBufferSize]; + circular_index_t block_index; + block_t *current_block; + + // 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 + rate_t acceleration_time, deceleration_time; + st_timer_t acc_step_rate; // decelaration start point + 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 + + /// 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 +} // namespace modules diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp index fe07788..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; } @@ -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/src/modules/speed_table.cpp b/src/modules/speed_table.cpp new file mode 100644 index 0000000..0da0d24 --- /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 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 }, + { 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 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 }, + { 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 st_timer_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 st_timer_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..028baaa --- /dev/null +++ b/src/modules/speed_table.h @@ -0,0 +1,66 @@ +#pragma once +#include "../config/config.h" +#include "../hal/progmem.h" +#include "../hal/cpu.h" +#include "math.h" + +namespace modules { + +/// Speed tables for acceleration calculations +namespace speed_table { + +typedef uint16_t st_timer_t; + +/// CPU timer frequency divider required for the speed tables +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 +extern const st_timer_t speed_table_fast[256][2] PROGMEM; + +/// Lookup table for lower step rates +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 > 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; + } 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; + + st_timer_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) { + // 20kHz this should never happen + timer = 100; + } + return timer; +} + +} // namespace speed_table +} // namespace modules 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/cut_filament/CMakeLists.txt b/tests/unit/logic/cut_filament/CMakeLists.txt index 6f8618f..651b238 100644 --- a/tests/unit/logic/cut_filament/CMakeLists.txt +++ b/tests/unit/logic/cut_filament/CMakeLists.txt @@ -15,11 +15,13 @@ add_executable( ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp + ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp ${MODULES_STUBS_DIR}/stub_adc.cpp ${MODULES_STUBS_DIR}/stub_eeprom.cpp ${MODULES_STUBS_DIR}/stub_gpio.cpp ${MODULES_STUBS_DIR}/stub_shr16.cpp ${MODULES_STUBS_DIR}/stub_timebase.cpp + ${MODULES_STUBS_DIR}/stub_tmc2130.cpp ${LOGIC_STUBS_DIR}/main_loop_stub.cpp ${LOGIC_STUBS_DIR}/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 0f424fe..e347b1d 100644 --- a/tests/unit/logic/eject_filament/CMakeLists.txt +++ b/tests/unit/logic/eject_filament/CMakeLists.txt @@ -15,11 +15,13 @@ add_executable( ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp + ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp ${MODULES_STUBS_DIR}/stub_adc.cpp ${MODULES_STUBS_DIR}/stub_eeprom.cpp ${MODULES_STUBS_DIR}/stub_gpio.cpp ${MODULES_STUBS_DIR}/stub_shr16.cpp ${MODULES_STUBS_DIR}/stub_timebase.cpp + ${MODULES_STUBS_DIR}/stub_tmc2130.cpp ${LOGIC_STUBS_DIR}/main_loop_stub.cpp ${LOGIC_STUBS_DIR}/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 550372b..66e312e 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( ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp + ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp ${MODULES_STUBS_DIR}/stub_adc.cpp ${MODULES_STUBS_DIR}/stub_eeprom.cpp ${MODULES_STUBS_DIR}/stub_gpio.cpp ${MODULES_STUBS_DIR}/stub_shr16.cpp ${MODULES_STUBS_DIR}/stub_timebase.cpp + ${MODULES_STUBS_DIR}/stub_tmc2130.cpp ${LOGIC_STUBS_DIR}/main_loop_stub.cpp ${LOGIC_STUBS_DIR}/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 1709ac7..bd3466c 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( ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp + ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp ${MODULES_STUBS_DIR}/stub_adc.cpp ${MODULES_STUBS_DIR}/stub_eeprom.cpp ${MODULES_STUBS_DIR}/stub_gpio.cpp ${MODULES_STUBS_DIR}/stub_shr16.cpp ${MODULES_STUBS_DIR}/stub_timebase.cpp + ${MODULES_STUBS_DIR}/stub_tmc2130.cpp ${LOGIC_STUBS_DIR}/main_loop_stub.cpp ${LOGIC_STUBS_DIR}/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 b95d53b..74e8951 100644 --- a/tests/unit/logic/load_filament/CMakeLists.txt +++ b/tests/unit/logic/load_filament/CMakeLists.txt @@ -14,11 +14,13 @@ add_executable( ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp + ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp ${MODULES_STUBS_DIR}/stub_adc.cpp ${MODULES_STUBS_DIR}/stub_eeprom.cpp ${MODULES_STUBS_DIR}/stub_gpio.cpp ${MODULES_STUBS_DIR}/stub_shr16.cpp ${MODULES_STUBS_DIR}/stub_timebase.cpp + ${MODULES_STUBS_DIR}/stub_tmc2130.cpp ${LOGIC_STUBS_DIR}/main_loop_stub.cpp ${LOGIC_STUBS_DIR}/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 07e5d50..b22ec88 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) { @@ -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::Position(Axis axis) const { return axes[axis].pos; } @@ -49,16 +42,17 @@ void Motion::Home(Axis axis, bool direction) { axes[Pulley].homed = true; } -void Motion::SetMode(MotorMode mode) { +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 { 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 c33f919..064f0f3 100644 --- a/tests/unit/logic/tool_change/CMakeLists.txt +++ b/tests/unit/logic/tool_change/CMakeLists.txt @@ -17,11 +17,13 @@ add_executable( ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp + ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp ${MODULES_STUBS_DIR}/stub_adc.cpp ${MODULES_STUBS_DIR}/stub_eeprom.cpp ${MODULES_STUBS_DIR}/stub_gpio.cpp ${MODULES_STUBS_DIR}/stub_shr16.cpp ${MODULES_STUBS_DIR}/stub_timebase.cpp + ${MODULES_STUBS_DIR}/stub_tmc2130.cpp ${LOGIC_STUBS_DIR}/main_loop_stub.cpp ${LOGIC_STUBS_DIR}/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 b5a8d58..01bf7f7 100644 --- a/tests/unit/logic/unload_filament/CMakeLists.txt +++ b/tests/unit/logic/unload_filament/CMakeLists.txt @@ -14,11 +14,13 @@ add_executable( ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp + ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp ${MODULES_STUBS_DIR}/stub_adc.cpp ${MODULES_STUBS_DIR}/stub_eeprom.cpp ${MODULES_STUBS_DIR}/stub_gpio.cpp ${MODULES_STUBS_DIR}/stub_shr16.cpp ${MODULES_STUBS_DIR}/stub_timebase.cpp + ${MODULES_STUBS_DIR}/stub_tmc2130.cpp ${LOGIC_STUBS_DIR}/main_loop_stub.cpp ${LOGIC_STUBS_DIR}/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 4721d4d..33c2f87 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( ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp + ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp ${MODULES_STUBS_DIR}/stub_adc.cpp ${MODULES_STUBS_DIR}/stub_eeprom.cpp ${MODULES_STUBS_DIR}/stub_gpio.cpp ${MODULES_STUBS_DIR}/stub_shr16.cpp ${MODULES_STUBS_DIR}/stub_timebase.cpp + ${MODULES_STUBS_DIR}/stub_tmc2130.cpp ${LOGIC_STUBS_DIR}/main_loop_stub.cpp ${LOGIC_STUBS_DIR}/stub_motion.cpp test_unload_to_finda.cpp diff --git a/tests/unit/modules/CMakeLists.txt b/tests/unit/modules/CMakeLists.txt index 442fa8a..3dfd031 100644 --- a/tests/unit/modules/CMakeLists.txt +++ b/tests/unit/modules/CMakeLists.txt @@ -1,3 +1,6 @@ add_subdirectory(buttons) 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..d241b83 --- /dev/null +++ b/tests/unit/modules/motion/test_motion.cpp @@ -0,0 +1,169 @@ +#include "catch2/catch.hpp" +#include "motion.h" + +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]") { + // 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 with difference accelerations + 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]") { + // 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); + + // 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); +} + +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); + } +} diff --git a/tests/unit/modules/pulse_gen/CMakeLists.txt b/tests/unit/modules/pulse_gen/CMakeLists.txt new file mode 100644 index 0000000..bc2cbf0 --- /dev/null +++ b/tests/unit/modules/pulse_gen/CMakeLists.txt @@ -0,0 +1,14 @@ +# define the test executable +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/pulse_gen/test_pulse_gen.cpp b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp new file mode 100644 index 0000000..e62a9fb --- /dev/null +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -0,0 +1,287 @@ +#include "catch2/catch.hpp" +#include "pulse_gen.h" +#include "../pins.h" +#include + +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 the lower-level 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]") { + 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 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) { + // 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 + 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_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); + + // 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 ~1/3 through + REQUIRE(pg.PlanMoveTo(10, 1)); + REQUIRE(stepUntilDone(pg, mp, 3) == -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)); + + // check that the aborted position matches + REQUIRE(pg.Position() == 3); +} + +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); + + 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"); + } +} diff --git a/tests/unit/modules/speed_table/CMakeLists.txt b/tests/unit/modules/speed_table/CMakeLists.txt new file mode 100644 index 0000000..858f68f --- /dev/null +++ b/tests/unit/modules/speed_table/CMakeLists.txt @@ -0,0 +1,10 @@ +# define the test executable +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) + +# 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..f0223e8 --- /dev/null +++ b/tests/unit/modules/speed_table/test_speed_table.cpp @@ -0,0 +1,111 @@ +#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 2MHz timer +static_assert(F_CPU / config::stepTimerFrequencyDivider == 2000000, + "speed tables not compatible for the requested frequency"); + +static const st_timer_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) { + st_timer_t step_rate = reference[i][0]; + uint8_t 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); + + // loops should be exact + REQUIRE(loops == reference[i][2]); + + // show the table + printf("%u %u %u\n", step_rate, timer, loops); + } +} 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 } diff --git a/tests/unit/modules/stubs/stub_tmc2130.cpp b/tests/unit/modules/stubs/stub_tmc2130.cpp new file mode 100644 index 0000000..0182c69 --- /dev/null +++ b/tests/unit/modules/stubs/stub_tmc2130.cpp @@ -0,0 +1,17 @@ +#include "tmc2130.h" + +namespace hal { +namespace tmc2130 { + +TMC2130::TMC2130(const MotorParams ¶ms, + const MotorCurrents ¤ts, + MotorMode mode) { + // TODO +} + +void TMC2130::SetMode(MotorMode mode) { + // TODO +} + +} // namespace tmc2130 +} // namespace hal