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
|
/********************************************************************
* Description: tp_types.h
* Trajectory planner types and constants
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
********************************************************************/
#ifndef TP_TYPES_H
#define TP_TYPES_H
#include "posemath.h"
#include "tc_types.h"
#include "tcq.h"
//KLUDGE hack to get bool working in sim
#ifdef SIM
#include <stdbool.h>
#endif
#define TP_DEFAULT_QUEUE_SIZE 32
/* Minimum length of a segment in cycles (must be greater than 1 to ensure each
* segment is hit at least once.) */
#define TP_MIN_SEGMENT_CYCLES 1.02
/* Values chosen for accel ratio to match parabolic blend acceleration
* limits. */
#define TP_OPTIMIZATION_CUTOFF 4
/* If the queue is shorter than the threshold, assume that we're approaching
* the end of the program */
#define TP_QUEUE_THRESHOLD 3
/* closeness to zero, for determining if a move is pure rotation */
#define TP_PURE_ROTATION_EPSILON 1e-6
/* "neighborhood" size (if two values differ by less than the epsilon,
* then they are effectively equal.)*/
#define TP_ACCEL_EPSILON 1e-4
#define TP_VEL_EPSILON 1e-8
#define TP_POS_EPSILON 1e-12
#define TP_TIME_EPSILON 1e-12
#define TP_ANGLE_EPSILON 1e-6
#define TP_MIN_ARC_ANGLE 1e-3
#define TP_MIN_ARC_LENGTH 1e-6
#define TP_BIG_NUM 1e10
/**
* TP return codes.
* This enum is a catch-all for useful return statuses from TP
* internal functions. This may be replaced with a better system in
* the future.
*/
typedef enum {
TP_ERR_RADIUS = -6,
TP_ERR_GEOM = -5,
TP_ERR_RANGE = -4,
TP_ERR_MISSING_OUTPUT = -3,
TP_ERR_MISSING_INPUT = -2,
TP_ERR_FAIL = -1,
TP_ERR_OK = 0,
TP_ERR_NO_ACTION,
TP_ERR_SLOWING,
TP_ERR_STOPPED,
TP_ERR_WAITING,
TP_ERR_LAST
} tp_err_t;
/**
* Persistant data for spindle status within tpRunCycle.
* This structure encapsulates some static variables to simplify refactoring of
* synchronized motion code.
*/
typedef struct {
double offset;
double revs;
int waiting_for_index;
int waiting_for_atspeed;
} tp_spindle_t;
/**
* Trajectory planner state structure.
* Stores persistant data for the trajectory planner that should be accessible
* by outside functions.
*/
typedef struct {
TC_QUEUE_STRUCT queue;
tp_spindle_t spindle; //Spindle data
EmcPose currentPos;
EmcPose goalPos;
int queueSize;
double cycleTime;
double vMax; /* vel for subsequent moves */
double ini_maxvel; /* max velocity allowed by machine
constraints (ini file) for
subsequent moves */
double vLimit; /* absolute upper limit on all vels */
double aMax; /* max accel (unused) */
//FIXME this shouldn't be a separate limit,
double aMaxCartesian; /* max cartesian acceleration by machine bounds */
double aLimit; /* max accel (unused) */
double wMax; /* rotational velocity max */
double wDotMax; /* rotational accelleration max */
int nextId;
int execId;
int termCond;
int done;
int depth; /* number of total queued motions */
int activeDepth; /* number of motions blending */
int aborting;
int pausing;
int motionType;
double tolerance; /* for subsequent motions, stay within this
distance of the programmed path during
blends */
int synchronized; // spindle sync required for this move
int velocity_mode; /* TRUE if spindle sync is in velocity mode,
FALSE if in position mode */
double uu_per_rev; /* user units per spindle revolution */
syncdio_t syncdio; //record tpSetDout's here
} TP_STRUCT;
#endif /* TP_TYPES_H */
|
