diff --git a/firmware/SConstruct b/firmware/SConstruct index 9247733..1c3f1ee 100644 --- a/firmware/SConstruct +++ b/firmware/SConstruct @@ -1,6 +1,10 @@ platform = ARGUMENTS.get('platform','mb24') +fived = ARGUMENTS.get('fived','false') if (platform == 'rrmbv12' or platform == 'mb24' or platform == 'mb24-2560'): + if (fived == 'true'): + platform = platform+'-5d' + SConscript(['src/SConscript.motherboard'], variant_dir='build/'+platform) elif (platform == 'ecv22' or platform == 'ecv34'): diff --git a/firmware/build_all.sh b/firmware/build_all.sh index e50cb15..edb186b 100755 --- a/firmware/build_all.sh +++ b/firmware/build_all.sh @@ -2,33 +2,27 @@ SCONS=SConstruct -PLATFORMS=( rrmbv12 mb24 mb24-2560 ecv22 ecv34 ) +PLATFORMS=( rrmbv12 'rrmbv12 fived=true' mb24 mb24-2560 ecv22 ecv34 ) LOG_FILE=build_all_output function build_firmware { - platform_list_name="$1[*]" - platform_list=(${!platform_list_name}) - scons_file=$2 - - for platform in ${platform_list[@]} + for platform in "${PLATFORMS[@]}" do - echo -n "Building firmware for ${platform}... " - - echo -e "\n\n\n\n" >> ${LOG_FILE} - echo Building firmware for ${platform} >> ${LOG_FILE} - - scons -f ${scons_file} platform=${platform} >> ${LOG_FILE} 2>&1 + echo -n "Building firmware for ${platform}... " - if [ "$?" -ne "0" ]; then - echo Failure - else - echo Success - fi -done + echo -e "\n\n\n\n" >> ${LOG_FILE} + echo Building firmware for ${platform} >> ${LOG_FILE} + scons -f "${SCONS}" platform=${platform} >> ${LOG_FILE} 2>&1 + if [ "$?" -ne "0" ]; then + echo Failure + else + echo Success + fi + done } @@ -51,8 +45,6 @@ function build_documentation { echo Building all firmware echo "Building all firmware" > ${LOG_FILE} -build_firmware PLATFORMS ${SCONS} - - +build_firmware build_documentation diff --git a/firmware/current_version.txt b/firmware/current_version.txt index 8c50098..a3ec5a4 100644 --- a/firmware/current_version.txt +++ b/firmware/current_version.txt @@ -1 +1 @@ -3.1 +3.2 diff --git a/firmware/src/Motherboard/Command.cc b/firmware/src/Motherboard/Command.cc index 91a6865..7b6c9a6 100644 --- a/firmware/src/Motherboard/Command.cc +++ b/firmware/src/Motherboard/Command.cc @@ -17,6 +17,7 @@ #include "Command.hh" #include "Steppers.hh" +#include "Planner.hh" #include "Commands.hh" #include "Tool.hh" #include "Configuration.hh" @@ -29,7 +30,8 @@ namespace command { -#define COMMAND_BUFFER_SIZE 512 + +#define COMMAND_BUFFER_SIZE 512 // Must be 2^N: 256, 512, 1204, etc uint8_t buffer_data[COMMAND_BUFFER_SIZE]; CircularBuffer command_buffer(COMMAND_BUFFER_SIZE, buffer_data); @@ -111,6 +113,44 @@ void reset() { mode = READY; } +// Handle movement comands -- called from a few places +static void handleMovementCommand(const uint8_t &command) { + // if we're already moving, check to make sure the buffer isn't full + if (mode == MOVING && planner::isBufferFull()) { + return; // we'll be back! + } + if (command == HOST_CMD_QUEUE_POINT_EXT) { + // check for completion + if (command_buffer.getLength() >= 25) { + command_buffer.pop(); // remove the command code + mode = MOVING; + int32_t x = pop32(); + int32_t y = pop32(); + int32_t z = pop32(); + int32_t a = pop32(); + int32_t b = pop32(); + int32_t dda = pop32(); + planner::addMoveToBuffer(Point(x,y,z,a,b), dda); + } + } + else if (command == HOST_CMD_QUEUE_POINT_NEW) { + // check for completion + if (command_buffer.getLength() >= 26) { + command_buffer.pop(); // remove the command code + mode = MOVING; + int32_t x = pop32(); + int32_t y = pop32(); + int32_t z = pop32(); + int32_t a = pop32(); + int32_t b = pop32(); + int32_t us = pop32(); + uint8_t relative = pop8(); + planner::addMoveToBufferRelative(Point(x,y,z,a,b), us, relative); + } + } + +} + // A fast slice for processing commands and refilling the stepper queue, etc. void runCommandSlice() { if (sdcard::isPlaying()) { @@ -123,12 +163,21 @@ void runCommandSlice() { if (!steppers::isRunning()) { mode = READY; } else if (homing_timeout.hasElapsed()) { - steppers::abort(); + planner::abort(); mode = READY; } } if (mode == MOVING) { - if (!steppers::isRunning()) { mode = READY; } + if (!steppers::isRunning()) { + mode = READY; + } else { + if (command_buffer.getLength() > 0) { + uint8_t command = command_buffer[0]; + if (command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { + handleMovementCommand(command); + } + } + } } if (mode == DELAY) { // check timers @@ -185,48 +234,24 @@ void runCommandSlice() { // process next command on the queue. if (command_buffer.getLength() > 0) { uint8_t command = command_buffer[0]; + if (command == HOST_CMD_QUEUE_POINT_ABS) { // check for completion if (command_buffer.getLength() >= 17) { + // No longer supported, but we clear it off of the buffer to avoid a inifinite loop command_buffer.pop(); // remove the command code - mode = MOVING; - int32_t x = pop32(); - int32_t y = pop32(); - int32_t z = pop32(); - int32_t dda = pop32(); - steppers::setTarget(Point(x,y,z),dda); - } - } else if (command == HOST_CMD_QUEUE_POINT_EXT) { - // check for completion - if (command_buffer.getLength() >= 25) { - command_buffer.pop(); // remove the command code - mode = MOVING; int32_t x = pop32(); int32_t y = pop32(); int32_t z = pop32(); - int32_t a = pop32(); - int32_t b = pop32(); int32_t dda = pop32(); - steppers::setTarget(Point(x,y,z,a,b),dda); - } - } else if (command == HOST_CMD_QUEUE_POINT_NEW) { - // check for completion - if (command_buffer.getLength() >= 26) { - command_buffer.pop(); // remove the command code - mode = MOVING; - int32_t x = pop32(); - int32_t y = pop32(); - int32_t z = pop32(); - int32_t a = pop32(); - int32_t b = pop32(); - int32_t us = pop32(); - uint8_t relative = pop8(); - steppers::setTargetNew(Point(x,y,z,a,b),us,relative); + // steppers::setTarget(Point(x,y,z),dda); } + } else if (command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { + handleMovementCommand(command); } else if (command == HOST_CMD_CHANGE_TOOL) { if (command_buffer.getLength() >= 2) { command_buffer.pop(); // remove the command code - tool::setCurrentToolheadIndex(command_buffer.pop()); + tool::setCurrentToolheadIndex(command_buffer.pop()); } } else if (command == HOST_CMD_ENABLE_AXES) { if (command_buffer.getLength() >= 2) { @@ -246,7 +271,7 @@ void runCommandSlice() { int32_t x = pop32(); int32_t y = pop32(); int32_t z = pop32(); - steppers::definePosition(Point(x,y,z)); + planner::definePosition(Point(x,y,z)); } } else if (command == HOST_CMD_SET_POSITION_EXT) { // check for completion @@ -257,7 +282,7 @@ void runCommandSlice() { int32_t z = pop32(); int32_t a = pop32(); int32_t b = pop32(); - steppers::definePosition(Point(x,y,z,a,b)); + planner::definePosition(Point(x,y,z,a,b)); } } else if (command == HOST_CMD_DELAY) { if (command_buffer.getLength() >= 5) { @@ -291,7 +316,7 @@ void runCommandSlice() { tool_wait_timeout.start(toolTimeout*1000000L); } } else if (command == HOST_CMD_WAIT_FOR_PLATFORM) { - // FIXME: Almost equivalent to WAIT_FOR_TOOL + // FIXME: Almost equivalent to WAIT_FOR_TOOL if (command_buffer.getLength() >= 6) { mode = WAIT_ON_PLATFORM; command_buffer.pop(); @@ -336,7 +361,7 @@ void runCommandSlice() { } } - steppers::definePosition(newPoint); + planner::definePosition(newPoint); } } else if (command == HOST_CMD_TOOL_COMMAND) { @@ -363,6 +388,7 @@ void runCommandSlice() { } } else { } + } else { // command buffer is empty } } } diff --git a/firmware/src/Motherboard/EepromMap.hh b/firmware/src/Motherboard/EepromMap.hh index 26b4f5e..7ce0321 100644 --- a/firmware/src/Motherboard/EepromMap.hh +++ b/firmware/src/Motherboard/EepromMap.hh @@ -23,32 +23,50 @@ namespace eeprom { -const static uint16_t EEPROM_SIZE = 0x0200; +const static uint16_t EEPROM_SIZE = 0x0200; /// Version, low byte: 1 byte -const static uint16_t VERSION_LOW = 0x0000; +const static uint16_t VERSION_LOW = 0x0000; /// Version, high byte: 1 byte -const static uint16_t VERSION_HIGH = 0x0001; +const static uint16_t VERSION_HIGH = 0x0001; /// Axis inversion flags: 1 byte. -/// Axis N (where X=0, Y=1, etc.) is inverted if the Nth bit is set. +/// Axis N (where X = 0, Y=1, etc.) is inverted if the Nth bit is set. /// Bit 7 is used for HoldZ OFF: 1 = off, 0 = on -const static uint16_t AXIS_INVERSION = 0x0002; +const static uint16_t AXIS_INVERSION = 0x0002; /// Endstop inversion flags: 1 byte. -/// The endstops for axis N (where X=0, Y=1, etc.) are considered +/// The endstops for axis N (where X = 0, Y=1, etc.) are considered /// to be logically inverted if the Nth bit is set. /// Bit 7 is set to indicate endstops are present; it is zero to indicate /// that endstops are not present. /// Ordinary endstops (H21LOB et. al.) are inverted. -const static uint16_t ENDSTOP_INVERSION = 0x0003; +const static uint16_t ENDSTOP_INVERSION = 0x0003; /// Name of this machine: 32 bytes. -const static uint16_t MACHINE_NAME = 0x0020; +const static uint16_t MACHINE_NAME = 0x0020; /// Default locations for the axis: 5 x 32 bit = 20 bytes -const static uint16_t AXIS_HOME_POSITIONS = 0x0060; +const static uint16_t AXIS_HOME_POSITIONS = 0x0060; +/// Default steps/mm for each axis: 32 bits = 4 bytes +const static uint16_t ESTOP_CONFIG = 0x0074; // .. 0x0077 + +/// Default steps/mm for each axis: 5 x 32 bit = 20 bytes +const static uint16_t STEPS_PER_MM = 0x0078; // .. 0x008B + +/// Master acceleration rate for each axis: 32 bits = 4 bytes +const static uint16_t MASTER_ACCELERATION_RATE = 0x008C; // .. 0x008F + +/// Default acceleration rates for each axis: 5 x 32 bit = 20 bytes +const static uint16_t AXIS_ACCELERATION_RATES = 0x0090; // .. 0x00A3 + +/// Default acceleration rates for each axis: 4 x 32 bit = 16 bytes +/// X+Y have an integrated value, and Z, A, and B have their own values. +const static uint16_t AXIS_JUNCTION_JERK = 0x00A4; // .. 0x00B3 + +/// Default minimum planner speed: 32 bits = 1 byte +const static uint16_t MINIMUM_PLANNER_SPEED = 0x00B4; // .. 0x00B5 /// Reset all data in the EEPROM to a default. void setDefaults(); diff --git a/firmware/src/Motherboard/Host.cc b/firmware/src/Motherboard/Host.cc index 6a57fec..158635d 100644 --- a/firmware/src/Motherboard/Host.cc +++ b/firmware/src/Motherboard/Host.cc @@ -20,6 +20,7 @@ #include "Tool.hh" #include "Commands.hh" #include "Steppers.hh" +#include "Planner.hh" #include "DebugPacketProcessor.hh" #include "Timeout.hh" #include "Version.hh" @@ -411,7 +412,7 @@ enum { // bit assignments inline void handleExtendedStop(const InPacket& from_host, OutPacket& to_host) { uint8_t flags = from_host.read8(1); if (flags & _BV(ES_STEPPERS)) { - steppers::abort(); + planner::abort(); } if (flags & _BV(ES_COMMANDS)) { command::reset(); diff --git a/firmware/src/Motherboard/Main.cc b/firmware/src/Motherboard/Main.cc index 147c157..3c38485 100644 --- a/firmware/src/Motherboard/Main.cc +++ b/firmware/src/Motherboard/Main.cc @@ -24,6 +24,7 @@ #include #include "Timeout.hh" #include "Steppers.hh" +#include "Planner.hh" #include "Motherboard.hh" #include "SDCard.hh" #include "Eeprom.hh" @@ -33,10 +34,10 @@ void reset(bool hard_reset) { ATOMIC_BLOCK(ATOMIC_FORCEON) { Motherboard& board = Motherboard::getBoard(); sdcard::reset(); - steppers::abort(); command::reset(); eeprom::init(); - board.reset(); + board.reset(); // sets up the steps/mm and such for the planner... + planner::abort(); // calls steppers::abort() sei(); // If we've just come from a hard reset, wait for 2.5 seconds before // trying to ping an extruder. This gives the extruder time to boot @@ -58,6 +59,7 @@ int main() { Motherboard& board = Motherboard::getBoard(); steppers::init(Motherboard::getBoard()); + planner::init(); reset(true); sei(); while (1) { diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc new file mode 100644 index 0000000..5995d40 --- /dev/null +++ b/firmware/src/Motherboard/Planner.cc @@ -0,0 +1,920 @@ +/* + * Copyright 2011 by Rob Giseburt http://tinkerin.gs + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see + */ + +/* + * This is heavily influenced by the Marlin RepRap firmware + * (https://github.com/ErikZalm/Marlin) which is derived from + * the Grbl firmware (https://github.com/simen/grbl/tree). + */ + + +/* + Reasoning behind the mathematics in this module (in the key of 'Mathematica'): + + s == speed, a == acceleration, t == time, d == distance + + Basic definitions: + + Speed[s_, a_, t_] := s + (a*t) + Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t] + + Distance to reach a specific speed with a constant acceleration: + + Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t] + d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance() + + Speed after a given distance of travel with constant acceleration: + + Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t] + m -> Sqrt[2 a d + s^2] + + DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2] + + When to start braking (di) to reach a specified destionation speed (s2) after accelerating + from initial speed s1 without ever stopping at a plateau: + + Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di] + di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance() + + IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) + + vt + (1/2)at^2=X for t + x + vt + (1/2)at^2=X + x = destination + X = position + v = current speed + a = acceleration rate + t = time + See: http://www.wolframalpha.com/input/?i=vt+%2B+%281%2F2%29at%5E2%3DX+for+t + + Solved for time, simplified (with a few multiplications as possible) gives: + (sqrt(v^2-2*a*(x-X)) - v)/a + So, making x-X = D gives: + (sqrt(v*v-2*a*D)-v)/a = time to accelerate from velocity v over D steps with acceleration a + +*/ + + +#include "Planner.hh" +#include +#include +#include +#include // for memmove and memcpy + +#include "Steppers.hh" +#include "Point.hh" + +// Give the processor some time to breathe and plan... +#define MIN_MS_PER_SEGMENT 12000 + +#define X_AXIS 0 +#define Y_AXIS 1 +#define Z_AXIS 2 +#define A_AXIS 3 +#define B_AXIS 4 + +#define FORCE_INLINE __attribute__((always_inline)) inline + +/* Setup some utilities */ + +// undefine stdlib's abs if encountered +#ifdef abs +#undef abs +#endif +#ifdef min +#undef min +#endif +#ifdef max +#undef max +#endif + +template +inline const T& min(const T& a, const T& b) { return (a)<(b)?(a):(b); } + +template +inline const T& max(const T& a, const T& b) { return (a)>(b)?(a):(b); } + +// undefine stdlib's abs if encountered +#ifdef abs +#undef abs +#endif + +#ifdef labs +#undef labs +#endif + +template +inline T abs(T x) { return (x)>0?(x):-(x); } + +template <> +inline int abs(int x) { return __builtin_abs(x); } + +template <> +inline long abs(long x) { return __builtin_labs(x); } + + +namespace planner { + + // Pin stepperTimingDebugPin = STEPPER_TIMER_DEBUG; + + // Super-simple circular buffer, where old nodes are reused + // TODO: Move to a seperate file + // WARNING WARNING WARNING: If the size of this buffer is not in the following list this WILL FAIL BADLY! + // (2, 4, 8, 16, 32, 64, 128) + template + class ReusingCircularBufferTempl + { + public: + typedef T BufDataType; + typedef uint8_t BufSizeType; + + private: + volatile BufSizeType head, tail; + volatile bool full; + BufSizeType size; + BufSizeType size_mask; + BufDataType* const data; /// Pointer to buffer data + + public: + ReusingCircularBufferTempl(BufSizeType size_in, BufDataType* buffer_in) : head(0), tail(0), full(false), size(size_in), size_mask(size_in-1), data(buffer_in) { + for (BufSizeType i = 0; i < size; i++) { + data[i] = BufDataType(); + } + }; + + inline BufDataType *getHead() { + return &data[head]; + } + inline BufSizeType getHeadIndex() { + return head; + } + + inline BufDataType *getTail() { + return &data[tail]; + } + inline BufSizeType getTailIndex() { + return tail; + } + + inline BufSizeType getNextIndex(BufSizeType from) { + return ((from + 1) & size_mask); + } + + inline BufSizeType getPreviousIndex(BufSizeType from) { + return (((from+size) - 1) & size_mask); + } + + inline BufDataType *getNextHead() { + return &data[getNextIndex(head)]; + } + + inline BufDataType &operator[] (BufSizeType index) { + // adding size should make negative indexes < size work ok + // int16_t offset = index < 0 ? index : ((index + size) & size_mask); + return data[index]; + } + + // bump the head. cannot return anything useful, so it doesn't + // WARNING: no sanity checks! + inline void bumpHead() { + head = getNextIndex(head); + if (getNextIndex(head) == tail) + full = true; + } + + // bump the tail. cannot return anything useful, so it doesn't + // WARNING: no sanity checks! + inline void bumpTail() { + tail = getNextIndex(tail); + full = false; + } + + inline bool isEmpty() { + return !full && head == tail; + } + + inline bool isFull() { + return full; + } + + inline BufSizeType getUsedCount() { + return full ? size : ((head-tail+size) & size_mask); + } + + inline void clear() { + head = 0; + tail = 0; + full = false; + } + }; + + // this is very similar to the StepperAxis, but geared toward planning + struct PlannerAxis + { + // how many steps does it take to go a mm (RepG should tell us this during init) + float steps_per_mm; + + // how fast can we go, in mm/s (RepG should have already limited this, disabling) + // float max_feedrate; + + // maximum acceleration for this axis in steps/s^2 (should be in EEPROM) + uint32_t max_acceleration; + + // the maximum amount of speed change allowable for this axis + // note that X+Y has it's own setting, and this if for all the rest + float max_axis_jerk; + }; + + PlannerAxis axes[STEPPER_COUNT]; + + float default_acceleration; + float minimum_planner_speed; + Point position; // the current position (planning-wise, not bot/stepper-wise) in steps + float previous_speed[STEPPER_COUNT]; // Speed of previous path line segment +#ifdef CENTREPEDAL + float default_junction_deviation; + float previous_unit_vec[3]; +#endif + float previous_nominal_speed; // Nominal speed of previous path line segment + static float max_xy_jerk; + + Block block_buffer_data[BLOCK_BUFFER_SIZE]; + ReusingCircularBufferTempl block_buffer(BLOCK_BUFFER_SIZE, block_buffer_data); + + // let's get verbose + volatile bool is_planning_and_using_prev_speed = false; + + void init() + { + abort(); + + // stepperTimingDebugPin.setDirection(true); + // stepperTimingDebugPin.setValue(false); + +#ifdef CENTREPEDAL + previous_unit_vec[0]= 0.0; + previous_unit_vec[1]= 0.0; + previous_unit_vec[2]= 0.0; +#endif + } + + + void setMaxAxisJerk(float jerk, uint8_t axis) { + if (axis < STEPPER_COUNT) + axes[axis].max_axis_jerk = jerk; + } + + void setMaxXYJerk(float jerk) { + max_xy_jerk = jerk; + } + + void setAxisStepsPerMM(float steps_per_mm, uint8_t axis) { + if (axis < STEPPER_COUNT) + axes[axis].steps_per_mm = steps_per_mm; + } + + void setAcceleration(int32_t new_acceleration) { + default_acceleration = (float)new_acceleration; + } + + // This is in steps/mm. + void setAxisAcceleration(int32_t new_acceleration, uint8_t axis) { + if (axis < STEPPER_COUNT) + axes[axis].max_acceleration = (float)new_acceleration*axes[axis].steps_per_mm; + } + + void setMinimumPlannerSpeed(float speed) { + minimum_planner_speed = speed; + } + +#ifdef CENTREPEDAL + void setJunctionDeviation(float new_junction_deviation) { + default_junction_deviation = new_junction_deviation; + } +#endif + + // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the + // acceleration within the allotted distance. + // Needs to be conbverted to fixed-point. + FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) { + return sqrt((target_velocity*target_velocity)-(acceleration*2.0)*distance); + } + + // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the + // given acceleration: + FORCE_INLINE int32_t estimate_acceleration_distance(int32_t initial_rate_squared, int32_t target_rate_squared, int32_t acceleration_doubled) + { + if (acceleration_doubled!=0) { + return ((target_rate_squared-initial_rate_squared)/acceleration_doubled); + } + else { + return 0; // acceleration was 0, set acceleration distance to 0 + } + } + + // This function gives you the point at which you must start braking (at the rate of -acceleration) if + // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after + // a total travel of distance. This can be used to compute the intersection point between acceleration and + // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) + + FORCE_INLINE int32_t intersection_distance(int32_t initial_rate_squared, int32_t final_rate_squared, int32_t acceleration_mangled, int32_t acceleration_quadrupled, int32_t distance) + { + if (acceleration_quadrupled!=0) { + return ((acceleration_mangled*distance-initial_rate_squared+final_rate_squared)/acceleration_quadrupled); + } + else { + return 0; // acceleration was 0, set intersection distance to 0 + } + } + +// Disabled because it's not used, but if it is in the future, here's how +#if 0 + // Calculates the time (not distance) in microseconds (S*1,000,000) it takes to go from initial_rate for distance at acceleration rate + FORCE_INLINE uint32_t estimate_time_to_accelerate(float initial_rate, float acceleration, float distance) { + /* + if (acceleration!=0.0 && initial_rate == 0.0) { + return (sqrt(-2*acceleration*distance)/acceleration) * 1000000; + } else */ + if (acceleration!=0.0) { + return abs((sqrt(2*acceleration*distance + initial_rate*initial_rate)-initial_rate)/acceleration) * 1000000; + } + else { + return (distance/initial_rate) * 1000000; // no acceleration is just distance/rate + } + } +#endif + + // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. + // calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, exit_factor_speed/block->nominal_speed); + void Block::calculate_trapezoid(float exit_factor_speed) { + // stepperTimingDebugPin.setValue(true); + + float entry_factor = entry_speed/nominal_speed; + float exit_factor = exit_factor_speed/nominal_speed; + + uint32_t local_initial_rate = ceil((float)nominal_rate*entry_factor); // (step/min) + uint32_t local_final_rate = ceil((float)nominal_rate*exit_factor); // (step/min) + + // Limit minimal step rate (Otherwise the timer will overflow.) + if(local_initial_rate < 120) + local_initial_rate = 120; + if(local_final_rate < 120) + local_final_rate = 120; + + int32_t local_initial_rate_squared = (local_initial_rate * local_initial_rate); + int32_t local_final_rate_squared = (local_final_rate * local_final_rate); + int32_t nominal_rate_squared = (nominal_rate * nominal_rate); + + int32_t local_acceleration_doubled = acceleration_st<<(1); // == acceleration_st*2 + + int32_t accelerate_steps = + /*ceil*/(estimate_acceleration_distance(local_initial_rate_squared, nominal_rate_squared, local_acceleration_doubled)); + int32_t decelerate_steps = + /*floor*/(estimate_acceleration_distance(nominal_rate_squared, local_final_rate_squared, -local_acceleration_doubled)); + + // Calculate the size of Plateau of Nominal Rate. + int32_t plateau_steps = step_event_count-accelerate_steps-decelerate_steps; + + // 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 local_final_rate exactly at the end of this block. + if (plateau_steps < 0) { + + // To get the math right when shifting, we need to alter the first acceleration_doubled by bit_shift_amount^2, and un-bit_shift_amount^2 after + int32_t local_acceleration_quadrupled = local_acceleration_doubled<<(1); // == acceleration_st*2 + accelerate_steps = /*ceil*/( + intersection_distance(local_initial_rate_squared, local_final_rate_squared, local_acceleration_doubled, local_acceleration_quadrupled, step_event_count)); + accelerate_steps = max(accelerate_steps, 0L); // Check limits due to numerical round-off + accelerate_steps = min(accelerate_steps, (int32_t)step_event_count); + plateau_steps = 0; + } + + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { // Fill variables used by the stepper in a critical section + // if(!(flags & Block::Busy)) { + accelerate_until = accelerate_steps; + decelerate_after = accelerate_steps+plateau_steps; + initial_rate = local_initial_rate; + final_rate = local_final_rate; + // } + if(flags & Block::Busy) + steppers::currentBlockChanged(); + } // ISR state will be automatically restored here + + // stepperTimingDebugPin.setValue(false); + } + + // forward declare, so we can order the code in a slightly more readable fashion + void planner_reverse_pass_kernel(Block *previous, Block *current, Block *next); + void planner_reverse_pass(); + void planner_forward_pass_kernel(Block *previous, Block *current, Block *next); + void planner_forward_pass(); + void planner_recalculate_trapezoids(); + + // Recalculates the motion plan according to the following algorithm: + // + // 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed) + // so that: + // a. The junction speed is equal to or less than the maximum junction speed limit + // b. No speed reduction within one block requires faster deceleration than the one, true constant + // acceleration. + // 2. Go over every block in chronological order and dial down junction speed values if + // a. The speed increase within one block would require faster acceleration than the one, true + // constant acceleration. + // + // When these stages are complete all blocks have an entry speed that will allow all speed changes to + // be performed using only the one, true constant acceleration, and where no junction speed is greater + // than the max limit. Finally it will: + // + // 3. Recalculate trapezoids for all blocks using the recently updated junction speeds. Block trapezoids + // with no updated junction speeds will not be recalculated and assumed ok as is. + // + // All planner computations are performed with doubles (float on Arduinos) to minimize numerical round- + // off errors. Only when planned values are converted to stepper rate parameters, these are integers. + + void planner_recalculate() { + planner_reverse_pass(); + planner_forward_pass(); + planner_recalculate_trapezoids(); + } + + // The kernel called by planner_recalculate() when scanning the plan from last to first entry. + void planner_reverse_pass_kernel(Block *previous, Block *current, Block *next) { + if(!current) { return; } + + if (next) { + // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. + // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and + // check for maximum allowable speed reductions to ensure maximum possible planned speed. + if (current->entry_speed != current->max_entry_speed) { + // If nominal length true, max junction speed is guaranteed to be reached. Only compute + // for max allowable speed if block is decelerating and nominal length is false. + if ((!(current->flags & Block::NominalLength)) && (current->max_entry_speed == next->entry_speed)) { + current->entry_speed = min( current->max_entry_speed, + max_allowable_speed(-current->acceleration,next->entry_speed,current->millimeters)); + } else { + current->entry_speed = current->max_entry_speed; + } + current->flags |= Block::Recalculate; + } + } + } + + // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This + // implements the reverse pass. + void planner_reverse_pass() { + if(block_buffer.getUsedCount() > 3) { + uint8_t block_index = block_buffer.getHeadIndex(); + Block *block[3] = { NULL, NULL, NULL }; + while(block_index != block_buffer.getTailIndex()) { + block_index = block_buffer.getPreviousIndex(block_index); + block[2] = block[1]; + block[1] = block[0]; + // Move two blocks worth of ram, from [0] to [1], using the overlap-safe memmove + //memmove(block[0], block[1], sizeof(Block)<<1); + block[0] = &block_buffer[block_index]; + planner_reverse_pass_kernel(block[0], block[1], block[2]); + } + planner_reverse_pass_kernel(NULL, block[0], block[1]); + } + } + + // The kernel called by planner_recalculate() when scanning the plan from first to last entry. + void planner_forward_pass_kernel(Block *previous, Block *current, Block *next) { + if(!previous) { return; } + + // If the previous block is busy, then we're currently executing it! + // We have to be careful here, but we want to try to smooth out the movement if it's not too late. + // That smoothing will happen in Block::calculate_trapezoid later. + // However, if it *is* too late, then we need to fix the current entry speed. + if (previous->flags & Block::Busy && current->flags & Block::Recalculate) { + // stepperTimingDebugPin.setValue(true); + uint32_t current_step = steppers::getCurrentStep(); + uint32_t current_feedrate = steppers::getCurrentFeedrate(); + // current_feedrate is in steps/second, but entry_speed is in mm/s + // use the ratio of nominal_speed/nominal_rate to figure the current speed + float current_speed = ((float)current_feedrate * previous->nominal_speed)/(float)previous->nominal_rate; + + // adjust the previous block to just cover the space left, and firect recalculation + previous->entry_speed = previous->max_entry_speed = current_speed; + + // Recalculate the length of the movement -- for acceleration only. + // The Stepper/Axis objects have track of actual movement length by now. + previous->step_event_count = previous->step_event_count - current_step; + previous->flags |= Block::Recalculate; + // assume it's not nominal length, to be safe + previous->flags &= ~Block::NominalLength; + // stepperTimingDebugPin.setValue(false); + } + + // If the previous block is an acceleration block, but it is not long enough to complete the + // full speed change within the block, we need to adjust the entry speed accordingly. Entry + // speeds have already been reset, maximized, and reverse planned by reverse planner. + // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. + if (!(previous->flags & Block::NominalLength)) { + if (previous->entry_speed == current->entry_speed) { + float entry_speed = min( current->entry_speed, + max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) ); + + // Check for junction speed change + if (current->entry_speed != entry_speed) { + current->entry_speed = entry_speed; + current->flags |= Block::Recalculate; + } + } + } + } + + // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This + // implements the forward pass. + void planner_forward_pass() { + uint8_t block_index = block_buffer.getTailIndex(); + Block *block[3] = { NULL, NULL, NULL }; + + while(block_index != block_buffer.getHeadIndex()) { + block[0] = block[1]; + block[1] = block[2]; + // Move two blocks worth of ram, from [1] to [0], using the overlap-safe memmove + //memmove(block[1], block[0], sizeof(Block)<<1); + block[2] = &block_buffer[block_index]; + planner_forward_pass_kernel(block[0],block[1],block[2]); + block_index = block_buffer.getNextIndex(block_index); + } + planner_forward_pass_kernel(block[1], block[2], NULL); + } + + // Recalculates the trapezoid speed profiles for all blocks in the plan according to the + // entry_factor for each junction. Must be called by planner_recalculate() after + // updating the blocks. + void planner_recalculate_trapezoids() { + int8_t block_index = block_buffer.getTailIndex(); + Block *current; + Block *next = NULL; + + while(block_index != block_buffer.getHeadIndex()) { + current = next; + next = &block_buffer[block_index]; + if (current) { + // Recalculate if current block entry or exit junction speed has changed. + if ((current->flags & Block::Recalculate) || (next->flags & Block::Recalculate)) { + // NOTE: Entry and exit factors always > 0 by all previous logic operations. + current->calculate_trapezoid(next->entry_speed); + current->flags &= ~Block::Recalculate; // Reset current only to ensure next trapezoid is computed + } + } + block_index = block_buffer.getNextIndex( block_index ); + } + + // Last/newest block in buffer. Exit speed is set with minimum_planner_speed. Always recalculated. + next->calculate_trapezoid(minimum_planner_speed); + next->flags &= ~Block::Recalculate; + } + + bool isBufferFull() { + return block_buffer.isFull(); + } + + bool isBufferEmpty() { + bool is_buffer_empty = block_buffer.isEmpty(); + + // if we buffer underrun, we need to make sure the planner starts from "stopped" + if (is_buffer_empty && !is_planning_and_using_prev_speed) { + for (int i = 0; i < STEPPER_COUNT; i++) { + previous_speed[i] = 0.0; + } + previous_nominal_speed = 0.0; + } + return is_buffer_empty; + } + + Block *getNextBlock() { + Block *block = block_buffer.getTail(); + return block; + } + + void doneWithNextBlock() { + block_buffer.bumpTail(); + } + + bool addMoveToBufferRelative(const Point& move, const int32_t ms, const int8_t relative) { + Point target; + int32_t max_delta = 0; + for (int i = 0; i < STEPPER_COUNT; i++) { + int32_t delta = 0; + if ((relative & (1 << i))) { + target[i] = position[i] + move[i]; + delta = abs(move[i]); + } else { + target[i] = move[i]; + delta = abs(target[i] - position[i]); + + } + if (delta > max_delta) { + max_delta = delta; + } + } + + return addMoveToBuffer(target, ms/max_delta); + } + + + // Buffer the move. IOW, add a new block, and recalculate the acceleration accordingly + bool addMoveToBuffer(const Point& target, int32_t us_per_step) + { + // stepperTimingDebugPin.setValue(true); + if (block_buffer.isFull()) { + // stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(false); + return false; + // stepperTimingDebugPin.setValue(false); + } + + Block *block = block_buffer.getHead(); + // Mark block as not busy (Not executed by the stepper interrupt) + block->flags = 0; + + block->target = target; + + // // store the absolute number of steps in each direction, without direction + Point steps = (target - position); + + float delta_mm[STEPPER_COUNT]; + block->millimeters = 0.0; + block->step_event_count = 0; + // // Compute direction bits for this block -- UNUSED FOR NOW + // block->direction_bits = 0; + for (int i = 0; i < STEPPER_COUNT; i++) { + int32_t abs_steps = abs(steps[i]); + if (abs_steps > block->step_event_count) { + block->step_event_count = abs_steps; + } + delta_mm[i] = ((float)steps[i])/axes[i].steps_per_mm; + if (i < A_AXIS || block->millimeters == 0) // cound distznce of A and B only if X, Y, and Z don't move + block->millimeters += delta_mm[i] * delta_mm[i]; + // if (target[i] < position[i]) { block->direction_bits |= (1<millimeters = sqrt(block->millimeters); + + if (block->step_event_count == 0) + return false; + + // CLEAN ME: Ugly dirty check to prevent a lot of small moves from causing a planner buffer underrun + // For now, we'll just make sure each movement takes at least MIN_MS_PER_SEGMENT millisesconds to complete + if ((us_per_step * block->step_event_count) < MIN_MS_PER_SEGMENT) { + us_per_step = MIN_MS_PER_SEGMENT / block->step_event_count; + } + + float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides + // Calculate 1 second/(seconds for this movement) + float inverse_second = 1000000.0/(float)(us_per_step * block->step_event_count); + float steps_per_mm = (float)block->step_event_count * inverse_millimeters; + + // we are given microseconds/step, and we need steps/mm, and steps/second + + // Calculate speed in steps/sec + uint32_t steps_per_second = 1000000/us_per_step; + float mm_per_second = block->millimeters * inverse_second; + + // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. + block->nominal_speed = mm_per_second; // (mm/sec) Always > 0 + block->nominal_rate = steps_per_second; // (step/sec) Always > 0 + + // TODO make sure we are going the minimum speed, at least + // if(feed_rate max_feedrate[i]) + // speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i])); + // } + + // TODO fancy frequency checks + + + // // Correct the speed + // if( speed_factor < 1.0) { + // // Serial.print("speed factor : "); Serial.println(speed_factor); + // for(int i=0; i < 4; i++) { + // if(fabs(current_speed[i]) > max_feedrate[i]) + // speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i])); + // /* + // if(speed_factor < 0.1) { + // Serial.print("speed factor : "); Serial.println(speed_factor); + // Serial.print("current_speed"); Serial.print(i); Serial.print(" : "); Serial.println(current_speed[i]); + // } + // */ + // } + // for(unsigned char i=0; i < 4; i++) { + // current_speed[i] *= speed_factor; + // } + // block->nominal_speed *= speed_factor; + // block->nominal_rate *= speed_factor; + // } + + // Compute and limit the acceleration rate for the trapezoid generator. + block->acceleration_st = ceil(default_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 + // Limit acceleration per axis + for(int i=0; i < STEPPER_COUNT; i++) { + // warning: arithmetic overflow is easy here. Try to mitigate. + float step_scale = (float)abs(steps[i]) / (float)block->step_event_count; + float axis_acceleration_st = (float)block->acceleration_st * step_scale; + if((uint32_t)axis_acceleration_st > axes[i].max_acceleration) + block->acceleration_st = axes[i].max_acceleration; + } + block->acceleration = block->acceleration_st / steps_per_mm; + block->acceleration_rate = block->acceleration_st / ACCELERATION_TICKS_PER_SECOND; + +#ifndef CENTREPEDAL + // Compute the speed trasitions, or "jerks" + // Start with a safe speed + float vmax_junction = minimum_planner_speed; + + // block clearing of previous_speed + is_planning_and_using_prev_speed = true; + + // Now determine the safe max entry speed for this move + // Skip the first block + if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { + + float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); + if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { + vmax_junction = block->nominal_speed; + } + + if (jerk > max_xy_jerk) { + vmax_junction *= (max_xy_jerk/jerk); + } + + for (int i_axis = Z_AXIS; i_axis < STEPPER_COUNT; i_axis++) { + jerk = abs(previous_speed[i_axis] - current_speed[i_axis]); + if (jerk > axes[i_axis].max_axis_jerk) { + vmax_junction *= (axes[i_axis].max_axis_jerk/jerk); + } + } + } + +#else // CENTREPEDAL + + // NEEDS MORE TESTING + + // Compute path unit vector + float unit_vec[3]; + + unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; + unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; + unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; + + // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + // Let a circle be tangent to both previous and current path line segments, where the junction + // deviation is defined as the distance from the junction to the closest edge of the circle, + // colinear with the circle center. The circular segment joining the two paths represents the + // path of centripetal acceleration. Solve for max velocity based on max acceleration about the + // radius of the circle, defined indirectly by junction deviation. This may be also viewed as + // path width or max_jerk in the previous grbl version. This approach does not actually deviate + // from path, but used as a robust way to compute cornering speeds, as it takes into account the + // nonlinearities of both the junction angle and junction velocity. + float vmax_junction = minimum_planner_speed; // Set default max junction speed + + // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. + if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { + // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) + // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. + float cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; + + // Skip and use default max junction speed for 0 degree acute junction. + if (cos_theta < 0.95) { + vmax_junction = min(previous_nominal_speed,block->nominal_speed); + // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. + if (cos_theta > -0.95) { + // Compute maximum junction velocity based on maximum acceleration and junction deviation + float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. + vmax_junction = min(vmax_junction, + (float)sqrt(default_acceleration * default_junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); + } + } + + // block clearing of previous_speed + is_planning_and_using_prev_speed = true; + for (int i_axis = Z_AXIS; i_axis < STEPPER_COUNT; i_axis++) { + float jerk = abs(previous_speed[i_axis] - current_speed[i_axis]); + if (jerk > axes[i_axis].max_axis_jerk) { + vmax_junction *= (axes[i_axis].max_axis_jerk/jerk); + } + } + } + + // Update previous path unit_vector and nominal speed + memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] +#endif + block->max_entry_speed = vmax_junction; + + // Initialize block entry speed. Compute based on deceleration to user-defined minimum_planner_speed. + float v_allowable = max_allowable_speed(-block->acceleration, minimum_planner_speed, block->millimeters); + block->entry_speed = min(vmax_junction, v_allowable); + + // Initialize planner efficiency flags + // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. + // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then + // the current block and next block junction speeds are guaranteed to always be at their maximum + // junction speeds in deceleration and acceleration, respectively. This is due to how the current + // block nominal speed limits both the current and next maximum junction speeds. Hence, in both + // the reverse and forward planners, the corresponding block junction speed will always be at the + // the maximum junction speed and may always be ignored for any speed reduction checks. + if (block->nominal_speed <= v_allowable) + block->flags |= Block::NominalLength; + else + block->flags &= ~Block::NominalLength; + block->flags |= Block::Recalculate; // Always calculate trapezoid for new block + + // Update previous path speed and nominal speed + memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] + previous_nominal_speed = block->nominal_speed; + + // allow clearing of previous speed again + is_planning_and_using_prev_speed = false; + + // Update position + position = target; + + // Move buffer head + block_buffer.bumpHead(); + + planner_recalculate(); + + steppers::startRunning(); + + // stepperTimingDebugPin.setValue(false); + return true; + } + + void startHoming(const bool maximums, + const uint8_t axes_enabled, + const uint32_t us_per_step) + { + // STUB + } + + void abort() { + steppers::abort(); + position = steppers::getPosition(); + + // reset speed + for (int i = 0; i < STEPPER_COUNT; i++) { + previous_speed[i] = 0.0; + } + previous_nominal_speed = 0.0; + + block_buffer.clear(); + +#ifdef CENTREPEDAL + previous_unit_vec[0]= 0.0; + previous_unit_vec[1]= 0.0; + previous_unit_vec[2]= 0.0; +#endif + } + + void definePosition(const Point& new_position) + { + position = new_position; + steppers::definePosition(new_position); + + // reset speed + for (int i = 0; i < STEPPER_COUNT; i++) { + previous_speed[i] = 0.0; + } + previous_nominal_speed = 0.0; + +#ifdef CENTREPEDAL + previous_unit_vec[0]= 0.0; + previous_unit_vec[1]= 0.0; + previous_unit_vec[2]= 0.0; +#endif + } + + const Point getPosition() + { + return position; + } +} \ No newline at end of file diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh new file mode 100644 index 0000000..9b53f4a --- /dev/null +++ b/firmware/src/Motherboard/Planner.hh @@ -0,0 +1,140 @@ +/* + * Copyright 2011 by Rob Giseburt http://tinkerin.gs + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see + */ + +/* + * This is heavily influenced by the Marlin RepRap firmware + * (https://github.com/ErikZalm/Marlin) which is derived from + * the Grbl firmware (https://github.com/simen/grbl/tree). + */ + +/* In this implenmentation, the motor control is handled by steppers, but this code does the planning. */ + +#ifndef PLANNER_HH +#define PLANNER_HH + +#include "Configuration.hh" +#include +#include "Point.hh" + +namespace planner { + // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in + // the source g-code and may never actually be reached if acceleration management is active. + class Block { + public: + typedef enum { + Busy = 1<<0, + Recalculate = 1<<1, + NominalLength = 1<<2, + } PlannerFlags; + + // Fields used by the bresenham algorithm for tracing the line + Point target; // Final 5-axis target + uint32_t step_event_count; // The number of step events required to complete this block + uint32_t accelerate_until; // The index of the step event on which to stop acceleration + uint32_t decelerate_after; // The index of the step event on which to start decelerating + uint32_t acceleration_rate; // The acceleration rate used for acceleration calculation + // uint8_t direction_bits; // The direction bit set for this block + // uint8_t active_extruder; // Selects the active extruder + + + // Fields used by the motion planner to manage acceleration + // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis + float nominal_speed; // The nominal speed for this block in mm/min + float entry_speed; // Entry speed at previous-current junction in mm/min + float max_entry_speed; // Maximum allowable junction entry speed in mm/min + float millimeters; // The total travel of this block in mm + float acceleration; // acceleration mm/sec^2 + // float stop_speed; // Speed to decelerate to if this is the last move + // uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction + // uint8_t nominal_length_flag; // Planner flag for nominal speed always reached + + // Settings for the trapezoid generator + uint32_t nominal_rate; // The nominal step rate for this block in step_events/sec + uint32_t initial_rate; // The jerk-adjusted step rate at start of block + uint32_t final_rate; // The minimal rate at exit + uint32_t acceleration_st; // acceleration steps/sec^2 + // uint8_t busy; + uint8_t flags; + + Block() : target() {}; + + // functions + void calculate_trapezoid(float exit_factor_speed); + }; + + /// Initilaize the planner data structures + void init(); + + /// Buffer a movement to the target point (in step-space), with us_per_step gaps between steps + /// \param[in] target New position to move to, in step-space + /// \param[in] us_per_step Homing speed, in us per step + /// \return If the move was buffered + bool addMoveToBuffer(const Point& target, int32_t us_per_step); + + /// Buffer a movement to the target point (in step-space). We should avoid this, as it requires more calculation. + /// \param[in] target New position to move to, in step-space + /// \param[in] ms Duration of the move, in milliseconds + /// \param[in] relative Bitfield specifying whether each axis should + /// interpret the new position as absolute or + /// relative. + /// \return If the move was buffered + bool addMoveToBufferRelative(const Point& target, const int32_t ms, const int8_t relative); + + /// Home one or more axes + /// \param[in] maximums If true, home in the positive direction + /// \param[in] axes_enabled Bitfield specifiying which axes to + /// home + /// \param[in] us_per_step Homing speed, in us per step + void startHoming(const bool maximums, + const uint8_t axes_enabled, + const uint32_t us_per_step); + + /// Reset the current system position to the given point + /// \param[in] position New system position + void definePosition(const Point& position); + + /// Abort the current motion (and all planeed movments) and set the stepper subsystem to + /// the not-running state. + void abort(); + + /// Get the current system position + /// \return The current machine position. + const Point getPosition(); + + void setMaxXYJerk(float jerk); + void setMaxAxisJerk(float jerk, uint8_t axis); + + void setMinimumPlannerSpeed(float speed); + + void setAcceleration(int32_t acceleration); + void setAxisAcceleration(int32_t new_acceleration, uint8_t axis); +#ifdef CENTREPEDAL + void setJunctionDeviation(float new_junction_deviation); +#endif + void setAxisStepsPerMM(float steps_per_mm, uint8_t axis); + + bool isBufferFull(); + bool isBufferEmpty(); + + // Fetches the *tail* + Block *getNextBlock(); + + // pushes the tail forward, making it available + void doneWithNextBlock(); +} + +#endif /* end of include guard: PLANNER_HH */ diff --git a/firmware/src/Motherboard/Point.cc b/firmware/src/Motherboard/Point.cc index 2184103..c33a130 100644 --- a/firmware/src/Motherboard/Point.cc +++ b/firmware/src/Motherboard/Point.cc @@ -1,33 +1,111 @@ #include "Point.hh" +#include // for labs() + +/* Setup some utilities -- TODO: Move this to a common file */ + +// undefine stdlib's abs if encountered +#ifdef abs +#undef abs +#endif + +#ifdef labs +#undef labs +#endif + +template +inline T abs(T x) { return (x)>0?(x):-(x); } + +template <> +inline int abs(int x) { return __builtin_abs(x); } + +template <> +inline long abs(long x) { return __builtin_labs(x); } + + +/* end utilities */ Point::Point() { + // coordinates[0] = 0; + // coordinates[1] = 0; + // coordinates[2] = 0; + // coordinates[3] = 0; + // coordinates[4] = 0; } -Point::Point(int32_t x, int32_t y, int32_t z, int32_t a, int32_t b) { - coordinates[0] = x; - coordinates[1] = y; - coordinates[2] = z; +// Point::Point(const Point &other) +// { +// coordinates[0] = other.coordinates[0]; +// coordinates[1] = other.coordinates[1]; +// coordinates[2] = other.coordinates[2]; +// #if AXIS_COUNT > 3 +// coordinates[3] = other.coordinates[3]; +// coordinates[4] = other.coordinates[4]; +// #endif +// } + + +Point::Point(const int32_t x, const int32_t y, const int32_t z, const int32_t a, const int32_t b) { + coordinates[0] = x; + coordinates[1] = y; + coordinates[2] = z; #if AXIS_COUNT > 3 - coordinates[3] = a; - coordinates[4] = b; + coordinates[3] = a; + coordinates[4] = b; #endif } -Point::Point(int32_t x, int32_t y, int32_t z) { - coordinates[0] = x; - coordinates[1] = y; - coordinates[2] = z; +Point::Point(const int32_t x, const int32_t y, const int32_t z) { + coordinates[0] = x; + coordinates[1] = y; + coordinates[2] = z; #if AXIS_COUNT > 3 - coordinates[3] = 0; - coordinates[4] = 0; + coordinates[3] = 0; + coordinates[4] = 0; #endif } -const int32_t& Point::operator[](unsigned int index) const { - return coordinates[index]; +const int32_t& Point::operator[](const unsigned int index) const { + return coordinates[index]; +} + +int32_t& Point::operator[](const unsigned int index) { + return coordinates[index]; } -int32_t& Point::operator[](unsigned int index) { - return coordinates[index]; +/// Subtraction operator, for fast deltas +Point operator- (const Point &a, const Point &b) { + Point c = Point( + a.coordinates[0] - b.coordinates[0], + a.coordinates[1] - b.coordinates[1], + a.coordinates[2] - b.coordinates[2], +#if AXIS_COUNT > 3 + a.coordinates[3] - b.coordinates[3], + a.coordinates[4] - b.coordinates[4] +#endif + ); + return c; } + +// Point &Point::operator=(const Point &other) { +// coordinates[0] = other.coordinates[0]; +// coordinates[1] = other.coordinates[1]; +// coordinates[2] = other.coordinates[2]; +// coordinates[3] = other.coordinates[3]; +// coordinates[4] = other.coordinates[4]; +// return *this; +// } + + +Point Point::abs() { + Point absPoint = Point( + ::abs(coordinates[0]), + ::abs(coordinates[1]), + ::abs(coordinates[2]), +#if AXIS_COUNT > 3 + ::abs(coordinates[3]), + ::abs(coordinates[4]) +#endif + ); + return absPoint; +} \ No newline at end of file diff --git a/firmware/src/Motherboard/Point.hh b/firmware/src/Motherboard/Point.hh index f20a679..21a4d55 100644 --- a/firmware/src/Motherboard/Point.hh +++ b/firmware/src/Motherboard/Point.hh @@ -5,7 +5,11 @@ #include -#define AXIS_COUNT STEPPER_COUNT +#if STEPPER_COUNT > 3 +#define AXIS_COUNT 5 +#else +#define AXIS_COUNT 3 +#endif /// Class that represents an N-dimensional point, where N is the number of /// stepper axes present in the system. Can support 3 or 5 axes. @@ -16,6 +20,9 @@ public: /// Default point constructor Point(); + /// Copy Point constructor + // Point(const Point &other); + /// Construct a point with the given cooridnates. Coordinates are in /// stepper steps. /// \param[in] x X axis position @@ -48,6 +55,15 @@ public: /// \return Reference to the variable containing the axis' position. int32_t& operator[](unsigned int index); + /// Subtraction operator, for fast deltas + friend Point operator- (const Point &a, const Point &b); + // Point & operator= (const Point &other); + + // friend const Point &operator-(const Point &a, const Point &b); + + + /// Absolute value -- convert all point to positive + Point abs(); } __attribute__ ((__packed__)); diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 23707a8..8be613f 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -1,36 +1,60 @@ /* - * Copyright 2010 by Adam Mayer - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see - */ +* Copyright 2010 by Adam Mayer +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program. If not, see +*/ #define __STDC_LIMIT_MACROS #include "Steppers.hh" #include "StepperAxis.hh" +#include "Planner.hh" +#include #include +#include namespace steppers { - volatile bool is_running; int32_t intervals; volatile int32_t intervals_remaining; + +struct feedrate_element { + uint32_t rate; // interval value of the feedrate axis + uint32_t steps; // number of steps of the master axis to change + uint32_t target; +}; +feedrate_element feedrate_elements[3]; +volatile int32_t feedrate_steps_remaining; +volatile int32_t feedrate; +volatile int32_t feedrate_target; // convenient storage to save lookup time +volatile int8_t feedrate_dirty; // indicates if the feedrate_inverted needs recalculated +volatile int32_t feedrate_inverted; +volatile int32_t feedrate_changerate; +volatile int32_t acceleration_tick_counter; +volatile int32_t feedrate_multiplier; // should always be 2^N and > 0 +volatile uint8_t current_feedrate_index; + +volatile int32_t timer_counter; StepperAxis axes[STEPPER_COUNT]; volatile bool is_homing; +// Pin stepperTimingDebugPin = STEPPER_TIMER_DEBUG; + bool holdZ = false; +planner::Block *current_block; + bool isRunning() { return is_running || is_homing; } @@ -38,14 +62,45 @@ bool isRunning() { //public: void init(Motherboard& motherboard) { is_running = false; + is_homing = false; for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i] = StepperAxis(motherboard.getStepperInterface(i)); + axes[i] = StepperAxis(motherboard.getStepperInterface(i)); + } + timer_counter = 0; + + current_block = NULL; + + for (int i = 0; i < 3; i++) { + feedrate_elements[i] = feedrate_element(); + feedrate_elements[i].rate = 0; + feedrate_elements[i].target = 0; + feedrate_elements[i].steps = 0; } + + feedrate_steps_remaining = 0; + feedrate = 0; + feedrate_inverted = 0; + feedrate_dirty = 1; + feedrate_multiplier = 1; + acceleration_tick_counter = 0; + current_feedrate_index = 0; + + // stepperTimingDebugPin.setDirection(true); + // stepperTimingDebugPin.setValue(false); } void abort() { is_running = false; is_homing = false; + timer_counter = 0; + current_block = NULL; + feedrate_steps_remaining = 0; + feedrate = 0; + feedrate_inverted = 0; + feedrate_dirty = 1; + feedrate_multiplier = 1; + acceleration_tick_counter = 0; + current_feedrate_index = 0; } /// Define current position as given point @@ -58,7 +113,11 @@ void definePosition(const Point& position) { /// Get current position const Point getPosition() { #if STEPPER_COUNT > 3 - return Point(axes[0].position,axes[1].position,axes[2].position,axes[3].position,axes[4].position); + #if STEPPER_COUNT > 4 + return Point(axes[0].position,axes[1].position,axes[2].position,axes[3].position,axes[4].position); + #else + return Point(axes[0].position,axes[1].position,axes[2].position,axes[3].position,0); + #endif #else return Point(axes[0].position,axes[1].position,axes[2].position); #endif @@ -68,55 +127,196 @@ void setHoldZ(bool holdZ_in) { holdZ = holdZ_in; } -void setTarget(const Point& target, int32_t dda_interval) { - int32_t max_delta = 0; - for (int i = 0; i < AXIS_COUNT; i++) { +inline void prepareFeedrateIntervals() { + if (current_feedrate_index > 2) + return; + feedrate_steps_remaining = feedrate_elements[current_feedrate_index].steps; + feedrate_changerate = feedrate_elements[current_feedrate_index].rate; + feedrate_target = feedrate_elements[current_feedrate_index].target; + // feedrate_dirty = 1; + // acceleration_tick_counter = 0; +} + +inline void recalcFeedrate() { + if (feedrate == 0) + return; // SHRIEK! + feedrate_inverted = 1000000/feedrate; + + feedrate_dirty = 0; +} + +uint32_t getCurrentStep() { + return intervals - intervals_remaining; +} + +// WARNING: Freezes the current feedrate! +uint32_t getCurrentFeedrate() { + feedrate_changerate = 0; + return feedrate; +} + +// load up the next movment +// WARNING: called from inside the ISR, so get out fast +bool getNextMove() { + // stepperTimingDebugPin.setValue(true); + is_running = false; // this ensures that the interrupt does not .. interrupt us + + if (current_block != NULL) { + current_block->flags &= ~planner::Block::Busy; + planner::doneWithNextBlock(); + current_block = NULL; + } + + if (planner::isBufferEmpty()) { + // stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(false); + return false; + } + + current_block = planner::getNextBlock(); + // Mark block as busy (being executed by the stepper interrupt) + current_block->flags |= planner::Block::Busy; + + Point &target = current_block->target; + + feedrate_multiplier = 1; // setTarget sets the multiplier to one + int32_t max_delta = current_block->step_event_count; + for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].setTarget(target[i], false); const int32_t delta = axes[i].delta; + // Only shut z axis on inactivity if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); else if (delta != 0) axes[i].enableStepper(true); - if (delta > max_delta) { - max_delta = delta; - } + + // if (delta > max_delta) { + // max_delta = delta; + // } + } + + current_feedrate_index = 0; + int feedrate_being_setup = 0; + // setup acceleration + feedrate = 0; + if (current_block->accelerate_until > 0) { + feedrate = current_block->initial_rate; + + feedrate_elements[feedrate_being_setup].steps = current_block->accelerate_until; + feedrate_elements[feedrate_being_setup].rate = current_block->acceleration_rate; + feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; + feedrate_being_setup++; + } + + // setup plateau + if (current_block->decelerate_after > current_block->accelerate_until) { + if (feedrate_being_setup == 0) + feedrate = current_block->nominal_rate; + + feedrate_elements[feedrate_being_setup].steps = current_block->decelerate_after - current_block->accelerate_until; + feedrate_elements[feedrate_being_setup].rate = 0; + feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; + feedrate_being_setup++; + } + + + // setup deceleration + if (current_block->decelerate_after < current_block->step_event_count) { + if (feedrate_being_setup == 0) + feedrate = current_block->nominal_rate; + + // To prevent "falling off the end" we will say we have a "bazillion" steps left... + feedrate_elements[feedrate_being_setup].steps = INT32_MAX; //current_block->step_event_count - current_block->decelerate_after; + feedrate_elements[feedrate_being_setup].rate = -current_block->acceleration_rate; + feedrate_elements[feedrate_being_setup].target = current_block->final_rate; + } else { + // and in case there wasn't a deceleration phase, we'll do the same for whichever phase was last... + feedrate_elements[feedrate_being_setup-1].steps = INT32_MAX; + // We don't setup anything else because we limit to the target speed anyway. } - // compute number of intervals for this move - intervals = ((max_delta * dda_interval) / INTERVAL_IN_MICROSECONDS); + + if (feedrate == 0) { + is_running = false; + return false; + } + + prepareFeedrateIntervals(); + recalcFeedrate(); + acceleration_tick_counter = TICKS_PER_ACCELERATION; + + timer_counter = feedrate_inverted; + + intervals = max_delta; intervals_remaining = intervals; - const int32_t negative_half_interval = -intervals / 2; - for (int i = 0; i < AXIS_COUNT; i++) { + const int32_t negative_half_interval = -(intervals>>1); + for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].counter = negative_half_interval; } is_running = true; + + // stepperTimingDebugPin.setValue(false); + return true; } -void setTargetNew(const Point& target, int32_t us, uint8_t relative) { - for (int i = 0; i < AXIS_COUNT; i++) { - axes[i].setTarget(target[i], (relative & (1 << i)) != 0); - // Only shut z axis on inactivity - const int32_t delta = axes[i].delta; - if (i == 2 && !holdZ) { - axes[i].enableStepper(delta != 0); - } else if (delta != 0) { - axes[i].enableStepper(true); - } +void currentBlockChanged() { + // stepperTimingDebugPin.setValue(true); + // If we are here, then we are moving AND the interrupts are frozen, so get out *fast* + + current_feedrate_index = 0; + int feedrate_being_setup = 0; + // setup acceleration + feedrate = 0; + if (current_block->accelerate_until > 0) { + feedrate = current_block->initial_rate; + + feedrate_elements[feedrate_being_setup].steps = current_block->accelerate_until; + feedrate_elements[feedrate_being_setup].rate = current_block->acceleration_rate; + feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; + feedrate_being_setup++; } - // compute number of intervals for this move - intervals = us / INTERVAL_IN_MICROSECONDS; - intervals_remaining = intervals; - const int32_t negative_half_interval = -intervals / 2; - for (int i = 0; i < AXIS_COUNT; i++) { - axes[i].counter = negative_half_interval; + + // setup plateau + if (current_block->decelerate_after > current_block->accelerate_until) { + if (feedrate_being_setup == 0) + feedrate = current_block->nominal_rate; + + feedrate_elements[feedrate_being_setup].steps = current_block->decelerate_after - current_block->accelerate_until; + feedrate_elements[feedrate_being_setup].rate = 0; + feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; + feedrate_being_setup++; } - is_running = true; + + + // setup deceleration + if (current_block->decelerate_after < current_block->step_event_count) { + if (feedrate_being_setup == 0) + feedrate = current_block->nominal_rate; + + // To prevent "falling off the end" we will say we have a "bazillion" steps left... + feedrate_elements[feedrate_being_setup].steps = INT32_MAX; //current_block->step_event_count - current_block->decelerate_after; + feedrate_elements[feedrate_being_setup].rate = -current_block->acceleration_rate; + feedrate_elements[feedrate_being_setup].target = current_block->final_rate; + } else { + // and in case there wasn't a deceleration phase, we'll do the same for whichever phase was last... + feedrate_elements[feedrate_being_setup-1].steps = INT32_MAX; + // We don't setup anything else because we limit to the target speed anyway. + } + + prepareFeedrateIntervals(); + recalcFeedrate(); + + timer_counter = feedrate_inverted; + + // the steppers themselves haven't changed... + + // stepperTimingDebugPin.setValue(false); } /// Start homing void startHoming(const bool maximums, const uint8_t axes_enabled, const uint32_t us_per_step) { intervals_remaining = INT32_MAX; intervals = us_per_step / INTERVAL_IN_MICROSECONDS; - const int32_t negative_half_interval = -intervals / 2; - for (int i = 0; i < AXIS_COUNT; i++) { + const int32_t negative_half_interval = -(intervals>>1); + for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].counter = negative_half_interval; if ((axes_enabled & (1< 0 && feedrate > feedrate_target) + || (feedrate_changerate < 0 && feedrate < feedrate_target)) { + feedrate_changerate = 0; + feedrate = feedrate_target; + } + + } + + // stepperTimingDebugPin.setValue(false); return is_running; } else if (is_homing) { is_homing = false; @@ -150,9 +404,12 @@ bool doInterrupt() { bool still_homing = axes[i].doHoming(intervals); is_homing = still_homing || is_homing; } + // if we're done, force a sync with the planner + if (!is_homing) + planner::abort(); return is_homing; } return false; } -} +} // namespace steppers diff --git a/firmware/src/Motherboard/Steppers.hh b/firmware/src/Motherboard/Steppers.hh index 6fc5ce5..57dd3e8 100644 --- a/firmware/src/Motherboard/Steppers.hh +++ b/firmware/src/Motherboard/Steppers.hh @@ -47,23 +47,6 @@ namespace steppers { /// \param[in] enable If true, enable the axis. If false, disable. void enableAxis(uint8_t index, bool enable); - /// Instruct the stepper subsystem to move the machine to the - /// given position. - /// \param[in] target Position to move to - /// \param[in] dda_interval Motion speed, in us per step. - void setTarget(const Point& target, int32_t dda_interval); - - /// Instruct the stepper subsystem to move the machine to the - /// given position. - /// \param[in] target Position to move to - /// \param[in] ms Duration of the move, in milliseconds - /// \param[in] relative Bitfield specifying whether each axis should - /// interpret the new position as absolute or - /// relative. - void setTargetNew(const Point& target, - int32_t ms, - uint8_t relative =0); - /// Home one or more axes /// \param[in] maximums If true, home in the positive direction /// \param[in] axes_enabled Bitfield specifiying which axes to @@ -92,6 +75,13 @@ namespace steppers { /// through the entire build. If false, it will be /// disabled when not moving. void setHoldZ(bool holdZ); + + void startRunning(); + + void currentBlockChanged(); + + uint32_t getCurrentStep(); + uint32_t getCurrentFeedrate(); }; #endif // STEPPERS_HH_ diff --git a/firmware/src/Motherboard/boards/mb24/Configuration.hh b/firmware/src/Motherboard/boards/mb24/Configuration.hh index f47eedb..c4e919e 100644 --- a/firmware/src/Motherboard/boards/mb24/Configuration.hh +++ b/firmware/src/Motherboard/boards/mb24/Configuration.hh @@ -28,6 +28,9 @@ /// starvation; leave this at 64uS or greater unless you know what you're doing. #define INTERVAL_IN_MICROSECONDS 128 +#define TICKS_PER_ACCELERATION 5 // lower is better +#define ACCELERATION_TICKS_PER_SECOND (1000000/(INTERVAL_IN_MICROSECONDS*TICKS_PER_ACCELERATION)) + // --- Secure Digital Card configuration --- // NOTE: If SD support is enabled, it is implicitly assumed that the // following pins are connected: @@ -146,6 +149,16 @@ // Define as 1 if debugging packets are honored; 0 if not. #define HONOR_DEBUG_PACKETS 1 +// The number of movements we can plan ahead at a time +// THIS MUst BE A POWER OF 2! 4, 8, 16, 32, you get the idea... +#define BLOCK_BUFFER_SIZE 16 + +#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ +#define DEFAULT_MAX_XY_JERK 20.0 +#define DEFAULT_MAX_Z_JERK 0.2 +#define DEFAULT_MAX_A_JERK 0.2 +#define DEFAULT_MAX_B_JERK 0.2 + #define HAS_INTERFACE_BOARD 1 @@ -178,4 +191,31 @@ #define INTERFACE_BAR_PIN Pin(PortL,0) #define INTERFACE_DEBUG_PIN Pin(PortB,7) + +// The number of movements we can plan ahead at a time +// THIS MUST BE A POWER OF 2! 4, 8, 16, 32, you get the idea... +#define BLOCK_BUFFER_SIZE 16 + +#define DEFAULT_ACCELERATION 2000.0 // mm/s/s +#define DEFAULT_X_ACCELERATION 2000.0 // mm/s/s +#define DEFAULT_Y_ACCELERATION 2000.0 // mm/s/s +#define DEFAULT_Z_ACCELERATION 10.0 // mm/s/s +#define DEFAULT_A_ACCELERATION 200.0 // mm/s/s +#define DEFAULT_B_ACCELERATION 200.0 // mm/s/s + +#define DEFAULT_MAX_XY_JERK 8.0 // ms/s <-- unused if CENTREPEDAL is defined below +#define DEFAULT_MAX_Z_JERK 8.0 // mm/s +#define DEFAULT_MAX_A_JERK 10.0 // mm/s +#define DEFAULT_MAX_B_JERK 10.0 // mm/s + +// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end +// of the buffer and all stops. This should not be much greater than zero and should only be changed +// if unwanted behavior is observed on a user's machine when running at very slow speeds. +#define DEFAULT_MINIMUM_PLANNER_SPEED 4.0 // (mm/sec) + +// define CENTREPEDAL to use centrepedal calucations -- so far I can't get there to work -Rob +#undef CENTREPEDAL +#define DEFAULT_JUNCTION_DEVIATION 0.05 // mm + + #endif // BOARDS_RRMBV12_CONFIGURATION_HH_ diff --git a/firmware/src/Motherboard/boards/mb24/Motherboard.cc b/firmware/src/Motherboard/boards/mb24/Motherboard.cc index f829190..a9624ea 100644 --- a/firmware/src/Motherboard/boards/mb24/Motherboard.cc +++ b/firmware/src/Motherboard/boards/mb24/Motherboard.cc @@ -22,6 +22,7 @@ #include "Motherboard.hh" #include "Configuration.hh" #include "Steppers.hh" +#include "Planner.hh" #include "Command.hh" #include "Interface.hh" #include "Tool.hh" @@ -113,6 +114,50 @@ void Motherboard::reset() { for (int i = 0; i < STEPPER_COUNT; i++) { stepper[i].init(i); } + + + // Defaults are from my cupcake -Rob + //X 94.1397046 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 0, 94.1397046), 0); + //Y 94.1397046 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 4, 94.1397046), 1); + //Z 2560.0 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 8, 2560.0), 2); + //A 100.470957613814818 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+12, 100.470957613814818), 3); + //B 100.470957613814818 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+16, 100.470957613814818), 4); + + + // Master acceleraion + planner::setAcceleration(eeprom::getEeprom32(eeprom::MASTER_ACCELERATION_RATE, DEFAULT_ACCELERATION)); + + + //X -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 0, DEFAULT_X_ACCELERATION), 0); + //Y -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 4, DEFAULT_Y_ACCELERATION), 1); + //Z -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 8, DEFAULT_Z_ACCELERATION), 2); + //A -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+12, DEFAULT_A_ACCELERATION), 3); + //B -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+16, DEFAULT_B_ACCELERATION), 4); + + +#ifdef CENTREPEDAL + // uses the same eeprom address as the X/Y junction jerk~ + planner::setJunctionDeviation(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_JUNCTION_DEVIATION)); +#else + planner::setMaxXYJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_MAX_XY_JERK)); +#endif + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 4, DEFAULT_MAX_Z_JERK), 2); + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 8, DEFAULT_MAX_A_JERK), 3); + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+12, DEFAULT_MAX_B_JERK), 4); + + + planner::setMinimumPlannerSpeed(eeprom::getEepromFixed32(eeprom::MINIMUM_PLANNER_SPEED, DEFAULT_MINIMUM_PLANNER_SPEED)); + // Initialize the host and slave UARTs UART::getHostUART().enable(true); UART::getHostUART().in.reset(); diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index a538829..f6a1ed3 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -26,7 +26,11 @@ // possible time between steps; in practical terms, your time between steps should // be at least eight times this large. Reducing the interval can cause resource // starvation; leave this at 64uS or greater unless you know what you're doing. -#define INTERVAL_IN_MICROSECONDS 64 +#define INTERVAL_IN_MICROSECONDS 256 + +// TICKS_PER_ACCELERATION should be set to that ACCELERATION_TICKS_PER_SECOND is not rounded +#define TICKS_PER_ACCELERATION 5 // lower is better +#define ACCELERATION_TICKS_PER_SECOND (1000000/(INTERVAL_IN_MICROSECONDS*TICKS_PER_ACCELERATION)) // The pin that connects to the /PS_ON pin on the PSU header. This pin switches // on the PSU when pulled low. @@ -140,6 +144,13 @@ #define A_DIR_PIN Pin(PortA,5) // The A stepper enable pin (active low) #define A_ENABLE_PIN Pin(PortA,0) + + // The B stepper step pin (active on rising edge) + #define B_STEP_PIN Pin() + // The B direction pin (forward on logic high) + #define B_DIR_PIN Pin() + // The B stepper enable pin (active low) + #define B_ENABLE_PIN Pin() #endif // FOURTH_STEPPER @@ -147,9 +158,36 @@ // The pin which controls the debug LED (active high) #define DEBUG_PIN Pin(PortB,0) +#define STEPPER_TIMER_DEBUG Pin(PortC,0) // SDA + // By default, debugging packets should be honored; this is made // configurable if we're short on cycles or EEPROM. // Define as 1 if debugging packets are honored; 0 if not. #define HONOR_DEBUG_PACKETS 1 +// The number of movements we can plan ahead at a time +// THIS MUST BE A POWER OF 2! 4, 8, 16, 32, you get the idea... +#define BLOCK_BUFFER_SIZE 16 + +#define DEFAULT_ACCELERATION 2000.0 // mm/s/s +#define DEFAULT_X_ACCELERATION 2000.0 // mm/s/s +#define DEFAULT_Y_ACCELERATION 2000.0 // mm/s/s +#define DEFAULT_Z_ACCELERATION 10.0 // mm/s/s +#define DEFAULT_A_ACCELERATION 200.0 // mm/s/s +#define DEFAULT_B_ACCELERATION 200.0 // mm/s/s + +#define DEFAULT_MAX_XY_JERK 8.0 // ms/s <-- unused if CENTREPEDAL is defined below +#define DEFAULT_MAX_Z_JERK 8.0 // mm/s +#define DEFAULT_MAX_A_JERK 10.0 // mm/s +#define DEFAULT_MAX_B_JERK 10.0 // mm/s + +// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end +// of the buffer and all stops. This should not be much greater than zero and should only be changed +// if unwanted behavior is observed on a user's machine when running at very slow speeds. +#define DEFAULT_MINIMUM_PLANNER_SPEED 4.0 // (mm/sec) + +// define CENTREPEDAL to use centrepedal calucations -- so far I can't get there to work -Rob +#undef CENTREPEDAL +#define DEFAULT_JUNCTION_DEVIATION 0.05 // mm + #endif // BOARDS_RRMBV12_CONFIGURATION_HH_ diff --git a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc index 98fe3bb..ce23973 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc +++ b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc @@ -22,6 +22,7 @@ #include "Motherboard.hh" #include "Configuration.hh" #include "Steppers.hh" +#include "Planner.hh" #include "Command.hh" #include "Eeprom.hh" #include "EepromMap.hh" @@ -101,21 +102,63 @@ void Motherboard::reset() { for (int i = 0; i < STEPPER_COUNT; i++) { stepper[i].init(i); } + + // Defaults are from my cupcake -Rob + //X 94.1397046 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 0, 94.1397046), 0); + //Y 94.1397046 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 4, 94.1397046), 1); + //Z 2560.0 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 8, 2560.0), 2); + //A 100.470957613814818 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+12, 100.470957613814818), 3); + //B 100.470957613814818 + // planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+16, 100.470957613814818), 4); + + + // Master acceleraion + planner::setAcceleration(eeprom::getEeprom32(eeprom::MASTER_ACCELERATION_RATE, DEFAULT_ACCELERATION)); + + + //X -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 0, DEFAULT_X_ACCELERATION), 0); + //Y -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 4, DEFAULT_Y_ACCELERATION), 1); + //Z -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 8, DEFAULT_Z_ACCELERATION), 2); + //A -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+12, DEFAULT_A_ACCELERATION), 3); + //B -- default conservative + // planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+16, DEFAULT_B_ACCELERATION), 4); + + +#ifdef CENTREPEDAL + // uses the same eeprom address as the X/Y junction jerk~ + planner::setJunctionDeviation(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_JUNCTION_DEVIATION)); +#else + planner::setMaxXYJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_MAX_XY_JERK)); +#endif + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 4, DEFAULT_MAX_Z_JERK), 2); + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 8, DEFAULT_MAX_A_JERK), 3); + // planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+12, DEFAULT_MAX_B_JERK), 4); + + planner::setMinimumPlannerSpeed(eeprom::getEepromFixed32(eeprom::MINIMUM_PLANNER_SPEED, DEFAULT_MINIMUM_PLANNER_SPEED)); + // Initialize the host and slave UARTs - UART::getHostUART().enable(true); - UART::getHostUART().in.reset(); + UART::getHostUART().enable(true); + UART::getHostUART().in.reset(); - // TODO: These aren't done on other platforms, are they necessary? - UART::getHostUART().reset(); - UART::getHostUART().out.reset(); + // TODO: These aren't done on other platforms, are they necessary? + UART::getHostUART().reset(); + UART::getHostUART().out.reset(); - UART::getSlaveUART().enable(true); - UART::getSlaveUART().in.reset(); + UART::getSlaveUART().enable(true); + UART::getSlaveUART().in.reset(); - // TODO: These aren't done on other platforms, are they necessary? - UART::getSlaveUART().reset(); - UART::getSlaveUART().out.reset(); + // TODO: These aren't done on other platforms, are they necessary? + UART::getSlaveUART().reset(); + UART::getSlaveUART().out.reset(); // Reset and configure timer 1, the microsecond and stepper diff --git a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.hh b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.hh index a151486..847d6a9 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.hh @@ -39,8 +39,8 @@ private: /// Microseconds since board initialization volatile micros_t micros; - /// Private constructor; use the singleton - Motherboard(const Pin& psu_pin); + /// Private constructor; use the singleton + Motherboard(const Pin& psu_pin); static Motherboard motherboard; public: diff --git a/firmware/src/SConscript.extruder b/firmware/src/SConscript.extruder index 9df8453..0a8abf3 100644 --- a/firmware/src/SConscript.extruder +++ b/firmware/src/SConscript.extruder @@ -88,6 +88,7 @@ include_paths = ['shared', 'Extruder', 'Extruder/boards/%s' % platform, '.'] flags=[ '-DF_CPU='+str(f_cpu), '-DVERSION='+str(version), + '-D__PROG_TYPES_COMPAT__', # see http://www.nongnu.org/avr-libc//changes-1.8.html '-mmcu='+mcu, '-g', '-Os', diff --git a/firmware/src/SConscript.motherboard b/firmware/src/SConscript.motherboard index 039b084..eafc148 100644 --- a/firmware/src/SConscript.motherboard +++ b/firmware/src/SConscript.motherboard @@ -61,8 +61,11 @@ vstr = File('#/current_version.txt').get_contents().strip() vstr = ARGUMENTS.get('version',vstr) version = parse_version(vstr) +subplatform = "" +if (fived == 'true'): + subplatform = "-5d" -target_name = "MB-"+platform+"-v"+str(version//100)+"."+str(version%100) +target_name = "MB-"+platform+subplatform+"-v"+str(version//100)+"."+str(version%100) if (platform == 'rrmbv12'): default_baud = '38400' @@ -102,6 +105,7 @@ include_paths = ['shared', 'Motherboard','Motherboard/boards/%s' % platform, '.' flags=[ '-DF_CPU='+str(f_cpu), '-DVERSION='+str(version), + '-D__PROG_TYPES_COMPAT__', # see http://www.nongnu.org/avr-libc//changes-1.8.html '-mmcu='+mcu, '-g', '-Os', diff --git a/firmware/src/shared/CircularBuffer.hh b/firmware/src/shared/CircularBuffer.hh index a27cd7b..40aae55 100644 --- a/firmware/src/shared/CircularBuffer.hh +++ b/firmware/src/shared/CircularBuffer.hh @@ -34,6 +34,7 @@ public: typedef T BufDataType; private: const BufSizeType size; /// Size of this buffer + const BufSizeType size_mask; /// Mask of the buffer size, in binary, as size-1. volatile BufSizeType length; /// Current length of valid buffer data volatile BufSizeType start; /// Current start point of valid bufffer data BufDataType* const data; /// Pointer to buffer data @@ -41,7 +42,7 @@ private: volatile bool underflow; /// Underflow indicator public: CircularBufferTempl(BufSizeType size_in, BufDataType* data_in) : - size(size_in), length(0), start(0), data(data_in), overflow(false), + size(size_in), size_mask(size-1), length(0), start(0), data(data_in), overflow(false), underflow(false) { } @@ -69,7 +70,7 @@ public: return BufDataType(); } const BufDataType& popped_byte = operator[](0); - start = (start + 1) % size; + start = (start + 1) & size_mask; length--; return popped_byte; } @@ -82,7 +83,7 @@ public: underflow = true; sz = length; } - start = (start + sz) % size; + start = (start + sz) & size_mask; length -= sz; } @@ -102,7 +103,7 @@ public: } /// Read the buffer directly inline BufDataType& operator[](BufSizeType index) { - const BufSizeType actual_index = (index + start) % size; + const BufSizeType actual_index = (index + start) & size_mask; return data[actual_index]; } /// Check the overflow flag diff --git a/firmware/src/shared/Eeprom.cc b/firmware/src/shared/Eeprom.cc index 693beb1..09edffa 100644 --- a/firmware/src/shared/Eeprom.cc +++ b/firmware/src/shared/Eeprom.cc @@ -40,4 +40,16 @@ float getEepromFixed16(const uint16_t location, const float default_value) { return ((float)data[0]) + ((float)data[1])/256.0; } +uint32_t getEeprom32(const uint16_t location, const uint32_t default_value) { + uint32_t data = eeprom_read_dword((const uint32_t*)location); + if (data == 0xffffffff) return default_value; + return data; +} + +float getEepromFixed32(const uint16_t location, const float default_value) { + int32_t data = getEeprom32(location, 0xffffffff); + if (data == 0xffffffff) return default_value; + return ((float)data)/65536.0; +} + } // namespace eeprom diff --git a/firmware/src/shared/Eeprom.hh b/firmware/src/shared/Eeprom.hh index ab3da86..672d997 100644 --- a/firmware/src/shared/Eeprom.hh +++ b/firmware/src/shared/Eeprom.hh @@ -9,7 +9,9 @@ void init(); uint8_t getEeprom8(const uint16_t location, const uint8_t default_value); uint16_t getEeprom16(const uint16_t location, const uint16_t default_value); +uint32_t getEeprom32(const uint16_t location, const uint32_t default_value); float getEepromFixed16(const uint16_t location, const float default_value); +float getEepromFixed32(const uint16_t location, const float default_value); } diff --git a/firmware/src/shared/Menu.cc b/firmware/src/shared/Menu.cc index 397dd19..b4656a2 100644 --- a/firmware/src/shared/Menu.cc +++ b/firmware/src/shared/Menu.cc @@ -5,6 +5,7 @@ #ifdef HAS_INTERFACE_BOARD #include "Steppers.hh" +#include "Planner.hh" #include "Commands.hh" #include "Errors.hh" #include "Tool.hh" @@ -178,7 +179,7 @@ void JogMode::jog(ButtonArray::ButtonName direction) { break; } - steppers::setTarget(position, interval); + planner::addMoveToBuffer(position, interval); } void JogMode::notifyButtonPressed(ButtonArray::ButtonName button) { diff --git a/firmware/src/shared/StepperAxis.cc b/firmware/src/shared/StepperAxis.cc index c69bce1..d099686 100644 --- a/firmware/src/shared/StepperAxis.cc +++ b/firmware/src/shared/StepperAxis.cc @@ -1,36 +1,48 @@ #include "StepperAxis.hh" -StepperAxis::StepperAxis() : - interface(0) { +StepperAxis::StepperAxis() : interface(0) { + reset(); } -StepperAxis::StepperAxis(StepperInterface& stepper_interface) : - interface(&stepper_interface) { +StepperAxis::StepperAxis(StepperInterface& stepper_interface) : interface(&stepper_interface) { reset(); } -void StepperAxis::setTarget(const int32_t target_in, - bool relative) { - target = target_in; +void StepperAxis::setStepMultiplier(const int8_t new_multiplier) { + step_multiplier = new_multiplier; + step_change = direction ? step_multiplier : -step_multiplier; +} + + +void StepperAxis::setTarget(const int32_t target_in, bool relative) { if (relative) { - delta = target; + delta = target_in; + target = position + target_in; } else { - delta = target - position; + delta = target_in - position; + target = target_in; } direction = true; if (delta != 0) { interface->setEnabled(true); } + step_multiplier = 1; if (delta < 0) { delta = -delta; direction = false; + step_change = -1; + } else { + step_change = 1; } + interface->setDirection(direction); } void StepperAxis::setHoming(const bool direction_in) { direction = direction_in; + interface->setDirection(direction); interface->setEnabled(true); delta = 1; + step_change = direction ? 1 : -1; } void StepperAxis::definePosition(const int32_t position_in) { @@ -48,66 +60,68 @@ void StepperAxis::reset() { target = 0; counter = 0; delta = 0; + step_multiplier = 1; + step_change = 1; #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) endstop_play = ENDSTOP_DEFAULT_PLAY; endstop_status = ESS_UNKNOWN; #endif //SINGLE_SWITCH_ENDSTOPS } + bool StepperAxis::checkEndstop(const bool isHoming) { #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) - bool hit_endstop = direction ? interface->isAtMaximum() : interface->isAtMinimum(); + bool hit_endstop = interface->isAtMinimum(); // We must move at least ENDSTOP_DEBOUNCE from where we hit the endstop before we declare traveling - if (hit_endstop || ((endstop_play < ENDSTOP_DEFAULT_PLAY - ENDSTOP_DEBOUNCE) && endstop_status != ESS_TRAVELING)) { + if (hit_endstop || (endstop_status == (direction?ESS_AT_MAXIMUM:ESS_AT_MINIMUM) && (endstop_play < ENDSTOP_DEFAULT_PLAY - ENDSTOP_DEBOUNCE))) { + hit_endstop = true; // Did we *just* hit the endstop? - if (endstop_status == ESS_TRAVELING || (isHoming && endstop_status == ESS_UNKNOWN)) { - endstop_play = ENDSTOP_DEFAULT_PLAY; - if (isHoming?direction:prev_direction) - endstop_status = ESS_AT_MAXIMUM; - else - endstop_status = ESS_AT_MINIMUM; - + if (endstop_status == ESS_TRAVELING || (isHoming && endstop_status == ESS_UNKNOWN)) { + endstop_play = ENDSTOP_DEFAULT_PLAY; + if (isHoming?direction:prev_direction) + endstop_status = ESS_AT_MAXIMUM; + else + endstop_status = ESS_AT_MINIMUM; + // OR, are we traveling away from the endstop we just hit and still have play... - } else if ((direction && endstop_status != ESS_AT_MAXIMUM) || (!direction && endstop_status != ESS_AT_MINIMUM)) { - if (endstop_play > 0) { - --endstop_play; - hit_endstop = false; // pretend this never happened... - } else { - // we ran out of play, so we must be ramming into the side, switch directions - // endstop_status = !direction ? ESS_AT_MAXIMUM : ESS_AT_MINIMUM; - // endstop_play = ENDSTOP_DEFAULT_PLAY; - } - } + } else if ((direction && endstop_status != ESS_AT_MAXIMUM) || (!direction && endstop_status != ESS_AT_MINIMUM)) { + if (endstop_play > 0) { + --endstop_play; + hit_endstop = false; // pretend this never happened... + } else { + // we ran out of play, so we must be ramming into the side, switch directions + // endstop_status = !direction ? ESS_AT_MAXIMUM : ESS_AT_MINIMUM; + // endstop_play = ENDSTOP_DEFAULT_PLAY; + } + } // otherwise we hit the endstop - + // but if we didn't hit an endstop, clear the status - } else { - endstop_status = ESS_TRAVELING; - if (!isHoming) { - endstop_play = ENDSTOP_DEFAULT_PLAY; - } - } - prev_direction = direction; - return hit_endstop; + } else { + endstop_status = ESS_TRAVELING; + if (!isHoming) { + endstop_play = ENDSTOP_DEFAULT_PLAY; + } + } + prev_direction = direction; + return hit_endstop; #else - return direction ? interface->isAtMaximum() : interface->isAtMinimum(); + return direction ? interface->isAtMaximum() : interface->isAtMinimum(); #endif } void StepperAxis::doInterrupt(const int32_t intervals) { counter += delta; + if (counter >= 0) { - interface->setDirection(direction); counter -= intervals; bool hit_endstop = checkEndstop(false); - if (direction) { - if (!hit_endstop) interface->step(true); - position++; - } else { - if (!hit_endstop) interface->step(true); - position--; - } - interface->step(false); + if (!hit_endstop) + for (int8_t steps = step_multiplier; steps > 0; steps--) { + interface->step(true); + interface->step(false); + } + position += step_change; } } @@ -116,25 +130,18 @@ bool StepperAxis::doHoming(const int32_t intervals) { if (delta == 0) return false; counter += delta; if (counter >= 0) { - interface->setDirection(direction); counter -= intervals; bool hit_endstop = checkEndstop(true); - if (direction) { - if (!hit_endstop) { + if (!hit_endstop) { + // we honor the step multiplier here, but it *really* should be 1 for homing + for (int8_t steps = step_multiplier; steps > 0; steps--) { interface->step(true); - } else { - return false; + interface->step(false); } - position++; } else { - if (!hit_endstop) { - interface->step(true); - } else { - return false; - } - position--; + return false; } - interface->step(false); + position += step_change; } return true; -} +} \ No newline at end of file diff --git a/firmware/src/shared/StepperAxis.hh b/firmware/src/shared/StepperAxis.hh index 6d13ef7..738cadd 100644 --- a/firmware/src/shared/StepperAxis.hh +++ b/firmware/src/shared/StepperAxis.hh @@ -11,21 +11,23 @@ class StepperAxis { public: - StepperInterface* interface; ///< Interface this axis is connected to - volatile int32_t position; ///< Current position of this axis, in steps - int32_t minimum; ///< Minimum position, in steps - int32_t maximum; ///< Maximum position, in steps - volatile int32_t target; ///< Target position, in steps - volatile int32_t counter; ///< Step counter; represents the proportion of - ///< a step so far passed. When the counter hits - ///< zero, a step is taken. - volatile int32_t delta; ///< Amount to increment counter per tick - volatile bool direction; ///< True for positive, false for negative + StepperInterface* interface; ///< Interface this axis is connected to + volatile int32_t position; ///< Current position of this axis, in steps + int32_t minimum; ///< Minimum position, in steps + int32_t maximum; ///< Maximum position, in steps + volatile int32_t target; ///< Target position, in steps + volatile int32_t counter; ///< Step counter; represents the proportion of + ///< a step so far passed. When the counter hits + ///< zero, a step is taken. + volatile int32_t delta; ///< Amount to increment counter per tick + volatile bool direction; ///< True for positive, false for negative + volatile int8_t step_multiplier; ///< Used to simulate dynamic microstep switching, must be > 0 and 2^N + volatile int8_t step_change; ///< Uses internally. step_change = direction ? step_multiplier : -step_multiplier; #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) - volatile bool prev_direction; ///< Record the previous direction for endstop detection - volatile int32_t endstop_play; ///< Amount to move while endstop triggered, to see which way to move - - enum endstop_status_t { ///< State of the endstop + volatile bool prev_direction; ///< Record the previous direction for endstop detection + volatile int32_t endstop_play; ///< Amount to move while endstop triggered, to see which way to move + + enum endstop_status_t { ///< State of the endstop ESS_UNKNOWN, ESS_TRAVELING, ESS_AT_MAXIMUM, @@ -58,6 +60,10 @@ public: /// to be relative to the current position. void setTarget(const int32_t target_in, bool relative); + /// Set the step multiplier + /// \param[in] new_multiplier + void setStepMultiplier(const int8_t new_multiplier); + /// Start a homing procedure /// \param[in] direction_in If true, home in the positive direction. void setHoming(const bool direction_in); @@ -83,4 +89,4 @@ public: bool doHoming(const int32_t intervals); }; -#endif // STEPPERAXIS_HH +#endif // STEPPERAXIS_HH \ No newline at end of file