1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
|
// Cartesian kinematics stepper pulse time generation
//
// Copyright (C) 2016-2018 Kevin O'Connor <kevin@koconnor.net>
//
// This file may be distributed under the terms of the GNU GPLv3 license.
#include <math.h> // sqrt
#include "compiler.h" // likely
#include "pyhelper.h" // errorf
#include "stepcompress.h" // queue_append
// Common suffixes: _sd is step distance (a unit length the same
// distance the stepper moves on each step), _sv is step velocity (in
// units of step distance per time), _sd2 is step distance squared, _r
// is ratio (scalar usually between 0.0 and 1.0). Times are in
// seconds and acceleration is in units of step distance per second
// squared.
// Wrapper around sqrt() to handle small negative numbers
static double
_safe_sqrt(double v)
{
// Due to floating point truncation, it's possible to get a small
// negative number - treat it as zero.
if (v < -0.001)
errorf("safe_sqrt of %.9f", v);
return 0.;
}
inline double safe_sqrt(double v) {
return likely(v >= 0.) ? sqrt(v) : _safe_sqrt(v);
}
// Schedule a step event at the specified step_clock time
int32_t __visible
stepcompress_push(struct stepcompress *sc, double print_time, int32_t sdir)
{
int ret = set_next_step_dir(sc, !!sdir);
if (ret)
return ret;
struct queue_append qa = queue_append_start(sc, print_time, 0.5);
ret = queue_append(&qa, 0.);
if (ret)
return ret;
queue_append_finish(qa);
return sdir ? 1 : -1;
}
// Schedule 'steps' number of steps at constant acceleration. If
// acceleration is zero (ie, constant velocity) it uses the formula:
// step_time = print_time + step_num/start_sv
// Otherwise it uses the formula:
// step_time = (print_time + sqrt(2*step_num/accel + (start_sv/accel)**2)
// - start_sv/accel)
int32_t __visible
stepcompress_push_const(
struct stepcompress *sc, double print_time
, double step_offset, double steps, double start_sv, double accel)
{
// Calculate number of steps to take
int sdir = 1;
if (steps < 0) {
sdir = 0;
steps = -steps;
step_offset = -step_offset;
}
int count = steps + .5 - step_offset;
if (count <= 0 || count > 10000000) {
if (count && steps) {
errorf("push_const invalid count %d %f %f %f %f %f"
, stepcompress_get_oid(sc), print_time, step_offset, steps
, start_sv, accel);
return ERROR_RET;
}
return 0;
}
int ret = set_next_step_dir(sc, sdir);
if (ret)
return ret;
int res = sdir ? count : -count;
// Calculate each step time
if (!accel) {
// Move at constant velocity (zero acceleration)
struct queue_append qa = queue_append_start(sc, print_time, .5);
double inv_cruise_sv = stepcompress_get_mcu_freq(sc) / start_sv;
double pos = (step_offset + .5) * inv_cruise_sv;
while (count--) {
ret = queue_append(&qa, pos);
if (ret)
return ret;
pos += inv_cruise_sv;
}
queue_append_finish(qa);
} else {
// Move with constant acceleration
double inv_accel = 1. / accel;
double mcu_freq = stepcompress_get_mcu_freq(sc);
double accel_time = start_sv * inv_accel * mcu_freq;
struct queue_append qa = queue_append_start(
sc, print_time, 0.5 - accel_time);
double accel_multiplier = 2. * inv_accel * mcu_freq * mcu_freq;
double pos = (step_offset + .5)*accel_multiplier + accel_time*accel_time;
while (count--) {
double v = safe_sqrt(pos);
int ret = queue_append(&qa, accel_multiplier >= 0. ? v : -v);
if (ret)
return ret;
pos += accel_multiplier;
}
queue_append_finish(qa);
}
return res;
}
|
