Merge pull request #715 from OskarLinde/zhomeofs_precision
[clinton/Smoothieware.git] / src / modules / robot / Planner.cpp
CommitLineData
df27a6a3 1/*
5886a464 2 This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl) with additions from Sungeun K. Jeon (https://github.com/chamnit/grbl)
4cff3ded
AW
3 Smoothie 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.
4 Smoothie 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.
df27a6a3 5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
4cff3ded
AW
6*/
7
8using namespace std;
9#include <vector>
4dc5513d
MM
10
11#include "mri.h"
12#include "nuts_bolts.h"
13#include "RingBuffer.h"
14#include "Gcode.h"
15#include "Module.h"
16#include "Kernel.h"
4cff3ded
AW
17#include "Block.h"
18#include "Planner.h"
3fceb8eb 19#include "Conveyor.h"
5673fe39 20#include "StepperMotor.h"
61134a65
JM
21#include "Config.h"
22#include "checksumm.h"
23#include "Robot.h"
24#include "Stepper.h"
8d54c34c 25#include "ConfigValue.h"
61134a65
JM
26
27#include <math.h>
b66fb830 28
8b69c90d 29#define acceleration_checksum CHECKSUM("acceleration")
c5fe1787 30#define z_acceleration_checksum CHECKSUM("z_acceleration")
8b69c90d
JM
31#define max_jerk_checksum CHECKSUM("max_jerk")
32#define junction_deviation_checksum CHECKSUM("junction_deviation")
44de6ef3 33#define z_junction_deviation_checksum CHECKSUM("z_junction_deviation")
8b69c90d
JM
34#define minimum_planner_speed_checksum CHECKSUM("minimum_planner_speed")
35
edac9072
AW
36// The Planner does the acceleration math for the queue of Blocks ( movements ).
37// It makes sure the speed stays within the configured constraints ( acceleration, junction_deviation, etc )
38// It goes over the list in both direction, every time a block is added, re-doing the math to make sure everything is optimal
4cff3ded
AW
39
40Planner::Planner(){
1cf31736 41 clear_vector_float(this->previous_unit_vec);
558e170c 42 config_load();
da24d6ae
AW
43}
44
edac9072 45// Configure acceleration
558e170c 46void Planner::config_load(){
c5fe1787
JM
47 this->acceleration = THEKERNEL->config->value(acceleration_checksum)->by_default(100.0F )->as_number(); // Acceleration is in mm/s^2
48 this->z_acceleration = THEKERNEL->config->value(z_acceleration_checksum)->by_default(0.0F )->as_number(); // disabled by default
49
44de6ef3
JM
50 this->junction_deviation = THEKERNEL->config->value(junction_deviation_checksum)->by_default(0.05F)->as_number();
51 this->z_junction_deviation = THEKERNEL->config->value(z_junction_deviation_checksum)->by_default(-1)->as_number(); // disabled by default
c5fe1787 52 this->minimum_planner_speed = THEKERNEL->config->value(minimum_planner_speed_checksum)->by_default(0.0f)->as_number();
4cff3ded
AW
53}
54
da24d6ae 55
4cff3ded 56// Append a block to the queue, compute it's speed factors
da947c62
MM
57void Planner::append_block( float actuator_pos[], float rate_mm_s, float distance, float unit_vec[] )
58{
44de6ef3 59 float acceleration, junction_deviation;
c5fe1787 60
edac9072 61 // Create ( recycle ) a new block
2134bcf2 62 Block* block = THEKERNEL->conveyor->queue.head_ref();
aab6cbba 63
c5fe1787 64
aab6cbba 65 // Direction bits
78d0e16a
MM
66 for (int i = 0; i < 3; i++)
67 {
68 int steps = THEKERNEL->robot->actuators[i]->steps_to_target(actuator_pos[i]);
1cf31736 69
558e170c 70 block->direction_bits[i] = (steps < 0) ? 1 : 0;
78d0e16a 71
338beb48 72 // Update current position
b2881caa 73 THEKERNEL->robot->actuators[i]->last_milestone_steps += steps;
338beb48
MM
74 THEKERNEL->robot->actuators[i]->last_milestone_mm = actuator_pos[i];
75
78d0e16a
MM
76 block->steps[i] = labs(steps);
77 }
1cf31736 78
44de6ef3
JM
79 acceleration= this->acceleration;
80 junction_deviation= this->junction_deviation;
81
c5fe1787 82 // use either regular acceleration or a z only move accleration
44de6ef3 83 if(block->steps[ALPHA_STEPPER] == 0 && block->steps[BETA_STEPPER] == 0) {
c5fe1787 84 // z only move
44de6ef3
JM
85 if(this->z_acceleration > 0.0F) acceleration= this->z_acceleration;
86 if(this->z_junction_deviation >= 0.0F) junction_deviation= this->z_junction_deviation;
c5fe1787
JM
87 }
88
4fdd2470
JM
89 block->acceleration= acceleration; // save in block
90
4cff3ded
AW
91 // Max number of steps, for all axes
92 block->steps_event_count = max( block->steps[ALPHA_STEPPER], max( block->steps[BETA_STEPPER], block->steps[GAMMA_STEPPER] ) );
93
4cff3ded 94 block->millimeters = distance;
aab6cbba 95
9db65137 96 // Calculate speed in mm/sec for each axis. No divide by zero due to previous checks.
aab6cbba 97 // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
130275f1 98 if( distance > 0.0F ){
da947c62 99 block->nominal_speed = rate_mm_s; // (mm/s) Always > 0
9502f9d5 100 block->nominal_rate = ceilf(block->steps_event_count * rate_mm_s / distance); // (step/s) Always > 0
436a2cd1 101 }else{
130275f1
MM
102 block->nominal_speed = 0.0F;
103 block->nominal_rate = 0;
436a2cd1 104 }
aab6cbba 105
4cff3ded
AW
106 // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
107 // average travel per step event changes. For a line along one axis the travel per step event
108 // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
109 // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
110 // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed
111 // specifically for each line to compensate for this phenomenon:
aab6cbba 112 // Convert universal acceleration for direction-dependent stepper rate change parameter
a157d099 113 block->rate_delta = (block->steps_event_count * acceleration) / (distance * THEKERNEL->acceleration_ticks_per_second); // (step/min/acceleration_tick)
1cf31736 114
aab6cbba
AW
115 // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
116 // Let a circle be tangent to both previous and current path line segments, where the junction
117 // deviation is defined as the distance from the junction to the closest edge of the circle,
118 // colinear with the circle center. The circular segment joining the two paths represents the
119 // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
120 // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
121 // path width or max_jerk in the previous grbl version. This approach does not actually deviate
122 // from path, but used as a robust way to compute cornering speeds, as it takes into account the
123 // nonlinearities of both the junction angle and junction velocity.
4dfd2dce
JM
124
125 // NOTE however it does not take into account independent axis, in most cartesian X and Y and Z are totally independent
126 // and this allows one to stop with little to no decleration in many cases. This is particualrly bad on leadscrew based systems that will skip steps.
8b69c90d 127 float vmax_junction = minimum_planner_speed; // Set default max junction speed
aab6cbba 128
38bf9a1c 129 if (!THEKERNEL->conveyor->is_queue_empty())
e75b3def
MM
130 {
131 float previous_nominal_speed = THEKERNEL->conveyor->queue.item_ref(THEKERNEL->conveyor->queue.prev(THEKERNEL->conveyor->queue.head_i))->nominal_speed;
132
44de6ef3 133 if (previous_nominal_speed > 0.0F && junction_deviation > 0.0F) {
e75b3def
MM
134 // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
135 // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
136 float cos_theta = - this->previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
137 - this->previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
138 - this->previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
139
140 // Skip and use default max junction speed for 0 degree acute junction.
141 if (cos_theta < 0.95F) {
142 vmax_junction = min(previous_nominal_speed, block->nominal_speed);
143 // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
144 if (cos_theta > -0.95F) {
145 // Compute maximum junction velocity based on maximum acceleration and junction deviation
146 float sin_theta_d2 = sqrtf(0.5F * (1.0F - cos_theta)); // Trig half angle identity. Always positive.
44de6ef3 147 vmax_junction = min(vmax_junction, sqrtf(acceleration * junction_deviation * sin_theta_d2 / (1.0F - sin_theta_d2)));
e75b3def
MM
148 }
149 }
aab6cbba 150 }
4cff3ded 151 }
aab6cbba 152 block->max_entry_speed = vmax_junction;
1cf31736 153
8b69c90d 154 // Initialize block entry speed. Compute based on deceleration to user-defined minimum_planner_speed.
c9cc5e06 155 float v_allowable = max_allowable_speed(-acceleration, minimum_planner_speed, block->millimeters);
aab6cbba
AW
156 block->entry_speed = min(vmax_junction, v_allowable);
157
158 // Initialize planner efficiency flags
159 // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
160 // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
161 // the current block and next block junction speeds are guaranteed to always be at their maximum
162 // junction speeds in deceleration and acceleration, respectively. This is due to how the current
163 // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
164 // the reverse and forward planners, the corresponding block junction speed will always be at the
165 // the maximum junction speed and may always be ignored for any speed reduction checks.
166 if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
167 else { block->nominal_length_flag = false; }
2134bcf2
MM
168
169 // Always calculate trapezoid for new block
170 block->recalculate_flag = true;
1cf31736 171
aab6cbba 172 // Update previous path unit_vector and nominal speed
3a425ecb 173 memcpy(this->previous_unit_vec, unit_vec, sizeof(previous_unit_vec)); // previous_unit_vec[] = unit_vec[]
1cf31736 174
df27a6a3 175 // Math-heavy re-computing of the whole queue to take the new
4cff3ded 176 this->recalculate();
1cf31736 177
df27a6a3 178 // The block can now be used
3a4fa0c1 179 block->ready();
2134bcf2
MM
180
181 THEKERNEL->conveyor->queue_head_block();
4cff3ded
AW
182}
183
4cff3ded 184void Planner::recalculate() {
a617ac35 185 Conveyor::Queue_t &queue = THEKERNEL->conveyor->queue;
4dc5513d 186
a617ac35 187 unsigned int block_index;
4cff3ded 188
391bc610
MM
189 Block* previous;
190 Block* current;
391bc610 191
a617ac35
MM
192 /*
193 * a newly added block is decel limited
194 *
195 * we find its max entry speed given its exit speed
196 *
d30d9611
MM
197 * for each block, walking backwards in the queue:
198 *
a617ac35
MM
199 * if max entry speed == current entry speed
200 * then we can set recalculate to false, since clearly adding another block didn't allow us to enter faster
d30d9611
MM
201 * and thus we don't need to check entry speed for this block any more
202 *
203 * once we find an accel limited block, we must find the max exit speed and walk the queue forwards
a617ac35 204 *
d30d9611 205 * for each block, walking forwards in the queue:
a617ac35
MM
206 *
207 * given the exit speed of the previous block and our own max entry speed
208 * we can tell if we're accel or decel limited (or coasting)
209 *
210 * if prev_exit > max_entry
d30d9611 211 * then we're still decel limited. update previous trapezoid with our max entry for prev exit
a617ac35 212 * if max_entry >= prev_exit
d30d9611 213 * then we're accel limited. set recalculate to false, work out max exit speed
a617ac35 214 *
d30d9611 215 * finally, work out trapezoid for the final (and newest) block.
a617ac35
MM
216 */
217
218 /*
219 * Step 1:
220 * For each block, given the exit speed and acceleration, find the maximum entry speed
221 */
222
223 float entry_speed = minimum_planner_speed;
224
225 block_index = queue.head_i;
226 current = queue.item_ref(block_index);
227
228 if (!queue.is_empty())
391bc610 229 {
a617ac35 230 while ((block_index != queue.tail_i) && current->recalculate_flag)
391bc610 231 {
a617ac35 232 entry_speed = current->reverse_pass(entry_speed);
391bc610 233
a617ac35
MM
234 block_index = queue.prev(block_index);
235 current = queue.item_ref(block_index);
2134bcf2 236 }
13e4a3f9 237
d30d9611
MM
238 /*
239 * Step 2:
240 * now current points to either tail or first non-recalculate block
241 * and has not had its reverse_pass called
242 * or its calc trap
243 * entry_speed is set to the *exit* speed of current.
244 * each block from current to head has its entry speed set to its max entry speed- limited by decel or nominal_rate
245 */
2134bcf2 246
a617ac35 247 float exit_speed = current->max_exit_speed();
4cff3ded 248
a617ac35
MM
249 while (block_index != queue.head_i)
250 {
251 previous = current;
252 block_index = queue.next(block_index);
253 current = queue.item_ref(block_index);
254
255 // we pass the exit speed of the previous block
256 // so this block can decide if it's accel or decel limited and update its fields as appropriate
257 exit_speed = current->forward_pass(exit_speed);
2134bcf2 258
a617ac35
MM
259 previous->calculate_trapezoid(previous->entry_speed, current->entry_speed);
260 }
4cff3ded 261 }
a617ac35 262
d30d9611
MM
263 /*
264 * Step 3:
265 * work out trapezoid for final (and newest) block
266 */
267
a617ac35
MM
268 // now current points to the head item
269 // which has not had calculate_trapezoid run yet
270 current->calculate_trapezoid(current->entry_speed, minimum_planner_speed);
4cff3ded 271}
aab6cbba 272
a617ac35 273
aab6cbba
AW
274// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
275// acceleration within the allotted distance.
1ad23cd3 276float Planner::max_allowable_speed(float acceleration, float target_velocity, float distance) {
aab6cbba 277 return(
95b4885b 278 sqrtf(target_velocity*target_velocity-2.0F*acceleration*distance) //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
aab6cbba
AW
279 );
280}
281
282