From bec861fbc6167dc4bbdd0653b950e862a9911521 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 23 Mar 2016 15:01:25 -0700 Subject: [PATCH] Broke up planner.h --- src/canonical_machine.c | 3 + src/controller.c | 3 +- src/plan/arc.c | 3 +- src/plan/arc.h | 6 +- src/plan/buffer.c | 21 ++++- src/plan/buffer.h | 138 +++++++++++++++++++++++++++++++++ src/plan/command.c | 2 + src/plan/command.h | 2 +- src/plan/dwell.c | 2 +- src/plan/exec.c | 1 + src/plan/exec.h | 33 ++++++++ src/plan/feedhold.c | 5 ++ src/plan/feedhold.h | 33 ++++++++ src/plan/jog.c | 2 + src/plan/line.c | 6 +- src/plan/line.h | 33 ++++++++ src/plan/planner.c | 3 +- src/plan/planner.h | 164 ++-------------------------------------- src/plan/zoid.c | 9 ++- src/plan/zoid.h | 48 ++++++++++++ src/stepper.c | 2 +- 21 files changed, 348 insertions(+), 171 deletions(-) create mode 100644 src/plan/buffer.h create mode 100644 src/plan/exec.h create mode 100644 src/plan/feedhold.h create mode 100644 src/plan/line.h create mode 100644 src/plan/zoid.h diff --git a/src/canonical_machine.c b/src/canonical_machine.c index acc4d56..8664b5b 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -110,9 +110,12 @@ #include "usart.h" // for serial queue flush #include "plan/planner.h" +#include "plan/buffer.h" +#include "plan/feedhold.h" #include "plan/dwell.h" #include "plan/command.h" #include "plan/arc.h" +#include "plan/line.h" #include #include diff --git a/src/controller.c b/src/controller.c index 52d00d0..205a4b6 100644 --- a/src/controller.c +++ b/src/controller.c @@ -43,8 +43,9 @@ #include "command.h" #include "report.h" +#include "plan/buffer.h" #include "plan/arc.h" -#include "plan/planner.h" +#include "plan/feedhold.h" #include #include diff --git a/src/plan/arc.c b/src/plan/arc.c index 1d4008f..21e2e98 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -34,7 +34,8 @@ #include "arc.h" -#include "planner.h" +#include "buffer.h" +#include "line.h" #include "config.h" #include "util.h" diff --git a/src/plan/arc.h b/src/plan/arc.h index ba8bb53..272ddb7 100644 --- a/src/plan/arc.h +++ b/src/plan/arc.h @@ -28,7 +28,11 @@ #pragma once -#include "status.h" +#include "planner.h" + +#define ARC_SEGMENT_LENGTH 0.1 // mm +#define MIN_ARC_RADIUS 0.1 +#define MIN_ARC_SEGMENT_TIME (MIN_ARC_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) stat_t cm_arc_callback(); void cm_abort_arc(); diff --git a/src/plan/buffer.c b/src/plan/buffer.c index 2354890..38b0cdf 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -49,18 +49,31 @@ * (test, get and unget have no effect) */ -#include "planner.h" +#include "buffer.h" #include -/// buffer incr & wrap -#define _bump(a) ((a < PLANNER_BUFFER_POOL_SIZE - 1) ? a + 1 : 0) + +typedef struct mpBufferPool { // ring buffer for sub-moves + uint8_t buffers_available; // running count of available buffers + mpBuf_t *w; // get_write_buffer pointer + mpBuf_t *q; // queue_write_buffer pointer + mpBuf_t *r; // get/end_run_buffer pointer + mpBuf_t bf[PLANNER_BUFFER_POOL_SIZE]; // buffer storage +} mpBufferPool_t; + + +mpBufferPool_t mb; // move buffer queue /// Returns # of available planner buffers uint8_t mp_get_planner_buffers_available() {return mb.buffers_available;} +/// buffer incr & wrap +#define _bump(a) ((a < PLANNER_BUFFER_POOL_SIZE - 1) ? a + 1 : 0) + + /// Initializes or resets buffers void mp_init_buffers() { mpBuf_t *pv; @@ -73,7 +86,7 @@ void mp_init_buffers() { pv = &mb.bf[PLANNER_BUFFER_POOL_SIZE - 1]; // setup ring pointers - for (uint8_t i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) { + for (int i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) { mb.bf[i].nx = &mb.bf[_bump(i)]; mb.bf[i].pv = pv; pv = &mb.bf[i]; diff --git a/src/plan/buffer.h b/src/plan/buffer.h new file mode 100644 index 0000000..945fe79 --- /dev/null +++ b/src/plan/buffer.h @@ -0,0 +1,138 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software 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 + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "canonical_machine.h" + +#include + +/* PLANNER_BUFFER_POOL_SIZE + * Should be at least the number of buffers requires to support optimal + * planning in the case of very short lines or arc segments. + * Suggest 12 min. Limit is 255 + */ +#define PLANNER_BUFFER_POOL_SIZE 32 + +/// Buffers to reserve in planner before processing new input line +#define PLANNER_BUFFER_HEADROOM 4 + + +typedef enum { // bf->move_type values + MOVE_TYPE_NULL, // null move - does a no-op + MOVE_TYPE_ALINE, // acceleration planned line + MOVE_TYPE_DWELL, // delay with no movement + MOVE_TYPE_COMMAND, // general command + MOVE_TYPE_JOG, // interactive jogging +} moveType_t; + +typedef enum { + MOVE_OFF, // move inactive (MUST BE ZERO) + MOVE_NEW, // general value if you need an initialization + MOVE_RUN, // general run state (for non-acceleration moves) + MOVE_SKIP_BLOCK // mark a skipped block +} moveState_t; + +typedef enum { + SECTION_OFF, // section inactive + SECTION_NEW, // uninitialized section + SECTION_1st_HALF, // first half of S curve + SECTION_2nd_HALF // second half of S curve or running a BODY (cruise) +} sectionState_t; + +// All the enums that equal zero must be zero. Don't change this +typedef enum { // bf->buffer_state values + MP_BUFFER_EMPTY, // struct is available for use (MUST BE 0) + MP_BUFFER_LOADING, // being written ("checked out") + MP_BUFFER_QUEUED, // in queue + MP_BUFFER_PENDING, // marked as the next buffer to run + MP_BUFFER_RUNNING // current running buffer +} mpBufferState_t; + + +// Callbacks +typedef void (*cm_exec_t)(float[], float[]); +struct mpBuffer; +typedef stat_t (*bf_func_t)(struct mpBuffer *bf); + + +typedef struct mpBuffer { // See Planning Velocity Notes + struct mpBuffer *pv; // pointer to previous buffer + struct mpBuffer *nx; // pointer to next buffer + + bf_func_t bf_func; // callback to buffer exec function + cm_exec_t cm_func; // callback to canonical machine + + float naive_move_time; + + mpBufferState_t buffer_state; // used to manage queuing/dequeuing + moveType_t move_type; // used to dispatch to run routine + uint8_t move_code; // byte used by used exec functions + moveState_t move_state; // move state machine sequence + bool replannable; // true if move can be re-planned + + float unit[AXES]; // unit vector for axis scaling & planning + + float length; // total length of line or helix in mm + float head_length; + float body_length; + float tail_length; + // See notes on these variables, in aline() + float entry_velocity; // entry velocity requested for the move + float cruise_velocity; // cruise velocity requested & achieved + float exit_velocity; // exit velocity requested for the move + + float entry_vmax; // max junction velocity at entry of this move + float cruise_vmax; // max cruise velocity requested for move + float exit_vmax; // max exit velocity possible (redundant) + float delta_vmax; // max velocity difference for this move + float braking_velocity; // current value for braking velocity + + uint8_t jerk_axis; // rate limiting axis used to compute jerk + float jerk; // maximum linear jerk term for this move + float recip_jerk; // 1/Jm used for planning (computed & cached) + float cbrt_jerk; // cube root of Jm (computed & cached) + + GCodeState_t gm; // Gode model state, used by planner & runtime +} mpBuf_t; + + +uint8_t mp_get_planner_buffers_available(); +void mp_init_buffers(); +mpBuf_t *mp_get_write_buffer(); +void mp_unget_write_buffer(); +void mp_commit_write_buffer(const uint8_t move_type); +mpBuf_t *mp_get_run_buffer(); +uint8_t mp_free_run_buffer(); +mpBuf_t *mp_get_first_buffer(); +mpBuf_t *mp_get_last_buffer(); +/// Returns pointer to prev buffer in linked list +#define mp_get_prev_buffer(b) ((mpBuf_t *)(b->pv)) +/// Returns pointer to next buffer in linked list +#define mp_get_next_buffer(b) ((mpBuf_t *)(b->nx)) +void mp_clear_buffer(mpBuf_t *bf); +void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp); diff --git a/src/plan/command.c b/src/plan/command.c index 6de36df..a2ef363 100644 --- a/src/plan/command.c +++ b/src/plan/command.c @@ -45,6 +45,8 @@ */ #include "command.h" + +#include "buffer.h" #include "canonical_machine.h" #include "stepper.h" diff --git a/src/plan/command.h b/src/plan/command.h index 07272b1..1db6f81 100644 --- a/src/plan/command.h +++ b/src/plan/command.h @@ -27,7 +27,7 @@ #pragma once -#include "plan/planner.h" +#include "plan/buffer.h" void mp_queue_command(cm_exec_t cm_exec, float *value, float *flag); void mp_runtime_command(mpBuf_t *bf); diff --git a/src/plan/dwell.c b/src/plan/dwell.c index 4fa2621..f1f6e29 100644 --- a/src/plan/dwell.c +++ b/src/plan/dwell.c @@ -29,7 +29,7 @@ #include "dwell.h" -#include "planner.h" +#include "buffer.h" #include "canonical_machine.h" #include "stepper.h" diff --git a/src/plan/exec.c b/src/plan/exec.c index 014a0c4..2c68580 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -28,6 +28,7 @@ \******************************************************************************/ #include "planner.h" +#include "buffer.h" #include "kinematics.h" #include "stepper.h" #include "encoder.h" diff --git a/src/plan/exec.h b/src/plan/exec.h new file mode 100644 index 0000000..c73bb1c --- /dev/null +++ b/src/plan/exec.h @@ -0,0 +1,33 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software 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 + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "buffer.h" + +stat_t mp_exec_move(); +stat_t mp_exec_aline(mpBuf_t *bf); diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c index 14cb1b5..8016c18 100644 --- a/src/plan/feedhold.c +++ b/src/plan/feedhold.c @@ -27,7 +27,12 @@ \******************************************************************************/ +#include "feedhold.h" + #include "planner.h" +#include "buffer.h" +#include "line.h" +#include "zoid.h" #include "util.h" #include diff --git a/src/plan/feedhold.h b/src/plan/feedhold.h new file mode 100644 index 0000000..c02c366 --- /dev/null +++ b/src/plan/feedhold.h @@ -0,0 +1,33 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software 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 + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "status.h" + +stat_t mp_plan_hold_callback(); +stat_t mp_end_hold(); diff --git a/src/plan/jog.c b/src/plan/jog.c index 6e5cf36..3bc3fa3 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -26,7 +26,9 @@ \******************************************************************************/ #include "jog.h" + #include "planner.h" +#include "buffer.h" #include "stepper.h" #include "motor.h" #include "canonical_machine.h" diff --git a/src/plan/line.c b/src/plan/line.c index 9cc5868..0a1be46 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -27,8 +27,12 @@ \******************************************************************************/ -#include "planner.h" +#include "line.h" +#include "planner.h" +#include "exec.h" +#include "buffer.h" +#include "zoid.h" #include "controller.h" #include "canonical_machine.h" #include "stepper.h" diff --git a/src/plan/line.h b/src/plan/line.h new file mode 100644 index 0000000..eca0dfc --- /dev/null +++ b/src/plan/line.h @@ -0,0 +1,33 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software 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 + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "buffer.h" + +void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag); +stat_t mp_aline(GCodeState_t *gm_in); diff --git a/src/plan/planner.c b/src/plan/planner.c index cb90406..af276a4 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -58,6 +58,8 @@ */ #include "planner.h" + +#include "buffer.h" #include "arc.h" #include "canonical_machine.h" #include "kinematics.h" @@ -69,7 +71,6 @@ #include -mpBufferPool_t mb; // move buffer queue mpMoveMasterSingleton_t mm; // context for line planning mpMoveRuntimeSingleton_t mr; // context for line runtime diff --git a/src/plan/planner.h b/src/plan/planner.h index a5eb65a..439beb1 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -34,52 +34,26 @@ #include "util.h" #include "config.h" -typedef enum { // bf->move_type values - MOVE_TYPE_NULL, // null move - does a no-op - MOVE_TYPE_ALINE, // acceleration planned line - MOVE_TYPE_DWELL, // delay with no movement - MOVE_TYPE_COMMAND, // general command - MOVE_TYPE_JOG, // interactive jogging -} moveType_t; - -typedef enum { - MOVE_OFF, // move inactive (MUST BE ZERO) - MOVE_NEW, // general value if you need an initialization - MOVE_RUN, // general run state (for non-acceleration moves) - MOVE_SKIP_BLOCK // mark a skipped block -} moveState_t; - typedef enum { SECTION_HEAD, // acceleration SECTION_BODY, // cruise - SECTION_TAIL // deceleration + SECTION_TAIL, // deceleration + SECTIONS // section count } moveSection_t; -#define SECTIONS 3 - -typedef enum { - SECTION_OFF, // section inactive - SECTION_NEW, // uninitialized section - SECTION_1st_HALF, // first half of S curve - SECTION_2nd_HALF // second half of S curve or running a BODY (cruise) -} sectionState_t; // Most of these factors are the result of a lot of tweaking. // Change with caution. -#define ARC_SEGMENT_LENGTH ((float)0.1) // Arc segment size (mm).(0.03) -#define MIN_ARC_RADIUS ((float)0.1) - -#define JERK_MULTIPLIER ((float)1000000) +#define JERK_MULTIPLIER 1000000.0 /// precision jerk must match to be considered same -#define JERK_MATCH_PRECISION ((float)1000) +#define JERK_MATCH_PRECISION 1000.0 -#define NOM_SEGMENT_USEC ((float)5000) // nominal segment time +#define NOM_SEGMENT_USEC 5000.0 // nominal segment time /// minimum segment time / minimum move time -#define MIN_SEGMENT_USEC ((float)2500) -#define MIN_ARC_SEGMENT_USEC ((float)10000) // minimum arc segment time +#define MIN_SEGMENT_USEC 2500.0 +#define MIN_ARC_SEGMENT_USEC 10000.0 // minimum arc segment time #define NOM_SEGMENT_TIME (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) #define MIN_SEGMENT_TIME (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) -#define MIN_ARC_SEGMENT_TIME (MIN_ARC_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) /// minimum time a move can be is one segment #define MIN_TIME_MOVE MIN_SEGMENT_TIME /// factor for minimum size Gcode block to process @@ -88,93 +62,6 @@ typedef enum { #define MIN_SEGMENT_TIME_PLUS_MARGIN \ ((MIN_SEGMENT_USEC + 1) / MICROSECONDS_PER_MINUTE) -/* PLANNER_BUFFER_POOL_SIZE - * Should be at least the number of buffers requires to support optimal - * planning in the case of very short lines or arc segments. - * Suggest 12 min. Limit is 255 - */ -#define PLANNER_BUFFER_POOL_SIZE 32 -/// Buffers to reserve in planner before processing new input line -#define PLANNER_BUFFER_HEADROOM 4 - -// Parameters for _generate_trapezoid() - -/// Max iterations for convergence in the HT asymmetric case. -#define TRAPEZOID_ITERATION_MAX 10 - -/// Error percentage for iteration convergence. As percent - 0.01 = 1% -#define TRAPEZOID_ITERATION_ERROR_PERCENT ((float)0.10) - -/// Tolerance for "exact fit" for H and T cases -/// allowable mm of error in planning phase -#define TRAPEZOID_LENGTH_FIT_TOLERANCE ((float)0.0001) - -/// Adaptive velocity tolerance term -#define TRAPEZOID_VELOCITY_TOLERANCE (max(2, bf->entry_velocity / 100)) - - -/// Callback to canonical_machine execution function -typedef void (*cm_exec_t)(float[], float[]); - - -// All the enums that equal zero must be zero. Don't change this -typedef enum { // bf->buffer_state values - MP_BUFFER_EMPTY, // struct is available for use (MUST BE 0) - MP_BUFFER_LOADING, // being written ("checked out") - MP_BUFFER_QUEUED, // in queue - MP_BUFFER_PENDING, // marked as the next buffer to run - MP_BUFFER_RUNNING // current running buffer -} mpBufferState_t; - - -typedef struct mpBuffer { // See Planning Velocity Notes - struct mpBuffer *pv; // static pointer to previous buffer - struct mpBuffer *nx; // static pointer to next buffer - stat_t (*bf_func)(struct mpBuffer *bf); // callback to buffer exec function - cm_exec_t cm_func; // callback to canonical machine - - float naiive_move_time; - - uint8_t buffer_state; // used to manage queuing/dequeuing - uint8_t move_type; // used to dispatch to run routine - uint8_t move_code; // byte used by used exec functions - uint8_t move_state; // move state machine sequence - uint8_t replannable; // TRUE if move can be re-planned - - float unit[AXES]; // unit vector for axis scaling & planning - - float length; // total length of line or helix in mm - float head_length; - float body_length; - float tail_length; - // See notes on these variables, in aline() - float entry_velocity; // entry velocity requested for the move - float cruise_velocity; // cruise velocity requested & achieved - float exit_velocity; // exit velocity requested for the move - - float entry_vmax; // max junction velocity at entry of this move - float cruise_vmax; // max cruise velocity requested for move - float exit_vmax; // max exit velocity possible (redundant) - float delta_vmax; // max velocity difference for this move - float braking_velocity; // current value for braking velocity - - uint8_t jerk_axis; // rate limiting axis used to compute jerk - float jerk; // maximum linear jerk term for this move - float recip_jerk; // 1/Jm used for planning (computed & cached) - float cbrt_jerk; // cube root of Jm (computed & cached) - - GCodeState_t gm; // Gode model state, used by planner & runtime -} mpBuf_t; - - -typedef struct mpBufferPool { // ring buffer for sub-moves - uint8_t buffers_available; // running count of available buffers - mpBuf_t *w; // get_write_buffer pointer - mpBuf_t *q; // queue_write_buffer pointer - mpBuf_t *r; // get/end_run_buffer pointer - mpBuf_t bf[PLANNER_BUFFER_POOL_SIZE]; // buffer storage -} mpBufferPool_t; - /// common variables for planning (move master) typedef struct mpMoveMasterSingleton { @@ -254,11 +141,10 @@ typedef struct mpMoveRuntimeSingleton { // persistent runtime variables // Reference global scope structures -extern mpBufferPool_t mb; // move buffer queue extern mpMoveMasterSingleton_t mm; // context for line planning extern mpMoveRuntimeSingleton_t mr; // context for line runtime -// planner.c functions + void planner_init(); void mp_flush_planner(); void mp_set_planner_position(uint8_t axis, const float position); @@ -270,37 +156,3 @@ float mp_get_runtime_absolute_position(uint8_t axis); void mp_set_runtime_work_offset(float offset[]); void mp_zero_segment_velocity(); uint8_t mp_get_runtime_busy(); - -// line.c functions -void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag); -stat_t mp_aline(GCodeState_t *gm_in); - -// zoid.c functions -void mp_calculate_trapezoid(mpBuf_t *bf); -float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf); -float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf); - -// exec.c functions -stat_t mp_exec_move(); -stat_t mp_exec_aline(mpBuf_t *bf); - -// feedhold.c functions -stat_t mp_plan_hold_callback(); -stat_t mp_end_hold(); - -// buffer.c functions -uint8_t mp_get_planner_buffers_available(); -void mp_init_buffers(); -mpBuf_t *mp_get_write_buffer(); -void mp_unget_write_buffer(); -void mp_commit_write_buffer(const uint8_t move_type); -mpBuf_t *mp_get_run_buffer(); -uint8_t mp_free_run_buffer(); -mpBuf_t *mp_get_first_buffer(); -mpBuf_t *mp_get_last_buffer(); -/// Returns pointer to prev buffer in linked list -#define mp_get_prev_buffer(b) ((mpBuf_t *)(b->pv)) -/// Returns pointer to next buffer in linked list -#define mp_get_next_buffer(b) ((mpBuf_t *)(b->nx)) -void mp_clear_buffer(mpBuf_t *bf); -void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp); diff --git a/src/plan/zoid.c b/src/plan/zoid.c index 4df3314..f09096e 100644 --- a/src/plan/zoid.c +++ b/src/plan/zoid.c @@ -27,11 +27,14 @@ \******************************************************************************/ +#include "zoid.h" + #include "planner.h" #include "util.h" #include + /* * This rather brute-force and long-ish function sets section lengths * and velocities based on the line length and velocities @@ -150,10 +153,10 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { // velocity you can get given the delta_vmax (maximum velocity slew) // supportable. - bf->naiive_move_time = + bf->naive_move_time = 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average - if (bf->naiive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) { + if (bf->naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) { bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN; bf->exit_velocity = max(0.0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax))); @@ -167,7 +170,7 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { } // B" case: Block is short, but fits into a single body segment - if (bf->naiive_move_time <= NOM_SEGMENT_TIME) { + if (bf->naive_move_time <= NOM_SEGMENT_TIME) { bf->entry_velocity = bf->pv->exit_velocity; if (fp_NOT_ZERO(bf->entry_velocity)) { diff --git a/src/plan/zoid.h b/src/plan/zoid.h new file mode 100644 index 0000000..d948d4c --- /dev/null +++ b/src/plan/zoid.h @@ -0,0 +1,48 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software 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 + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "buffer.h" + +/// Max iterations for convergence in the HT asymmetric case. +#define TRAPEZOID_ITERATION_MAX 10 + +/// Error percentage for iteration convergence. As percent - 0.01 = 1% +#define TRAPEZOID_ITERATION_ERROR_PERCENT 0.1 + +/// Tolerance for "exact fit" for H and T cases +/// allowable mm of error in planning phase +#define TRAPEZOID_LENGTH_FIT_TOLERANCE 0.0001 + +/// Adaptive velocity tolerance term +#define TRAPEZOID_VELOCITY_TOLERANCE (max(2, bf->entry_velocity / 100)) + + +void mp_calculate_trapezoid(mpBuf_t *bf); +float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf); +float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf); diff --git a/src/stepper.c b/src/stepper.c index 450374b..07a2176 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -31,7 +31,7 @@ #include "config.h" #include "canonical_machine.h" -#include "plan/planner.h" +#include "plan/exec.h" #include "plan/command.h" #include "motor.h" #include "hardware.h" -- 2.27.0