b30e6ad4e1
Fixed bug that caused the cooling fan PWM frequency to be 165kHz. It is now 25kHz. Fan RPM is now included in the web interface status response (thanks zpl). Many more error messages are now reported to the web interface as well as the USB interface. Interrupt-driven network timer task to better cope with slow SD card writes (thanks zpl) Module SamNonDuePin is now used for all digital and PWM pin I/O.
678 lines
24 KiB
C++
678 lines
24 KiB
C++
/****************************************************************************************************
|
|
|
|
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 30
|
|
#define LOOK_AHEAD 20 // Must be less than LOOK_AHEAD_RING_LENGTH
|
|
|
|
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
|
|
};
|
|
|
|
// The possible states of a movement in the look-ahead ring as the look-ahead is
|
|
// being done.
|
|
|
|
enum MovementState
|
|
{
|
|
unprocessed = 0,
|
|
vCosineSet = 1,
|
|
upPass = 2,
|
|
complete = 4,
|
|
released = 8
|
|
};
|
|
|
|
|
|
enum PointCoordinateSet
|
|
{
|
|
unset = 0,
|
|
xSet = 1,
|
|
ySet = 2,
|
|
zSet = 4
|
|
};
|
|
|
|
/**
|
|
* This class implements a look-ahead buffer for moves. It allows colinear
|
|
* moves not to decelerate between them, sets velocities at ends and beginnings
|
|
* for angled moves, and so on. Entries are joined in a doubly-linked list
|
|
* to form a ring buffer.
|
|
*/
|
|
class LookAhead
|
|
{
|
|
friend class Move;
|
|
friend class DDA;
|
|
|
|
protected:
|
|
|
|
LookAhead(Move* m, Platform* p, LookAhead* n);
|
|
void Init(long ep[], float requsestedFeedRate, float minSpeed, // Set up this move
|
|
float maxSpeed, float acceleration, EndstopChecks ce);
|
|
LookAhead* Next() const; // Next one in the ring
|
|
LookAhead* Previous() const; // Previous one in the ring
|
|
const long* MachineCoordinates() const; // Endpoints of a move in machine coordinates
|
|
float MachineToEndPoint(int8_t drive) const; // Convert a move endpoint to real mm coordinates
|
|
static float MachineToEndPoint(int8_t drive, long coord); // Convert any number to a real coordinate
|
|
static long EndPointToMachine(int8_t drive, float coord); // Convert real mm to a machine coordinate
|
|
float FeedRate() const; // How fast is the set speed for this move
|
|
float MinSpeed() const; // What is the slowest that this move can be
|
|
float MaxSpeed() const; // What is the fastest this move can be
|
|
float Acceleration() const; // What is the acceleration available for this move
|
|
float V() const; // The speed at the end of the move
|
|
void SetV(float vv); // Set the end speed
|
|
void SetFeedRate(float f); // Set the desired feedrate
|
|
int8_t Processed() const; // Where we are in the look-ahead prediction sequence
|
|
void SetProcessed(MovementState ms); // Set where we are the the look ahead processing
|
|
void SetDriveCoordinateAndZeroEndSpeed(float a, int8_t drive); // Force an end point and set its speed to stopped
|
|
EndstopChecks EndStopsToCheck() const; // Which endstops we are checking on this move
|
|
void Release(); // This move has been processed and executed
|
|
void PrintMove(); // Print diagnostics
|
|
|
|
private:
|
|
|
|
Move* move; // The main movement control class
|
|
Platform* platform; // The RepRap machine
|
|
LookAhead* next; // Next entry in the ring
|
|
LookAhead* previous; // Previous entry in the ring
|
|
long endPoint[DRIVES+1]; // Machine coordinates of the endpoint. Should never use the +1, but safety first
|
|
float Cosine(); // The angle between the previous move and this one
|
|
EndstopChecks endStopsToCheck; // Endstops to check for this move
|
|
float cosine; // Store for the cosine value - the function uses lazy evaluation
|
|
float v; // The feedrate we can actually do
|
|
float requestedFeedrate; // The requested feedrate
|
|
float minSpeed; // The slowest that this move may run at
|
|
float maxSpeed; // The fastest this move may run at
|
|
float acceleration; // The fastest acceleration allowed
|
|
volatile int8_t processed; // The stage in the look ahead process that this move is at.
|
|
};
|
|
|
|
/**
|
|
* This implements an integer space machine coordinates Bressenham-style DDA to step the drives.
|
|
* DDAs are stored in a linked list forming a ring buffer.
|
|
*/
|
|
class DDA
|
|
{
|
|
friend class Move;
|
|
friend class LookAhead;
|
|
|
|
protected:
|
|
|
|
DDA(Move* m, Platform* p, DDA* n);
|
|
MovementProfile Init(LookAhead* lookAhead, float& u, float& v); // Set up the DDA. Also used experimentally in look ahead.
|
|
void Start(); // Start executing the DDA. I.e. move the move.
|
|
void Step(); // Take one step of the DDA. Called by timed interrupt.
|
|
bool Active() const;
|
|
DDA* Next(); // Next entry in the ring
|
|
float InstantDv() const;
|
|
|
|
private:
|
|
|
|
MovementProfile AccelerationCalculation(float& u, float& v, // Compute acceleration profiles
|
|
MovementProfile result);
|
|
|
|
Move* move; // The main movement control class
|
|
Platform* platform; // The RepRap machine
|
|
DDA* next; // The next one in the ring
|
|
LookAhead* myLookAheadEntry; // The look-ahead entry corresponding to this DDA
|
|
long counter[DRIVES]; // Step counters
|
|
long delta[DRIVES]; // How far to move each drive
|
|
bool directions[DRIVES]; // Forwards or backwards?
|
|
long totalSteps; // Total number of steps for this move
|
|
long stepCount; // How many steps we have already taken
|
|
EndstopChecks endStopsToCheck; // Which endstops we are checking
|
|
float timeStep; // The current timestep (seconds)
|
|
float velocity; // The current velocity
|
|
long stopAStep; // The stepcount at which we stop accelerating
|
|
long startDStep; // The stepcount at which we start decelerating
|
|
float distance; // How long is the move in real distance
|
|
float acceleration; // The acceleration to use
|
|
float instantDv; // The lowest possible velocity
|
|
float feedRate;
|
|
volatile bool active; // Is the DDA running?
|
|
};
|
|
|
|
/**
|
|
* This is the master movement class. It controls all movement in the machine.
|
|
*/
|
|
class Move
|
|
{
|
|
friend class DDA;
|
|
|
|
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
|
|
bool GetCurrentUserPosition(float m[]); // Return the current position in transformed coords if possible. Send false otherwise
|
|
// DANGER!!! the above function is mis-named because it has the side-effect of clearing currentFeedrate!!!
|
|
void LiveCoordinates(float m[]) const; // 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 ResumeMoving(); // Allow moves to be added after a call to AllMovesAreFinished()
|
|
void DoLookAhead(); // Run the look-ahead procedure
|
|
void HitLowStop(int8_t drive, // What to do when a low endstop is hit
|
|
LookAhead* la, DDA* hitDDA);
|
|
void HitHighStop(int8_t drive, // What to do when a high endstop is hit
|
|
LookAhead* la, DDA* hitDDA);
|
|
void SetPositions(float move[]); // Force the coordinates to be these
|
|
void SetFeedrate(float feedRate); // Sometimes we want to override the feedrate
|
|
void SetLiveCoordinates(float coords[]); // Force the live coordinates (see above) to be these
|
|
void SetXBedProbePoint(int index, float x); // Record the X coordinate of a probe point
|
|
void SetYBedProbePoint(int index, float y); // Record the Y coordinate of a probe point
|
|
void SetZBedProbePoint(int index, float z); // Record the Z coordinate of a probe point
|
|
float XBedProbePoint(int index) const; // Get the X coordinate of a probe point
|
|
float YBedProbePoint(int index) const; // Get the Y coordinate of a probe point
|
|
float ZBedProbePoint(int index)const ; // Get the Z coordinate of a probe point
|
|
int NumberOfProbePoints() const; // How many points to probe have been set? 0 if incomplete
|
|
int 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?
|
|
void SetZProbing(bool probing); // Set the Z probe live
|
|
void SetProbedBedEquation(StringRef& reply); // When we have a full set of probed points, work out the bed's equation
|
|
float SecondDegreeTransformZ(float x, float y) const; // Used for second degree bed equation
|
|
float GetLastProbedZ() const; // What was the Z when the probe last fired?
|
|
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[]) const; // Take a position and apply the bed and the axis-angle compensations
|
|
void InverseTransform(float move[]) const; // Go from a transformed point back to user coordinates
|
|
void Diagnostics(); // Report useful stuff
|
|
float ComputeCurrentCoordinate(int8_t drive,// Turn a DDA value back into a real world coordinate
|
|
LookAhead* la, DDA* runningDDA);
|
|
float Normalise(float v[], int8_t dimensions); // Normalise a vector to unit length
|
|
void Absolute(float v[], int8_t dimensions); // Put a vector in the positive hyperquadrant
|
|
float Magnitude(const float v[], int8_t dimensions); // Return the length of a vector
|
|
void Scale(float v[], float scale, // Multiply a vector by a scalar
|
|
int8_t dimensions);
|
|
float VectorBoxIntersection(const float v[], // Compute the length that a vector would have to have to...
|
|
const float box[], int8_t dimensions);// ...just touch the surface of a hyperbox.
|
|
|
|
private:
|
|
|
|
void BedTransform(float move[]) const; // Take a position and apply the bed compensations
|
|
bool GetCurrentMachinePosition(float m[]); // Get the current position in untransformed coords if possible. Return false otherwise
|
|
// DANGER!!! the above function is mis-named because it has the side-effect of clearing currentFeedrate!!!
|
|
void InverseBedTransform(float move[]) const; // Go from a bed-transformed point back to user coordinates
|
|
void AxisTransform(float move[]) const; // Take a position and apply the axis-angle compensations
|
|
void InverseAxisTransform(float move[]) const; // Go from an axis transformed point back to user coordinates
|
|
void BarycentricCoordinates(int8_t p0, int8_t p1, // Compute the barycentric coordinates of a point in a triangle
|
|
int8_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
|
|
bool DDARingAdd(LookAhead* lookAhead); // Add a processed look-ahead entry to the DDA ring
|
|
DDA* DDARingGet(); // Get the next DDA ring entry to be run
|
|
bool DDARingEmpty() const;
|
|
bool NoLiveMovement() const;
|
|
bool DDARingFull() const;
|
|
bool GetDDARingLock(); // Lock the ring so only this function may access it
|
|
void ReleaseDDARingLock(); // Release the DDA ring lock
|
|
bool LookAheadRingEmpty() const; // Anything there?
|
|
bool LookAheadRingFull() const; // Any more room?
|
|
bool LookAheadRingAdd(long ep[], float requestedFeedRate, // Add an entry to the look-ahead ring for processing
|
|
float minSpeed, float maxSpeed,
|
|
float acceleration, EndstopChecks ce);
|
|
LookAhead* LookAheadRingGet(); // Get the next entry from the look-ahead ring
|
|
|
|
Platform* platform; // The RepRap machine
|
|
GCodes* gCodes; // The G Codes processing class
|
|
|
|
// These implement the DDA ring
|
|
|
|
DDA* dda;
|
|
DDA* ddaRingAddPointer;
|
|
DDA* ddaRingGetPointer;
|
|
volatile bool ddaRingLocked;
|
|
|
|
// These implement the look-ahead ring
|
|
|
|
LookAhead* lookAheadRingAddPointer;
|
|
LookAhead* lookAheadRingGetPointer;
|
|
LookAhead* lastMove;
|
|
DDA* lookAheadDDA;
|
|
int lookAheadRingCount;
|
|
|
|
float lastTime; // The last time we were called (secs)
|
|
bool addNoMoreMoves; // If true, allow no more moves to be added to the look-ahead
|
|
bool active; // Are we live and running?
|
|
float currentFeedrate; // Err... the current feed rate...
|
|
volatile float liveCoordinates[DRIVES + 1]; // The last endpoint that the machine moved to
|
|
float nextMove[DRIVES + 1]; // The endpoint of the next move to processExtra entry is for feedrate
|
|
float normalisedDirectionVector[DRIVES]; // Used to hold a unit-length vector in the direction of motion
|
|
long nextMachineEndPoints[DRIVES+1]; // The next endpoint in machine coordinates (i.e. steps)
|
|
float xBedProbePoints[NUMBER_OF_PROBE_POINTS]; // The X coordinates of the points on the bed at which to probe
|
|
float yBedProbePoints[NUMBER_OF_PROBE_POINTS]; // The Y coordinates of the points on the bed at which to probe
|
|
float zBedProbePoints[NUMBER_OF_PROBE_POINTS]; // The Z coordinates of the points on the bed at which to probe
|
|
float baryXBedProbePoints[NUMBER_OF_PROBE_POINTS]; // The X coordinates of the triangle corner points
|
|
float baryYBedProbePoints[NUMBER_OF_PROBE_POINTS]; // The Y coordinates of the triangle corner points
|
|
float baryZBedProbePoints[NUMBER_OF_PROBE_POINTS]; // The Z coordinates of the triangle corner points
|
|
uint8_t probePointSet[NUMBER_OF_PROBE_POINTS]; // 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
|
|
bool identityBedTransform; // Is the bed transform in operation?
|
|
float xRectangle, yRectangle; // The side lengths of the rectangle used for second-degree bed compensation
|
|
volatile float lastZHit; // The last Z value hit by the probe
|
|
bool zProbing; // Are we bed probing as well as moving?
|
|
float longWait; // A long time for things that need to be done occasionally
|
|
};
|
|
|
|
//********************************************************************************************************
|
|
|
|
inline LookAhead* LookAhead::Next() const
|
|
{
|
|
return next;
|
|
}
|
|
|
|
inline LookAhead* LookAhead::Previous() const
|
|
{
|
|
return previous;
|
|
}
|
|
|
|
|
|
inline float LookAhead::MachineToEndPoint(int8_t drive) const
|
|
{
|
|
if(drive >= DRIVES)
|
|
{
|
|
platform->Message(HOST_MESSAGE, "MachineToEndPoint() called for feedrate!\n");
|
|
return 0.0;
|
|
}
|
|
return ((float)(endPoint[drive]))/platform->DriveStepsPerUnit(drive);
|
|
}
|
|
|
|
inline float LookAhead::FeedRate() const
|
|
{
|
|
return requestedFeedrate;
|
|
}
|
|
|
|
inline float LookAhead::MinSpeed() const
|
|
{
|
|
return minSpeed;
|
|
}
|
|
|
|
inline float LookAhead::MaxSpeed() const
|
|
{
|
|
return maxSpeed;
|
|
}
|
|
|
|
inline float LookAhead::Acceleration() const
|
|
{
|
|
return acceleration;
|
|
}
|
|
|
|
inline void LookAhead::SetV(float vv)
|
|
{
|
|
v = vv;
|
|
}
|
|
|
|
inline float LookAhead::V() const
|
|
{
|
|
return v;
|
|
}
|
|
|
|
inline void LookAhead::SetFeedRate(float f)
|
|
{
|
|
requestedFeedrate = f;
|
|
v = f;
|
|
}
|
|
|
|
inline int8_t LookAhead::Processed() const
|
|
{
|
|
return processed;
|
|
}
|
|
|
|
inline void LookAhead::SetProcessed(MovementState ms)
|
|
{
|
|
if(ms == unprocessed)
|
|
processed = unprocessed;
|
|
else
|
|
processed |= ms;
|
|
}
|
|
|
|
inline void LookAhead::Release()
|
|
{
|
|
processed = released;
|
|
}
|
|
|
|
inline EndstopChecks LookAhead::EndStopsToCheck() const
|
|
{
|
|
return endStopsToCheck;
|
|
}
|
|
|
|
// This is called from the step ISR. Any variables it modifies that are also read by code outside the ISR should be declared 'volatile'.
|
|
inline void LookAhead::SetDriveCoordinateAndZeroEndSpeed(float a, int8_t drive)
|
|
{
|
|
endPoint[drive] = EndPointToMachine(drive, a);
|
|
cosine = 2.0;
|
|
v = platform->InstantDv(platform->SlowestDrive());
|
|
}
|
|
|
|
inline const long* LookAhead::MachineCoordinates() const
|
|
{
|
|
return endPoint;
|
|
}
|
|
|
|
//inline int8_t LookAhead::GetMovementType()
|
|
//{
|
|
// return movementType;
|
|
//}
|
|
|
|
//******************************************************************************************************
|
|
|
|
inline bool DDA::Active() const
|
|
{
|
|
return active;
|
|
}
|
|
|
|
inline DDA* DDA::Next()
|
|
{
|
|
return next;
|
|
}
|
|
|
|
inline float DDA::InstantDv() const
|
|
{
|
|
return instantDv;
|
|
}
|
|
|
|
|
|
//***************************************************************************************
|
|
|
|
inline bool Move::DDARingEmpty() const
|
|
{
|
|
return ddaRingGetPointer == ddaRingAddPointer;
|
|
}
|
|
|
|
inline bool Move::NoLiveMovement() const
|
|
{
|
|
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() const
|
|
{
|
|
return ddaRingAddPointer->Next()->Next() == ddaRingGetPointer;
|
|
}
|
|
|
|
inline bool Move::LookAheadRingEmpty() const
|
|
{
|
|
return lookAheadRingCount == 0;
|
|
}
|
|
|
|
// Leave a gap of 2 as the last Get result may still be being processed
|
|
|
|
inline bool Move::LookAheadRingFull() const
|
|
{
|
|
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;
|
|
}
|
|
|
|
inline void Move::LiveCoordinates(float m[]) const
|
|
{
|
|
for(int8_t drive = 0; drive <= DRIVES; drive++)
|
|
{
|
|
m[drive] = liveCoordinates[drive];
|
|
}
|
|
InverseTransform(m);
|
|
}
|
|
|
|
|
|
// These are the actual numbers that we want to be the coordinates, so
|
|
// don't transform them.
|
|
|
|
inline void Move::SetLiveCoordinates(float coords[])
|
|
{
|
|
for(int8_t drive = 0; drive <= DRIVES; drive++)
|
|
{
|
|
liveCoordinates[drive] = coords[drive];
|
|
}
|
|
}
|
|
|
|
// 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::SetXBedProbePoint(int index, float x)
|
|
{
|
|
if(index < 0 || index >= NUMBER_OF_PROBE_POINTS)
|
|
{
|
|
platform->Message(HOST_MESSAGE, "Z probe point X index out of range.\n");
|
|
return;
|
|
}
|
|
xBedProbePoints[index] = x;
|
|
probePointSet[index] |= xSet;
|
|
}
|
|
|
|
inline void Move::SetYBedProbePoint(int index, float y)
|
|
{
|
|
if(index < 0 || index >= NUMBER_OF_PROBE_POINTS)
|
|
{
|
|
platform->Message(HOST_MESSAGE, "Z probe point Y index out of range.\n");
|
|
return;
|
|
}
|
|
yBedProbePoints[index] = y;
|
|
probePointSet[index] |= ySet;
|
|
}
|
|
|
|
inline void Move::SetZBedProbePoint(int index, float z)
|
|
{
|
|
if(index < 0 || index >= NUMBER_OF_PROBE_POINTS)
|
|
{
|
|
platform->Message(HOST_MESSAGE, "Z probe point Z index out of range.\n");
|
|
return;
|
|
}
|
|
zBedProbePoints[index] = z;
|
|
probePointSet[index] |= zSet;
|
|
}
|
|
|
|
inline float Move::XBedProbePoint(int index) const
|
|
{
|
|
return xBedProbePoints[index];
|
|
}
|
|
|
|
inline float Move::YBedProbePoint(int index) const
|
|
{
|
|
return yBedProbePoints[index];
|
|
}
|
|
|
|
inline float Move::ZBedProbePoint(int index) const
|
|
{
|
|
return zBedProbePoints[index];
|
|
}
|
|
|
|
inline void Move::SetZProbing(bool probing)
|
|
{
|
|
zProbing = probing;
|
|
}
|
|
|
|
inline float Move::GetLastProbedZ() const
|
|
{
|
|
return lastZHit;
|
|
}
|
|
|
|
// Note that we don't set the tan values to 0 here. This means that the bed probe
|
|
// values will be a fraction of a millimeter out in X and Y, which, as the bed should
|
|
// be nearly flat (and the probe doesn't coincide with the nozzle anyway), won't matter.
|
|
// But it means that the tan values can be set for the machine
|
|
// at the start in the configuration file and be retained, without having to know and reset
|
|
// them after every Z probe of the bed.
|
|
|
|
inline void Move::SetIdentityTransform()
|
|
{
|
|
identityBedTransform = true;
|
|
}
|
|
|
|
inline bool Move::AllProbeCoordinatesSet(int index) const
|
|
{
|
|
return probePointSet[index] == (xSet | ySet | zSet);
|
|
}
|
|
|
|
inline bool Move::XYProbeCoordinatesSet(int index) const
|
|
{
|
|
return (probePointSet[index] & xSet) && (probePointSet[index] & ySet);
|
|
}
|
|
|
|
inline int Move::NumberOfProbePoints() const
|
|
{
|
|
for(int i = 0; i < NUMBER_OF_PROBE_POINTS; i++)
|
|
{
|
|
if(!AllProbeCoordinatesSet(i))
|
|
return i;
|
|
}
|
|
return NUMBER_OF_PROBE_POINTS;
|
|
}
|
|
|
|
inline int Move::NumberOfXYProbePoints() const
|
|
{
|
|
for(int i = 0; i < NUMBER_OF_PROBE_POINTS; i++)
|
|
{
|
|
if(!XYProbeCoordinatesSet(i))
|
|
return i;
|
|
}
|
|
return NUMBER_OF_PROBE_POINTS;
|
|
}
|
|
|
|
/*
|
|
* Transform to a ruled-surface quadratic. The corner points for interpolation are indexed:
|
|
*
|
|
* ^ [1] [2]
|
|
* |
|
|
* Y
|
|
* |
|
|
* | [0] [3]
|
|
* -----X---->
|
|
*
|
|
* The values of x and y are transformed to put them in the interval [0, 1].
|
|
*/
|
|
inline float Move::SecondDegreeTransformZ(float x, float y) const
|
|
{
|
|
x = (x - xBedProbePoints[0])*xRectangle;
|
|
y = (y - yBedProbePoints[0])*yRectangle;
|
|
return (1.0 - x)*(1.0 - y)*zBedProbePoints[0] + x*(1.0 - y)*zBedProbePoints[3] + (1.0 - x)*y*zBedProbePoints[1] + x*y*zBedProbePoints[2];
|
|
}
|
|
|
|
|
|
// This is called from the step ISR. Any variables it modifies that are also read by code outside the ISR must be declared 'volatile'.
|
|
inline void Move::HitLowStop(int8_t drive, LookAhead* la, DDA* hitDDA)
|
|
{
|
|
float hitPoint = platform->AxisMinimum(drive);
|
|
if(drive == Z_AXIS)
|
|
{
|
|
if(zProbing)
|
|
{
|
|
// Executing G32, so record the Z position at which we hit the end stop
|
|
if (gCodes->GetAxisIsHomed(drive))
|
|
{
|
|
// Z-axis has already been homed, so just record the height of the bed at this point
|
|
lastZHit = ComputeCurrentCoordinate(drive, la, hitDDA);
|
|
la->SetDriveCoordinateAndZeroEndSpeed(lastZHit, drive);
|
|
lastZHit = lastZHit - platform->ZProbeStopHeight();
|
|
}
|
|
else
|
|
{
|
|
// Z axis has not yet been homed, so treat this probe as a homing command
|
|
la->SetDriveCoordinateAndZeroEndSpeed(platform->ZProbeStopHeight(), drive);
|
|
gCodes->SetAxisIsHomed(drive);
|
|
lastZHit = hitPoint;
|
|
}
|
|
return;
|
|
} else
|
|
{
|
|
// Executing G30, so set the current Z height to the value at which the end stop is triggered
|
|
// Transform it first so that the height is correct in user coordinates
|
|
float xyzPoint[DRIVES + 1];
|
|
LiveCoordinates(xyzPoint);
|
|
xyzPoint[Z_AXIS] = lastZHit = platform->ZProbeStopHeight();
|
|
Transform(xyzPoint);
|
|
hitPoint = xyzPoint[Z_AXIS];
|
|
}
|
|
}
|
|
la->SetDriveCoordinateAndZeroEndSpeed(hitPoint, drive);
|
|
gCodes->SetAxisIsHomed(drive);
|
|
}
|
|
|
|
// This is called from the step ISR. Any variables it modifies that are also read by code outside the ISR must be declared 'volatile'.
|
|
inline void Move::HitHighStop(int8_t drive, LookAhead* la, DDA* hitDDA)
|
|
{
|
|
la->SetDriveCoordinateAndZeroEndSpeed(platform->AxisMaximum(drive), drive);
|
|
gCodes->SetAxisIsHomed(drive);
|
|
}
|
|
|
|
inline float Move::ComputeCurrentCoordinate(int8_t drive, LookAhead* la, DDA* runningDDA)
|
|
{
|
|
float previous = la->Previous()->MachineToEndPoint(drive);
|
|
if(runningDDA->totalSteps <= 0)
|
|
return previous;
|
|
return previous + (la->MachineToEndPoint(drive) - previous)*(float)runningDDA->stepCount/(float)runningDDA->totalSteps;
|
|
}
|
|
|
|
inline float Move::AxisCompensation(int8_t axis) const
|
|
{
|
|
switch(axis)
|
|
{
|
|
case X_AXIS:
|
|
return tanXY;
|
|
|
|
case Y_AXIS:
|
|
return tanYZ;
|
|
|
|
case Z_AXIS:
|
|
return tanXZ;
|
|
|
|
default:
|
|
platform->Message(HOST_MESSAGE, "Axis compensation requested for non-existent axis.");
|
|
}
|
|
return 0.0;
|
|
}
|
|
|
|
#endif
|