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
|
// Trapezoidal velocity movement queue
//
// Copyright (C) 2018-2019 Kevin O'Connor <kevin@koconnor.net>
//
// This file may be distributed under the terms of the GNU GPLv3 license.
#include <math.h> // sqrt
#include <stdlib.h> // malloc
#include <string.h> // memset
#include "compiler.h" // unlikely
#include "trapq.h" // move_get_coord
struct move * __visible
move_alloc(void)
{
struct move *m = malloc(sizeof(*m));
memset(m, 0, sizeof(*m));
return m;
}
// Populate a 'struct move' with a velocity trapezoid
void __visible
move_fill(struct move *m, double print_time
, double accel_t, double cruise_t, double decel_t
, double start_pos_x, double start_pos_y, double start_pos_z
, double axes_d_x, double axes_d_y, double axes_d_z
, double start_v, double cruise_v, double accel)
{
// Setup velocity trapezoid
m->print_time = print_time;
m->move_t = accel_t + cruise_t + decel_t;
m->accel_t = accel_t;
m->cruise_t = cruise_t;
m->cruise_start_d = accel_t * .5 * (cruise_v + start_v);
m->decel_start_d = m->cruise_start_d + cruise_t * cruise_v;
// Setup for accel/cruise/decel phases
m->cruise_v = cruise_v;
m->accel.c1 = start_v;
m->accel.c2 = .5 * accel;
m->decel.c1 = cruise_v;
m->decel.c2 = -m->accel.c2;
// Setup for move_get_coord()
m->start_pos.x = start_pos_x;
m->start_pos.y = start_pos_y;
m->start_pos.z = start_pos_z;
double inv_move_d = 1. / sqrt(axes_d_x*axes_d_x + axes_d_y*axes_d_y
+ axes_d_z*axes_d_z);
m->axes_r.x = axes_d_x * inv_move_d;
m->axes_r.y = axes_d_y * inv_move_d;
m->axes_r.z = axes_d_z * inv_move_d;
}
// Find the distance travel during acceleration/deceleration
static inline double
move_eval_accel(struct move_accel *ma, double move_time)
{
return (ma->c1 + ma->c2 * move_time) * move_time;
}
// Return the distance moved given a time in a move
inline double
move_get_distance(struct move *m, double move_time)
{
if (unlikely(move_time < m->accel_t))
// Acceleration phase of move
return move_eval_accel(&m->accel, move_time);
move_time -= m->accel_t;
if (likely(move_time <= m->cruise_t))
// Cruising phase
return m->cruise_start_d + m->cruise_v * move_time;
// Deceleration phase
move_time -= m->cruise_t;
return m->decel_start_d + move_eval_accel(&m->decel, move_time);
}
// Return the XYZ coordinates given a time in a move
inline struct coord
move_get_coord(struct move *m, double move_time)
{
double move_dist = move_get_distance(m, move_time);
return (struct coord) {
.x = m->start_pos.x + m->axes_r.x * move_dist,
.y = m->start_pos.y + m->axes_r.y * move_dist,
.z = m->start_pos.z + m->axes_r.z * move_dist };
}
|
