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
|
/*
* Move.h
*
* Created on: 7 Dec 2014
* Author: David
*/
#ifndef MOVE_H_
#define MOVE_H_
#include "RepRapFirmware.h"
#include "MessageType.h"
#include "DDA.h" // needed because of our inline functions
#include "Libraries/Math/Matrix.h"
#ifdef DUET_NG
const unsigned int DdaRingLength = 30;
#else
// We are more memory-constrained on the SAM3X
const unsigned int DdaRingLength = 20;
#endif
#include "DeltaParameters.h"
#include "DeltaProbe.h"
#include "Grid.h"
enum PointCoordinateSet
{
unset = 0,
xSet = 1,
ySet = 2,
zSet = 4,
xyCorrected = 8,
probeError = 16
};
/**
* This is the master movement class. It controls all movement in the machine.
*/
class Move
{
public:
Move(Platform* p, GCodes* g);
void Init(); // Start me up
void Spin(); // Called in a tight loop to keep the class going
void Exit(); // Shut down
void GetCurrentMachinePosition(float m[DRIVES], bool disableMotorMapping) const; // Get the current position in untransformed coords
void GetCurrentUserPosition(float m[DRIVES], uint8_t moveType, uint32_t xAxes) const; // Return the position (after all queued moves have been executed) in transformed coords
int32_t GetEndPoint(size_t drive) const { return liveEndPoints[drive]; } // Get the current position of a motor
void LiveCoordinates(float m[DRIVES], uint32_t xAxes); // Gives the last point at the end of the last complete DDA transformed to user coords
void Interrupt(); // The hardware's (i.e. platform's) interrupt should call this.
void InterruptTime(); // Test function - not used
bool AllMovesAreFinished(); // Is the look-ahead ring empty? Stops more moves being added as well.
void DoLookAhead(); // Run the look-ahead procedure
void HitLowStop(size_t axis, DDA* hitDDA); // What to do when a low endstop is hit
void HitHighStop(size_t axis, DDA* hitDDA); // What to do when a high endstop is hit
void ZProbeTriggered(DDA* hitDDA); // What to do when a the Z probe is triggered
void SetPositions(const float move[DRIVES]); // Force the coordinates to be these
void SetLiveCoordinates(const float coords[DRIVES]); // Force the live coordinates (see above) to be these
void ResetExtruderPositions(); // Resets the extrusion amounts of the live coordinates
void SetXBedProbePoint(size_t index, float x); // Record the X coordinate of a probe point
void SetYBedProbePoint(size_t index, float y); // Record the Y coordinate of a probe point
void SetZBedProbePoint(size_t index, float z, bool wasXyCorrected, bool wasError); // Record the Z coordinate of a probe point
float XBedProbePoint(size_t index) const; // Get the X coordinate of a probe point
float YBedProbePoint(size_t index) const; // Get the Y coordinate of a probe point
float ZBedProbePoint(size_t index) const; // Get the Z coordinate of a probe point
size_t NumberOfProbePoints() const; // How many points to probe have been set? 0 if incomplete
size_t NumberOfXYProbePoints() const; // How many XY coordinates of probe points have been set (Zs may not have been probed yet)
bool AllProbeCoordinatesSet(int index) const; // XY, and Z all set for this one?
bool XYProbeCoordinatesSet(int index) const; // Just XY set for this one?
float GetProbeCoordinates(int count, float& x, float& y, bool wantNozzlePosition) const; // Get pre-recorded probe coordinates
void FinishedBedProbing(int sParam, StringRef& reply); // Calibrate or set the bed equation after probing
float SecondDegreeTransformZ(float x, float y) const; // Used for second degree bed equation
void SetAxisCompensation(int8_t axis, float tangent); // Set an axis-pair compensation angle
float AxisCompensation(int8_t axis) const; // The tangent value
void SetIdentityTransform(); // Cancel the bed equation; does not reset axis angle compensation
void Transform(float move[], uint32_t xAxes, bool useBedCompensation) const; // Take a position and apply the bed and the axis-angle compensations
void InverseTransform(float move[], uint32_t xAxes) const; // Go from a transformed point back to user coordinates
float GetTaperHeight() const { return (useTaper) ? taperHeight : 0.0; }
void SetTaperHeight(float h);
void Diagnostics(MessageType mtype); // Report useful stuff
const DeltaParameters& GetDeltaParams() const { return deltaParams; }
DeltaParameters& AccessDeltaParams() { return deltaParams; }
bool IsDeltaMode() const { return deltaParams.IsDeltaMode(); }
const char* GetGeometryString() const;
int GetCoreXYMode() const { return coreXYMode; }
void SetCoreXYMode(int mode) { coreXYMode = mode; }
float GetCoreAxisFactor(size_t axis) const { return axisFactors[axis]; }
void SetCoreAxisFactor(size_t axis, float f) { axisFactors[axis] = f; }
bool IsCoreXYAxis(size_t axis) const; // Return true if the specified axis shares its motors with another
void CurrentMoveCompleted(); // Signal that the current move has just been completed
bool TryStartNextMove(uint32_t startTime); // Try to start another move, returning true if Step() needs to be called immediately
void MotorTransform(const float machinePos[MAX_AXES], int32_t motorPos[MAX_AXES]) const; // Convert Cartesian coordinates to delta motor coordinates
float MotorFactor(size_t drive, const float directionVector[]) const; // Calculate the movement fraction for a single axis motor of a Cartesian or CoreXY printer
void MachineToEndPoint(const int32_t motorPos[], float machinePos[], size_t numDrives) const; // Convert motor coordinates to machine coordinates
void EndPointToMachine(const float coords[], int32_t ep[], size_t numDrives) const;
float IdleTimeout() const { return idleTimeout; } // Returns the idle timeout in seconds
void SetIdleTimeout(float timeout) { idleTimeout = timeout; } // Set the idle timeout in seconds
void Simulate(uint8_t simMode); // Enter or leave simulation mode
float GetSimulationTime() const { return simulationTime; } // Get the accumulated simulation time
void PrintCurrentDda() const; // For debugging
FilePosition PausePrint(float positions[DRIVES], float& pausedFeedRate, uint32_t xAxes); // Pause the print as soon as we can
bool NoLiveMovement() const; // Is a move running, or are there any queued?
int DoDeltaProbe(float frequency, float amplitude, float rate, float distance);
static int32_t MotorEndPointToMachine(size_t drive, float coord); // Convert a single motor position to number of steps
static float MotorEndpointToPosition(int32_t endpoint, size_t drive); // Convert number of motor steps to motor position
bool IsExtruding() const; // Is filament being extruded?
HeightMap& AccessBedProbeGrid() { return grid; } // Access the bed probing grid
private:
enum class IdleState : uint8_t { idle, busy, timing };
bool StartNextMove(uint32_t startTime); // start the next move, returning true if Step() needs to be called immediately
void SetProbedBedEquation(size_t numPoints, StringRef& reply); // When we have a full set of probed points, work out the bed's equation
void DoDeltaCalibration(size_t numPoints, StringRef& reply);
void BedTransform(float move[MAX_AXES], uint32_t xAxes) const; // Take a position and apply the bed compensations
void InverseBedTransform(float move[MAX_AXES], uint32_t xAxes) const; // Go from a bed-transformed point back to user coordinates
void AxisTransform(float move[MAX_AXES]) const; // Take a position and apply the axis-angle compensations
void InverseAxisTransform(float move[MAX_AXES]) const; // Go from an axis transformed point back to user coordinates
void BarycentricCoordinates(size_t p0, size_t p1, // Compute the barycentric coordinates of a point in a triangle
size_t p2, float x, float y, float& l1, // (see http://en.wikipedia.org/wiki/Barycentric_coordinate_system).
float& l2, float& l3) const;
float TriangleZ(float x, float y) const; // Interpolate onto a triangular grid
void AdjustDeltaParameters(const floatc_t v[], size_t numFactors); // Perform delta adjustment
void JustHomed(size_t axis, float hitPoint, DDA* hitDDA); // Deal with setting positions after a drive has been homed
void DeltaProbeInterrupt(); // Step ISR when using the experimental delta probe
static void PrintMatrix(const char* s, const MathMatrix<floatc_t>& m, size_t numRows = 0, size_t maxCols = 0); // for debugging
static void PrintVector(const char *s, const floatc_t *v, size_t numElems); // for debugging
bool DDARingAdd(); // Add a processed look-ahead entry to the DDA ring
DDA* DDARingGet(); // Get the next DDA ring entry to be run
bool DDARingEmpty() const; // Anything there?
DDA* volatile currentDda;
DDA* ddaRingAddPointer;
DDA* volatile ddaRingGetPointer;
DDA* ddaRingCheckPointer;
bool active; // Are we live and running?
uint8_t simulationMode; // Are we simulating, or really printing?
bool waitingForMove; // True if we are waiting for a new move
unsigned int numLookaheadUnderruns; // How many times we have run out of moves to adjust during lookahead
unsigned int numPrepareUnderruns; // How many times we wanted a new move but there were only un-prepared moves in the queue
unsigned int idleCount; // The number of times Spin was called and had no new moves to process
uint32_t longestGcodeWaitInterval; // the longest we had to wait for a new gcode
uint32_t gcodeWaitStartTime; // When we last asked for a gcode and didn't get one
float simulationTime; // Print time since we started simulating
volatile float liveCoordinates[DRIVES]; // The endpoint that the machine moved to in the last completed move
volatile bool liveCoordinatesValid; // True if the XYZ live coordinates are reliable (the extruder ones always are)
volatile int32_t liveEndPoints[DRIVES]; // The XYZ endpoints of the last completed move in motor coordinates
// Variable for G32 bed probing, for bed compensation and delta calibration
float xBedProbePoints[MaxProbePoints]; // The X coordinates of the points on the bed at which to probe
float yBedProbePoints[MaxProbePoints]; // The Y coordinates of the points on the bed at which to probe
float zBedProbePoints[MaxGridProbePoints]; // The Z coordinates of the points on the bed that were probed
float baryXBedProbePoints[5]; // The X coordinates of the triangle corner points
float baryYBedProbePoints[5]; // The Y coordinates of the triangle corner points
float baryZBedProbePoints[5]; // The Z coordinates of the triangle corner points
uint8_t probePointSet[MaxProbePoints]; // Has the XY of this point been set? Has the Z been probed?
float aX, aY, aC; // Bed plane explicit equation z' = z + aX*x + aY*y + aC
float tanXY, tanYZ, tanXZ; // Axis compensation - 90 degrees + angle gives angle between axes
int numBedCompensationPoints; // The number of points we are actually using for bed compensation, 0 means identity bed transform
float xRectangle, yRectangle; // The side lengths of the rectangle used for second-degree bed compensation
float taperHeight; // Height over which we taper
float recipTaperHeight; // Reciprocal of the taper height
bool useTaper; // True to taper off the compensation
HeightMap grid; // Grid definition and height map for G29 bed probing. The probe heights are stored in zBedProbePoints, see above.
float idleTimeout; // How long we wait with no activity before we reduce motor currents to idle
float lastMoveTime; // The approximate time at which the last move was completed, or 0
float longWait; // A long time for things that need to be done occasionally
IdleState iState; // whether the idle timer is active
DeltaParameters deltaParams; // Information about the delta parameters of this machine
DeltaProbe deltaProbe; // Delta probing state
uint32_t deltaProbingStartTime;
bool deltaProbing;
int coreXYMode; // 0 = Cartesian, 1 = CoreXY, 2 = CoreXZ, 3 = CoreYZ
float axisFactors[MAX_AXES]; // How much further the motors need to move for each axis movement, on a CoreXY/CoreXZ/CoreYZ machine
unsigned int stepErrors; // count of step errors, for diagnostics
};
//******************************************************************************************************
inline bool Move::DDARingEmpty() const
{
return ddaRingGetPointer == ddaRingAddPointer;
}
inline bool Move::NoLiveMovement() const
{
return DDARingEmpty() && currentDda == nullptr; // must test currentDda and DDARingEmpty *in this order* !
}
// 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 ResumeMoving() otherwise nothing more will ever happen.
inline bool Move::AllMovesAreFinished()
{
return NoLiveMovement();
}
// Start the next move. Must be called with interrupts disabled, to avoid a race with the step ISR.
inline bool Move::StartNextMove(uint32_t startTime)
pre(ddaRingGetPointer->GetState() == DDA::frozen)
{
currentDda = ddaRingGetPointer;
return currentDda->Start(startTime);
}
// This is the function that is called by the timer interrupt to step the motors.
inline void Move::Interrupt()
{
if (currentDda != nullptr)
{
do
{
} while (currentDda->Step());
}
else if (deltaProbing)
{
DeltaProbeInterrupt();
}
}
#endif /* MOVE_H_ */
|