fork download 
  1. /*
  2.  * Copyright (c) 2013 Ambroz Bizjak
  3.  * All rights reserved.
  4.  * 
  5.  * Redistribution and use in source and binary forms, with or without
  6.  * modification, are permitted provided that the following conditions are met:
  7.  * 1. Redistributions of source code must retain the above copyright
  8.  *    notice, this list of conditions and the following disclaimer.
  9.  * 2. Redistributions in binary form must reproduce the above copyright
  10.  *    notice, this list of conditions and the following disclaimer in the
  11.  *    documentation and/or other materials provided with the distribution.
  12.  * 
  13.  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
  14.  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  15.  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  16.  * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
  17.  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  18.  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  19.  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  20.  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  21.  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  22.  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  23.  */
  24.  
  25. #ifndef AMBROLIB_AXIS_STEPPER_H
  26. #define AMBROLIB_AXIS_STEPPER_H
  27.  
  28. #include <stddef.h>
  29. #include <stdint.h>
  30.  
  31. #include <aprinter/meta/BoundedInt.h>
  32. #include <aprinter/meta/FixedPoint.h>
  33. #include <aprinter/meta/Modulo.h>
  34. #include <aprinter/meta/WrapCallback.h>
  35. #include <aprinter/base/DebugObject.h>
  36. #include <aprinter/base/Assert.h>
  37. #include <aprinter/base/OffsetCallback.h>
  38. #include <aprinter/base/Lock.h>
  39.  
  40. #include <aprinter/BeginNamespace.h>
  41.  
  42. #define AXIS_STEPPER_AMUL_EXPR(x, t, a) (-(a).template shiftBits<(-2)>())
  43. #define AXIS_STEPPER_V0_EXPR(x, t, a) (((x).toSigned() + (a)).toUnsignedUnsafe())
  44. #define AXIS_STEPPER_V02_EXPR(x, t, a) ((AXIS_STEPPER_V0_EXPR((x), (t), (a)) * AXIS_STEPPER_V0_EXPR((x), (t), (a)))).toSigned()
  45. #define AXIS_STEPPER_TMUL_EXPR(x, t, a) ((t).template bitsTo<time_mul_bits>())
  46.  
  47. #define AXIS_STEPPER_AMUL_EXPR_HELPER(args) AXIS_STEPPER_AMUL_EXPR(args)
  48. #define AXIS_STEPPER_V0_EXPR_HELPER(args) AXIS_STEPPER_V0_EXPR(args)
  49. #define AXIS_STEPPER_V02_EXPR_HELPER(args) AXIS_STEPPER_V02_EXPR(args)
  50. #define AXIS_STEPPER_TMUL_EXPR_HELPER(args) AXIS_STEPPER_TMUL_EXPR(args)
  51.  
  52. #define AXIS_STEPPER_DUMMY_VARS (StepFixedType()), (TimeFixedType()), (AccelFixedType())
  53.  
  54. template <typename Context, int CommandBufferBits, typename Stepper, typename GetStepper, template<typename, typename> class Timer, typename AvailHandler>
  55. class AxisStepper
  56. : private DebugObject<Context, AxisStepper<Context, CommandBufferBits, Stepper, GetStepper, Timer, AvailHandler>>
  57. {
  58.     static_assert(CommandBufferBits >= 2, "");
  59.  
  60. private:
  61.     using Loop = typename Context::EventLoop;
  62.     using Clock = typename Context::Clock;
  63.     using Lock = typename Context::Lock;
  64.  
  65.     // DON'T TOUCH!
  66.     // These were chosen carefully for speed, and some operations
  67.     // were written in assembly specifically for use here.
  68.     static const int step_bits = 13;
  69.     static const int time_bits = 22;
  70.     static const int q_div_shift = 16;
  71.     static const int time_mul_bits = 23;
  72.  
  73.     struct TimerHandler;
  74.  
  75.     using TimerInstance = Timer<Context, TimerHandler>;
  76.  
  77. public:
  78.     using TimeType = typename Clock::TimeType;
  79.     using BufferSizeType = BoundedInt<CommandBufferBits, false>;
  80.     using StepFixedType = FixedPoint<step_bits, false, 0>;
  81.     using AccelFixedType = FixedPoint<step_bits, true, 0>;
  82.     using TimeFixedType = FixedPoint<time_bits, false, 0>;
  83.  
  84.     void init (Context c)
  85.     {
  86.         m_timer.init(c);
  87.         m_avail_event.init(c, AMBRO_OFFSET_CALLBACK_T(&AxisStepper::m_avail_event, &AxisStepper::avail_event_handler));
  88.         m_running = false;
  89.         m_start = BufferSizeType::import(0);
  90.         m_end = BufferSizeType::import(0);
  91.         m_event = m_end;
  92.         m_lock.init(c);
  93.         this->debugInit(c);
  94.     }
  95.  
  96.     void deinit (Context c)
  97.     {
  98.         this->debugDeinit(c);
  99.         m_lock.deinit(c);
  100.         m_avail_event.deinit(c);
  101.         m_timer.deinit(c);
  102.     }
  103.  
  104.     void clearBuffer (Context c)
  105.     {
  106.         this->debugAccess(c);
  107.         AMBRO_ASSERT(!m_running)
  108.         AMBRO_ASSERT(m_event == m_end)
  109.         AMBRO_ASSERT(!m_avail_event.isSet(c))
  110.  
  111.         m_start = BufferSizeType::import(0);
  112.         m_end = BufferSizeType::import(0);
  113.         m_event = m_end;
  114.     }
  115.  
  116.     BufferSizeType bufferGetAvail (Context c)
  117.     {
  118.         this->debugAccess(c);
  119.  
  120.         BufferSizeType start;
  121.         AMBRO_LOCK_T(m_lock, c, lock_c, {
  122.             start = m_start;
  123.         });
  124.  
  125.         return buffer_avail(start, m_end);
  126.     }
  127.  
  128.     void bufferProvideTest (Context c, bool dir, float x, float t, float ha)
  129.     {
  130.         float step_length = 0.0125;
  131.         bufferProvide(c, dir, StepFixedType::importDouble(x / step_length), TimeFixedType::importDouble(t / Clock::time_unit), AccelFixedType::importDouble(ha / step_length));
  132.     }
  133.  
  134.     void bufferProvide (Context c, bool dir, StepFixedType x, TimeFixedType t, AccelFixedType a)
  135.     {
  136.         this->debugAccess(c);
  137.         AMBRO_ASSERT(m_event == m_end)
  138.         AMBRO_ASSERT(!m_avail_event.isSet(c))
  139.         AMBRO_ASSERT(bufferQuery(c).value() > 0)
  140.         AMBRO_ASSERT(a >= -x)
  141.         AMBRO_ASSERT(a <= x)
  142.  
  143.         // compute the command parameters
  144.         Command cmd;
  145.         cmd.dir = dir;
  146.         cmd.x = x;
  147.         cmd.t_plain = t.bitsValue();
  148.         cmd.a_mul = AXIS_STEPPER_AMUL_EXPR(x, t, a);
  149.         cmd.v0 = AXIS_STEPPER_V0_EXPR(x, t, a);
  150.         cmd.v02 = AXIS_STEPPER_V02_EXPR(x, t, a);
  151.         cmd.t_mul = AXIS_STEPPER_TMUL_EXPR(x, t, a);
  152.  
  153.         // compute the clock offset based on the last command. If not running start() will do it.
  154.         if (m_running) {
  155.             Command *last_cmd = &m_commands[buffer_last(m_end).value()];
  156.             cmd.clock_end = last_cmd->clock_end + cmd.t_plain;
  157.         }
  158.  
  159.         // add command to queue
  160.         m_commands[m_end.value()] = cmd;
  161.         bool was_empty;
  162.         AMBRO_LOCK_T(m_lock, c, lock_c, {
  163.             was_empty = (m_start == m_end);
  164.             m_end = BoundedModuloInc(m_end);
  165.             m_event = m_end;
  166.         });
  167.  
  168.         // if we have run out of commands, continue motion
  169.         if (m_running && was_empty) {
  170.             m_current_command = m_commands[m_start.value()];
  171.             stepper(this)->setDir(c, m_current_command.dir);
  172.             TimeType timer_t = (m_current_command.x.bitsValue() == 0) ? m_current_command.clock_end : (m_current_command.clock_end - m_current_command.t_plain);
  173.             m_timer.set(c, timer_t);
  174.         }
  175.     }
  176.  
  177.     void bufferRequestEvent (Context c, BufferSizeType min_amount = BufferSizeType::import(1))
  178.     {
  179.         this->debugAccess(c);
  180.         AMBRO_ASSERT(min_amount.value() > 0)
  181.  
  182.         AMBRO_LOCK_T(m_lock, c, lock_c, {
  183.             if (buffer_avail(m_start, m_end) >= min_amount) {
  184.                 m_event = m_end;
  185.                 m_avail_event.prependNow(lock_c);
  186.             } else {
  187.                 m_event = BoundedModuloAdd(m_end, min_amount);
  188.                 m_avail_event.unset(lock_c);
  189.             }
  190.         });
  191.     }
  192.  
  193.     void bufferCancelEvent (Context c)
  194.     {
  195.         this->debugAccess(c);
  196.  
  197.         AMBRO_LOCK_T(m_lock, c, lock_c, {
  198.             m_event = m_end;
  199.             m_avail_event.unset(lock_c);
  200.         });
  201.     }
  202.  
  203.     void start (Context c, TimeType start_time)
  204.     {
  205.         this->debugAccess(c);
  206.         AMBRO_ASSERT(!m_running)
  207.  
  208.         // compute clock offsets for commands
  209.         if (m_start == m_end) {
  210.             Command *last_cmd = &m_commands[buffer_last(m_end).value()];
  211.             last_cmd->clock_end = start_time;
  212.             last_cmd->t_plain = 0;
  213.         } else {
  214.             TimeType clock_offset = start_time;
  215.             for (BufferSizeType i = m_start; i != m_end; i = BoundedModuloInc(i)) {
  216.                 clock_offset += m_commands[i.value()].t_plain;
  217.                 m_commands[i.value()].clock_end = clock_offset;
  218.             }
  219.         }
  220.  
  221.         m_running = true;
  222.  
  223.         // unless we don't have any commands, begin motion
  224.         if (m_start != m_end) {
  225.             m_current_command = m_commands[m_start.value()];
  226.             stepper(this)->setDir(c, m_current_command.dir);
  227.             TimeType timer_t = (m_current_command.x.bitsValue() == 0) ? m_current_command.clock_end : (m_current_command.clock_end - m_current_command.t_plain);
  228.             m_timer.set(c, timer_t);
  229.         }
  230.     }
  231.  
  232.     void stop (Context c)
  233.     {
  234.         this->debugAccess(c);
  235.         AMBRO_ASSERT(m_running)
  236.  
  237.         m_timer.unset(c);
  238.         m_running = false;
  239.     }
  240.  
  241.     bool isRunning (Context c)
  242.     {
  243.         this->debugAccess(c);
  244.  
  245.         return m_running;
  246.     }
  247.  
  248.     TimerInstance * getTimer ()
  249.     {
  250.         return &m_timer;
  251.     }
  252.  
  253. private:
  254.     static Stepper * stepper (AxisStepper *o)
  255.     {
  256.         return GetStepper::call(o);
  257.     }
  258.  
  259.     static BufferSizeType buffer_avail (BufferSizeType start, BufferSizeType end)
  260.     {
  261.         return BoundedModuloSubtract(BoundedModuloSubtract(start, BufferSizeType::import(1)), end);
  262.     }
  263.  
  264.     static BufferSizeType buffer_last (BufferSizeType end)
  265.     {
  266.         return BoundedModuloSubtract(end, BufferSizeType::import(1));
  267.     }
  268.  
  269.     struct Command {
  270.         bool dir;
  271.         StepFixedType x;
  272.         TimeType t_plain;
  273.         decltype(AXIS_STEPPER_AMUL_EXPR_HELPER(AXIS_STEPPER_DUMMY_VARS)) a_mul;
  274.         decltype(AXIS_STEPPER_V0_EXPR_HELPER(AXIS_STEPPER_DUMMY_VARS)) v0;
  275.         decltype(AXIS_STEPPER_V02_EXPR_HELPER(AXIS_STEPPER_DUMMY_VARS)) v02;
  276.         decltype(AXIS_STEPPER_TMUL_EXPR_HELPER(AXIS_STEPPER_DUMMY_VARS)) t_mul;
  277.         TimeType clock_end;
  278.     };
  279.  
  280.     void timer_handler (typename TimerInstance::HandlerContext c)
  281.     {
  282.         this->debugAccess(c);
  283.         AMBRO_ASSERT(m_running)
  284.         AMBRO_LOCK_T(m_lock, c, lock_c, {
  285.             AMBRO_ASSERT(m_start != m_end)
  286.         });
  287.  
  288.         if (m_current_command.x.bitsValue() < (int)m_running) {
  289.             bool run_out;
  290.             AMBRO_LOCK_T(m_lock, c, lock_c, {
  291.                 // report avail event if we'll have enough buffer space
  292.                 if (m_start == m_event) {
  293.                     m_event = m_end;
  294.                     m_avail_event.appendNowNotAlready(lock_c);
  295.                 }
  296.  
  297.                 // consume command
  298.                 m_start = BoundedModuloInc(m_start);
  299.  
  300.                 run_out = (m_start == m_end);
  301.             });
  302.  
  303.             // have we run out of commands?
  304.             if (run_out) {
  305.                 return;
  306.             }
  307.  
  308.             // continue with next command
  309.             m_current_command = m_commands[m_start.value()];
  310.             stepper(this)->setDir(c, m_current_command.dir);
  311.  
  312.             // if this is a motionless command, wait
  313.             if (m_current_command.x.bitsValue() == 0) {
  314.                 TimeType timer_t = m_current_command.clock_end;
  315.                 m_timer.set(c, timer_t);
  316.                 return;
  317.             }
  318.         }
  319.  
  320.         // imcrement position
  321.         m_current_command.x.m_bits.m_int--;
  322.  
  323.         // perform the step
  324.         stepper(this)->step(c);
  325.  
  326.         // compute product part of discriminant
  327.         auto s_prod = (m_current_command.a_mul * m_current_command.x).template shift<2>();
  328.  
  329.         // compute discriminant. It is not negative because of the constraints and the exact computation.
  330.         auto s = m_current_command.v02 + s_prod;
  331.         AMBRO_ASSERT(s.bitsValue() >= 0)
  332.  
  333.         // compute the thing with the square root. It can be proved it's not zero.
  334.         auto q = (m_current_command.v0 + FixedSquareRoot(s)).template shift<-1>();
  335.         AMBRO_ASSERT(q.bitsValue() > 0)
  336.  
  337.         // compute solution as fraction of total time
  338.         auto t_frac = FixedFracDivide(m_current_command.x, q);
  339.  
  340.         // multiply by the time of this command, and drop fraction bits at the same time
  341.         TimeFixedType t = FixedResMultiply(m_current_command.t_mul, t_frac);
  342.  
  343.         // schedule next step
  344.         TimeType timer_t = m_current_command.clock_end - t.bitsValue();
  345.         m_timer.set(c, timer_t);
  346.     }
  347.  
  348.     void avail_event_handler (Context c)
  349.     {
  350.         this->debugAccess(c);
  351.         AMBRO_ASSERT(buffer_avail(m_start, m_end).value() > 0)
  352.         AMBRO_ASSERT(m_event == m_end)
  353.  
  354.         return AvailHandler::call(this, c);
  355.     }
  356.  
  357.     TimerInstance m_timer;
  358.     typename Loop::QueuedEvent m_avail_event;
  359.     Command m_commands[(size_t)BufferSizeType::maxIntValue() + 1];
  360.     BufferSizeType m_start;
  361.     BufferSizeType m_end;
  362.     BufferSizeType m_event;
  363.     Command m_current_command;
  364.     bool m_running;
  365.     Lock m_lock;
  366.  
  367.     struct TimerHandler : public AMBRO_WCALLBACK_TD(&AxisStepper::timer_handler, &AxisStepper::m_timer) {};
  368. };
  369.  
  370. #include <aprinter/EndNamespace.h>
  371.  
  372. #endif
  373.  
Not running #stdin #stdout 0s 0KB
comments (?)
stdin 
Standard input is empty
stdout 
Standard output is empty
https://ideone.com/UvwUie
language:
C++14 (gcc 8.3)
created:
10 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!