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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
|
// 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 <stddef.h> // offsetof
#include <stdlib.h> // malloc
#include <string.h> // memset
#include "compiler.h" // unlikely
#include "trapq.h" // move_get_coord
// Allocate a new 'move' object
struct move *
move_alloc(void)
{
struct move *m = malloc(sizeof(*m));
memset(m, 0, sizeof(*m));
return m;
}
// Fill and add a move to the trapezoid velocity queue
void __visible
trapq_append(struct trapq *tq, 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)
{
struct move *m = move_alloc();
// 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;
trapq_add_move(tq, m);
}
// 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 };
}
// Allocate a new 'trapq' object
struct trapq * __visible
trapq_alloc(void)
{
struct trapq *tq = malloc(sizeof(*tq));
memset(tq, 0, sizeof(*tq));
list_init(&tq->moves);
return tq;
}
// Free memory associated with a 'trapq' object
void __visible
trapq_free(struct trapq *tq)
{
while (!list_empty(&tq->moves)) {
struct move *m = list_first_entry(&tq->moves, struct move, node);
list_del(&m->node);
free(m);
}
free(tq);
}
// Add a move to the trapezoid velocity queue
void
trapq_add_move(struct trapq *tq, struct move *m)
{
list_add_tail(&m->node, &tq->moves);
}
// Free any moves older than `print_time` from the trapezoid velocity queue
void __visible
trapq_free_moves(struct trapq *tq, double print_time)
{
while (!list_empty(&tq->moves)) {
struct move *m = list_first_entry(&tq->moves, struct move, node);
if (m->print_time + m->move_t > print_time)
return;
list_del(&m->node);
free(m);
}
}
|
