motion.h

Go to the documentation of this file.

#pragma once
#include "../pins.h"
#include "pulse_gen.h"
#include "axisunit.h"
 
namespace modules {
 
namespace motion {
 
// Import axes definitions
using config::NUM_AXIS;
 
using namespace hal::tmc2130;
using pulse_gen::st_timer_t;
 
// Check for configuration invariants
static_assert(
    (1. / (F_CPU / config::stepTimerFrequencyDivider) * config::stepTimerQuantum)
        > (1. / config::maxStepFrequency),
    "stepTimerQuantum must be larger than the maximal stepping frequency interval");
 
struct AxisParams {
    char name;
    MotorParams params;
    MotorCurrents currents;
    MotorMode mode;
    steps_t jerk;
    steps_t accel;
};
 
static constexpr MotorMode DefaultMotorMode(const config::AxisConfig &axis) {
    return axis.stealth ? MotorMode::Stealth : MotorMode::Normal;
}
 
static AxisParams axisParams[NUM_AXIS] = {
    // Pulley
    {
        .name = 'P',
        .params = { .spi = hal::spi::TmcSpiBus, .idx = Pulley, .dirOn = config::pulley.dirOn, .csPin = PULLEY_CS_PIN, .stepPin = PULLEY_STEP_PIN, .sgPin = PULLEY_SG_PIN, .mRes = config::pulley.mRes, .sg_thrs = config::pulley.sg_thrs, Axis::Pulley },
        .currents = MotorCurrents(config::pulley.iRun, config::pulley.iHold),
        .mode = DefaultMotorMode(config::pulley),
        .jerk = unitToSteps<P_speed_t>(config::pulleyLimits.jerk),
        .accel = unitToSteps<P_accel_t>(config::pulleyLimits.accel),
    },
    // Selector
    {
        .name = 'S',
        .params = { .spi = hal::spi::TmcSpiBus, .idx = Selector, .dirOn = config::selector.dirOn, .csPin = SELECTOR_CS_PIN, .stepPin = SELECTOR_STEP_PIN, .sgPin = SELECTOR_SG_PIN, .mRes = config::selector.mRes, .sg_thrs = config::selector.sg_thrs, Axis::Selector },
        .currents = MotorCurrents(config::selector.iRun, config::selector.iHold),
        .mode = DefaultMotorMode(config::selector),
        .jerk = unitToSteps<S_speed_t>(config::selectorLimits.jerk),
        .accel = unitToSteps<S_accel_t>(config::selectorLimits.accel),
    },
    // Idler
    {
        .name = 'I',
        .params = { .spi = hal::spi::TmcSpiBus, .idx = Idler, .dirOn = config::idler.dirOn, .csPin = IDLER_CS_PIN, .stepPin = IDLER_STEP_PIN, .sgPin = IDLER_SG_PIN, .mRes = config::idler.mRes, .sg_thrs = config::idler.sg_thrs, Axis::Idler },
        .currents = MotorCurrents(config::idler.iRun, config::idler.iHold),
        .mode = DefaultMotorMode(config::idler),
        .jerk = unitToSteps<I_speed_t>(config::idlerLimits.jerk),
        .accel = unitToSteps<I_accel_t>(config::idlerLimits.accel),
    },
};
 
class Motion {
public:
    inline constexpr Motion() = default;
 
    bool InitAxis(Axis axis);
    bool InitAxis(Axis axis, MotorCurrents mc);
 
    bool Enabled(Axis axis) const { return axisData[axis].enabled; }
 
    void SetEnabled(Axis axis, bool enabled);
 
    void Disable(Axis axis) { SetEnabled(axis, false); }
 
    void SetMode(Axis axis, MotorMode mode);
 
    void SetMode(MotorMode mode);
 
    bool StallGuard(Axis axis);
 
    void StallGuardReset(Axis axis);
 
    void PlanStallGuardThreshold(Axis axis, int8_t sg_thrs);
 
    void PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rate = 0);
 
    template <Axis A>
    constexpr void PlanMoveTo(AxisUnit<pos_t, A, Lenght> pos,
        AxisUnit<steps_t, A, Speed> feed_rate, AxisUnit<steps_t, A, Speed> end_rate = { 0 }) {
        PlanMoveTo(A, pos.v, feed_rate.v, end_rate.v);
    }
 
    template <Axis A, config::UnitBase B>
    constexpr void PlanMoveTo(config::Unit<long double, B, Lenght> pos,
        config::Unit<long double, B, Speed> feed_rate, config::Unit<long double, B, Speed> end_rate = { 0 }) {
        PlanMoveTo<A>(
            unitToAxisUnit<AxisUnit<pos_t, A, Lenght>>(pos),
            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feed_rate),
            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(end_rate));
    }
 
    void PlanMove(Axis axis, pos_t delta, steps_t feed_rate, steps_t end_rate = 0) {
        PlanMoveTo(axis, Position(axis) + delta, feed_rate, end_rate);
    }
 
    template <Axis A>
    constexpr void PlanMove(AxisUnit<pos_t, A, Lenght> delta,
        AxisUnit<steps_t, A, Speed> feed_rate, AxisUnit<steps_t, A, Speed> end_rate = { 0 }) {
        PlanMove(A, delta.v, feed_rate.v, end_rate.v);
    }
 
    template <Axis A, config::UnitBase B>
    constexpr void PlanMove(config::Unit<long double, B, Lenght> delta,
        config::Unit<long double, B, Speed> feed_rate, config::Unit<long double, B, Speed> end_rate = { 0 }) {
        PlanMove<A>(
            unitToAxisUnit<AxisUnit<pos_t, A, Lenght>>(delta),
            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feed_rate),
            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(end_rate));
    }
 
    pos_t Position(Axis axis) const;
 
    template <Axis A>
    constexpr AxisUnit<pos_t, A, Lenght> Position() const {
        return AxisUnit<pos_t, A, Lenght> { Position(A) };
    }
 
    pos_t CurPosition(Axis axis) const;
 
    template <Axis A>
    constexpr AxisUnit<pos_t, A, Lenght> CurPosition() const {
        return AxisUnit<pos_t, A, Lenght> { CurPosition(A) };
    }
 
#if !defined(UNITTEST) || defined(UNITTEST_MOTION)
    void SetPosition(Axis axis, pos_t x) {
        axisData[axis].ctrl.SetPosition(x);
    }
#else
    // Force STUB for testing
    void SetPosition(Axis axis, pos_t x);
#endif
 
    steps_t Acceleration(Axis axis) const {
        return axisData[axis].ctrl.Acceleration();
    }
 
    void SetAcceleration(Axis axis, steps_t accel) {
        axisData[axis].ctrl.SetAcceleration(accel);
    }
 
    template <Axis A>
    void SetAcceleration(AxisUnit<steps_t, A, Accel> accel) {
        SetAcceleration(A, accel.v);
    }
 
    template <Axis A, config::UnitBase B>
    void SetAcceleration(config::Unit<long double, B, Accel> accel) {
        SetAcceleration<A>(unitToAxisUnit<AxisUnit<steps_t, A, Accel>>(accel));
    }
 
    steps_t Jerk(Axis axis) const {
        return axisData[axis].ctrl.Jerk();
    }
 
    void SetJerk(Axis axis, steps_t max_jerk) {
        return axisData[axis].ctrl.SetJerk(max_jerk);
    }
 
    steps_t Rate(Axis axis) const {
        return axisData[axis].ctrl.Rate();
    }
 
#if !defined(UNITTEST) || defined(UNITTEST_MOTION)
    inline st_timer_t 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) {
                if (timers[i] || !axisData[i].ctrl.QueueEmpty()) {
                    st_timer_t next = axisData[i].ctrl.Step(axisParams[i].params);
                    if (next) {
                        timers[i] += next;
 
                        // axis has been moved, run the tmc2130 Isr for this axis
                        axisData[i].drv.Isr(axisParams[i].params);
                    } else {
                        // axis finished, reset residual
                        timers[i] = 0;
                    }
                }
            }
        }
 
        // 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;
    }
#else
    // Force STUB for testing
    st_timer_t Step();
#endif
 
    bool QueueEmpty() const;
 
#if !defined(UNITTEST) || defined(UNITTEST_MOTION)
    bool QueueEmpty(Axis axis) const {
        return axisData[axis].ctrl.QueueEmpty();
    }
#else
    // Force STUB for testing
    bool QueueEmpty(Axis axis) const;
#endif
 
#if !defined(UNITTEST) || defined(UNITTEST_MOTION)
    uint8_t PlannedMoves(Axis axis) const {
        return axisData[axis].ctrl.PlannedMoves();
    }
#else
    // Force STUB for testing
    uint8_t PlannedMoves(Axis axis) const;
#endif
    bool Full(Axis axis) const { return axisData[axis].ctrl.Full(); }
 
    void AbortPlannedMoves(Axis axis, bool halt = true);
 
    void AbortPlannedMoves(bool halt = true);
 
    inline hal::tmc2130::TMC2130 &DriverForAxis(Axis axis) {
        return axisData[axis].drv;
    }
 
    inline hal::tmc2130::TMC2130 &MMU_NEEDS_ATTENTION_DriverForAxis(Axis axis) {
        return axisData[axis].drv;
    }
 
    inline const pulse_gen::PulseGen &CtrlForAxis(Axis axis) const {
        return axisData[axis].ctrl;
    }
 
    inline const MotorCurrents &CurrentsForAxis(Axis axis) const {
        return axisData[axis].currents;
    }
    inline void SetIRunForAxis(Axis axis, uint8_t i) {
        axisData[axis].currents.iRun = i;
    }
 
private:
    struct AxisData {
        TMC2130 drv; 
        pulse_gen::PulseGen ctrl; 
        bool enabled; 
        MotorCurrents currents; 
        st_timer_t residual; 
    };
 
    static AxisData DataForAxis(Axis axis) {
        return {
            .drv = {},
            .ctrl = {
                axisParams[axis].jerk,
                axisParams[axis].accel,
            },
            .enabled = false,
            .currents = axisParams[axis].currents
        };
    }
 
    AxisData axisData[NUM_AXIS] = {
        DataForAxis(Pulley),
        DataForAxis(Selector),
        DataForAxis(Idler),
    };
};
 
extern void Init();
 
extern Motion motion;
 
} // namespace motion
} // namespace modules
 
namespace mm = modules::motion;