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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
|
/****************************************************************************************************
RepRapFirmware - Move
This is all the code to deal with movement and kinematics.
-----------------------------------------------------------------------------------------------------
Version 0.1
18 November 2012
Adrian Bowyer
RepRap Professional Ltd
http://reprappro.com
Licence: GPL
****************************************************************************************************/
#ifndef MOVE_H
#define MOVE_H
#define DDA_RING_LENGTH 5
#define LOOK_AHEAD_RING_LENGTH 20
#define LOOK_AHEAD 7
#define SMALL_Z_MOVE 0.03 // If a Z movement is less than this fraction of an XY move, the movement is classed as XY
enum MovementProfile
{
moving = 0, // Ordinary trapezoidal-velocity-profile movement
noFlat = 1, // Triangular profile movement
change = 2 // To make this movement, the initial and/or final velocities must change
};
enum MovementState
{
unprocessed = 0,
vCosineSet = 1,
upPass = 2,
complete = 4,
released = 8
};
enum MovementType
{
noMove = 0,
xyMove = 1,
zMove = 2,
eMove = 4
};
class LookAhead
{
public:
LookAhead(Move* m, Platform* p, LookAhead* n);
void Init(float ep[], float vv, bool ce);
LookAhead* Next();
LookAhead* Previous();
float* EndPoint();
float V();
void SetV(float vv);
int8_t Processed();
void SetProcessed(MovementState ms);
void SetDriveZeroEndSpeed(float a, int8_t drive);
bool CheckEndStops();
void Release();
friend class Move;
private:
Move* move;
Platform* platform;
LookAhead* next;
LookAhead* previous;
float endPoint[DRIVES+1];
float Cosine();
bool checkEndStops;
float cosine;
float v;
float instantDv;
volatile int8_t processed;
};
class DDA
{
public:
DDA(Move* m, Platform* p, DDA* n);
MovementProfile Init(LookAhead* lookAhead, float& u, float& v);
void Start(bool noTest);
void Step(bool noTest);
bool Active();
DDA* Next();
float InstantDv();
friend class Move;
private:
Move* move;
Platform* platform;
DDA* next;
LookAhead* myLookAheadEntry;
long counter[DRIVES];
long delta[DRIVES];
bool directions[DRIVES];
long totalSteps;
long stepCount;
bool checkEndStops;
float timeStep;
float velocity;
long stopAStep;
long startDStep;
float distance;
float dCross;
float acceleration;
float instantDv;
volatile bool active;
};
class Move
{
public:
Move(Platform* p, GCodes* g);
void Init();
void Spin();
void Exit();
bool GetCurrentState(float m[]);
void Interrupt();
void InterruptTime();
bool AllMovesAreFinished();
void ResumeMoving();
void DoLookAhead();
void HitLowStop(int8_t drive, LookAhead* la, DDA* hitDDA);
void HitHighStop(int8_t drive, LookAhead* la, DDA* hitDDA);
void SetPositions(float move[]);
void SetZProbing(bool probing);
void SetProbedBedPlane();
float GetLastProbedZ();
void SetIdentityTransform();
void Transform(float move[]);
void InverseTransform(float move[]);
void Diagnostics();
float ComputeCurrentCoordinate(int8_t drive, LookAhead* la, DDA* runningDDA);
bool zProbing;
friend class DDA;
private:
bool DDARingAdd(LookAhead* lookAhead);
DDA* DDARingGet();
bool DDARingEmpty();
bool NoLiveMovement();
bool DDARingFull();
bool GetDDARingLock();
void ReleaseDDARingLock();
bool LookAheadRingEmpty();
bool LookAheadRingFull();
bool LookAheadRingAdd(float ep[], float vv, bool ce);
LookAhead* LookAheadRingGet();
int8_t GetMovementType(float sp[], float ep[]);
Platform* platform;
GCodes* gCodes;
DDA* dda;
DDA* ddaRingAddPointer;
DDA* ddaRingGetPointer;
volatile bool ddaRingLocked;
LookAhead* lookAheadRingAddPointer;
LookAhead* lookAheadRingGetPointer;
LookAhead* lastMove;
DDA* lookAheadDDA;
int lookAheadRingCount;
float lastTime;
bool addNoMoreMoves;
bool active;
bool checkEndStopsOnNextMove;
float currentFeedrate;
float nextMove[DRIVES + 1]; // Extra is for feedrate
float stepDistances[(1<<AXES)]; // Index bits: lsb -> dx, dy, dz <- msb
float extruderStepDistances[(1<<(DRIVES-AXES))]; // NB - limits us to 5 extruders
float aX, aY, aC;
float lastZHit;
};
//********************************************************************************************************
inline LookAhead* LookAhead::Next()
{
return next;
}
inline LookAhead* LookAhead::Previous()
{
return previous;
}
inline void LookAhead::SetV(float vv)
{
v = vv;
}
inline float* LookAhead::EndPoint()
{
return endPoint;
}
inline float LookAhead::V()
{
return v;
}
inline int8_t LookAhead::Processed()
{
return processed;
}
inline void LookAhead::SetProcessed(MovementState ms)
{
if(ms == unprocessed)
processed = unprocessed;
else
processed |= ms;
}
inline void LookAhead::Release()
{
SetProcessed(released);
}
inline bool LookAhead::CheckEndStops()
{
return checkEndStops;
}
inline void LookAhead::SetDriveZeroEndSpeed(float a, int8_t drive)
{
endPoint[drive] = a;
cosine = 2.0;
v = 0.0;
}
//******************************************************************************************************
inline bool DDA::Active()
{
return active;
}
inline DDA* DDA::Next()
{
return next;
}
inline float DDA::InstantDv()
{
return instantDv;
}
//***************************************************************************************
inline bool Move::DDARingEmpty()
{
return ddaRingGetPointer == ddaRingAddPointer;
}
inline bool Move::NoLiveMovement()
{
if(dda != NULL)
return false;
return DDARingEmpty();
}
// Leave a gap of 2 as the last Get result may still be being processed
inline bool Move::DDARingFull()
{
return ddaRingAddPointer->Next()->Next() == ddaRingGetPointer;
}
inline bool Move::LookAheadRingEmpty()
{
return lookAheadRingCount == 0;
}
// Leave a gap of 2 as the last Get result may still be being processed
inline bool Move::LookAheadRingFull()
{
if(!(lookAheadRingAddPointer->Processed() & released))
return true;
return lookAheadRingAddPointer->Next()->Next() == lookAheadRingGetPointer; // probably not needed; just return the bool in the if above
}
inline bool Move::GetDDARingLock()
{
if(ddaRingLocked)
return false;
ddaRingLocked = true;
return true;
}
inline void Move::ReleaseDDARingLock()
{
ddaRingLocked = false;
}
// To wait until all the current moves in the buffers are
// complete, call this function repeatedly and wait for it to
// return true. Then do whatever you wanted to do after all
// current moves have finished. THEN CALL THE ResumeMoving() FUNCTION
// OTHERWISE NOTHING MORE WILL EVER HAPPEN.
inline bool Move::AllMovesAreFinished()
{
addNoMoreMoves = true;
return LookAheadRingEmpty() && NoLiveMovement();
}
inline void Move::ResumeMoving()
{
addNoMoreMoves = false;
}
inline void Move::SetZProbing(bool probing)
{
zProbing = probing;
}
inline float Move::GetLastProbedZ()
{
return lastZHit;
}
inline void Move::HitLowStop(int8_t drive, LookAhead* la, DDA* hitDDA)
{
float hitPoint = 0.0;
if(drive == Z_AXIS && zProbing)
{
lastZHit = ComputeCurrentCoordinate(drive, la, hitDDA);
hitPoint = lastZHit;
}
la->SetDriveZeroEndSpeed(hitPoint, drive);
}
inline void Move::HitHighStop(int8_t drive, LookAhead* la, DDA* hitDDA)
{
la->SetDriveZeroEndSpeed(platform->AxisLength(drive), drive);
}
inline float Move::ComputeCurrentCoordinate(int8_t drive, LookAhead* la, DDA* runningDDA)
{
if(runningDDA->totalSteps <= 0)
return 0.0;
return la->Previous()->endPoint[drive] + (la->endPoint[drive] - la->Previous()->endPoint[drive])*(float)runningDDA->stepCount/(float)runningDDA->totalSteps;
}
#endif
|