357 lines
8.5 KiB
C++
357 lines
8.5 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
|
|
|
|
****************************************************************************************************/
|
|
|
|
#include "RepRapFirmware.h"
|
|
|
|
Move::Move(Platform* p, GCodes* g)
|
|
{
|
|
active = false;
|
|
platform = p;
|
|
gCodes = g;
|
|
dda = new DDA(this, platform);
|
|
}
|
|
|
|
void Move::Init()
|
|
{
|
|
char i;
|
|
|
|
for(i = 0; i < DRIVES; i++)
|
|
platform->SetDirection(i, FORWARDS);
|
|
for(i = 0; i <= AXES; i++)
|
|
currentPosition[i] = 0.0;
|
|
lastTime = platform->Time();
|
|
currentFeedrate = START_FEED_RATE;
|
|
float dx = 1.0/platform->DriveStepsPerUnit(X_AXIS);
|
|
float dy = 1.0/platform->DriveStepsPerUnit(Y_AXIS);
|
|
float dz = 1.0/platform->DriveStepsPerUnit(Z_AXIS);
|
|
stepDistances[0] = dx; // Should never be used. Wrong, but safer than 0.0...
|
|
stepDistances[1] = dx;
|
|
stepDistances[2] = dy;
|
|
stepDistances[3] = sqrt(dx*dx + dy*dy);
|
|
stepDistances[4] = dz;
|
|
stepDistances[5] = sqrt(dx*dx + dz*dz);
|
|
stepDistances[6] = sqrt(dy*dy + dz*dz);
|
|
stepDistances[7] = sqrt(dx*dx + dy*dy + dz*dz);
|
|
|
|
active = true;
|
|
}
|
|
|
|
void Move::Exit()
|
|
{
|
|
active = false;
|
|
}
|
|
|
|
void Move::Spin()
|
|
{
|
|
if(!active)
|
|
return;
|
|
Qmove();
|
|
}
|
|
|
|
|
|
void Move::Qmove()
|
|
{
|
|
if(!gCodes->ReadMove(nextMove))
|
|
return;
|
|
|
|
//FIXME
|
|
float u = platform->Jerk(X_AXIS);
|
|
float v = u;
|
|
|
|
boolean work = dda->Init(currentPosition, nextMove, u, v);
|
|
|
|
for(char i = 0; i <= AXES; i++)
|
|
currentPosition[i] = nextMove[i];
|
|
|
|
if(work)
|
|
dda->Start(true);
|
|
}
|
|
|
|
void Move::GetCurrentState(float m[])
|
|
{
|
|
for(char i = 0; i < DRIVES; i++)
|
|
{
|
|
if(i < AXES)
|
|
m[i] = currentPosition[i];
|
|
else
|
|
m[i] = 0.0;
|
|
}
|
|
m[DRIVES] = currentFeedrate;
|
|
}
|
|
|
|
//****************************************************************************************************
|
|
|
|
DDA::DDA(Move* m, Platform* p)
|
|
{
|
|
active = false;
|
|
move = m;
|
|
platform = p;
|
|
}
|
|
|
|
/*
|
|
|
|
This sets up the DDA to take us between two positions and extrude states.
|
|
The start velocity is u, and the end one is v. The requested maximum feedrate
|
|
is in targetPosition[DRIVES].
|
|
|
|
Almost everything that needs to be done to set this up is also useful
|
|
for GCode look-ahead, so this one function is used for both. It flags when
|
|
u and v cannot be satisfied with the distance available and reduces them
|
|
proportionately to give values that can just be achieved, which is why they
|
|
are passed by reference.
|
|
|
|
The return value is true for an actual move, false for a zero-length (i.e. null) move.
|
|
|
|
*/
|
|
boolean DDA::Init(float currentPosition[], float targetPosition[], float& u, float& v)
|
|
{
|
|
char drive;
|
|
active = false;
|
|
velocitiesAltered = false;
|
|
totalSteps = -1;
|
|
distance = 0; // X+Y+Z
|
|
float d;
|
|
char axesMoving = 0;
|
|
|
|
// How far are we going, both in steps and in mm?
|
|
|
|
for(drive = 0; drive < DRIVES; drive++)
|
|
{
|
|
if(drive < AXES)
|
|
{
|
|
d = targetPosition[drive] - currentPosition[drive];
|
|
delta[drive] = (long)(d*platform->DriveStepsPerUnit(drive)); //Absolute
|
|
distance += d*d;
|
|
} else
|
|
delta[drive] = (long)(targetPosition[drive]*platform->DriveStepsPerUnit(drive)); // Relative
|
|
if(delta[drive] >= 0)
|
|
directions[drive] = FORWARDS;
|
|
else
|
|
directions[drive] = BACKWARDS;
|
|
delta[drive] = abs(delta[drive]);
|
|
|
|
// Which axes (if any) are moving?
|
|
|
|
if(drive == X_AXIS && delta[drive] > 0)
|
|
axesMoving |= 1;
|
|
if(drive == Y_AXIS && delta[drive] > 0)
|
|
axesMoving |= 2;
|
|
if(drive == Z_AXIS && delta[drive] > 0)
|
|
axesMoving |= 4;
|
|
|
|
// Keep track of the biggest drive move in totalSteps
|
|
|
|
if(delta[drive] > totalSteps)
|
|
totalSteps = delta[drive];
|
|
}
|
|
|
|
// Not going anywhere?
|
|
|
|
if(totalSteps <= 0)
|
|
return false;
|
|
|
|
// Set up the DDA
|
|
|
|
counter[0] = totalSteps/2;
|
|
for(drive = 1; drive < DRIVES; drive++)
|
|
counter[drive] = counter[0];
|
|
|
|
// Acceleration and velocity calculations
|
|
|
|
distance = sqrt(distance);
|
|
float acc;
|
|
|
|
if(axesMoving|4) // Z involved?
|
|
{
|
|
acc = platform->Acceleration(Z_AXIS);
|
|
velocity = platform->Jerk(Z_AXIS);
|
|
} else // No - only X, Y and Es
|
|
{
|
|
acc = platform->Acceleration(X_AXIS);
|
|
velocity = platform->Jerk(X_AXIS);
|
|
}
|
|
|
|
// If velocities requested are (almost) zero, set them to the jerk
|
|
|
|
if(v < 0.01)
|
|
v = velocity;
|
|
if(u < 0.01)
|
|
u = velocity;
|
|
|
|
// At which DDA step should we stop accelerating?
|
|
|
|
d = 0.5*(targetPosition[DRIVES]*targetPosition[DRIVES] - u*u)/acc; // d = (v1^2 - v0^2)/2a
|
|
stopAStep = (long)((d*totalSteps)/distance);
|
|
|
|
// At which DDA step should we start decelerating?
|
|
|
|
d = 0.5*(v*v - targetPosition[DRIVES]*targetPosition[DRIVES])/acc; // This should be 0 or negative...
|
|
startDStep = totalSteps + (long)((d*totalSteps)/distance);
|
|
|
|
// If acceleration stop is at or after deceleration start, then the distance moved
|
|
// is not enough to get to full speed.
|
|
|
|
if(stopAStep >= startDStep)
|
|
{
|
|
// Work out the point at which to stop accelerating and then
|
|
// immediately start decelerating.
|
|
|
|
dCross = 0.5*(0.5*(v*v - u*u)/acc + distance);
|
|
|
|
if(dCross < 0.0 || dCross > distance)
|
|
{
|
|
// With the acceleration available, it is not possible
|
|
// to satisfy u and v within the distance; reduce u and v
|
|
// proportionately to get ones that work and flag the fact.
|
|
// The result is two velocities that can just be accelerated
|
|
// or decelerated between over the distance to get
|
|
// from one to the other.
|
|
|
|
float k = v/u;
|
|
u = 2.0*acc*distance/(k*k - 1);
|
|
if(u >= 0.0)
|
|
{
|
|
u = sqrt(u);
|
|
v = k*u;
|
|
} else
|
|
{
|
|
v = sqrt(-u);
|
|
u = v/k;
|
|
}
|
|
|
|
dCross = 0.5*(0.5*(v*v - u*u)/acc + distance);
|
|
velocitiesAltered = true;
|
|
}
|
|
|
|
// The DDA steps at which acceleration stops and deceleration starts
|
|
|
|
stopAStep = (long)((dCross*totalSteps)/distance);
|
|
startDStep = stopAStep + 1;
|
|
}
|
|
|
|
// The initial velocity
|
|
|
|
velocity = u;
|
|
|
|
// Sanity check
|
|
|
|
if(velocity <= 0.0)
|
|
{
|
|
velocity = 1.0;
|
|
platform->Message(HOST_MESSAGE, "DDA.Init(): Zero or negative initial velocity!");
|
|
}
|
|
|
|
// How far have we gone?
|
|
|
|
stepCount = 0;
|
|
|
|
// Guess that the first DDA move will be in roughly the direction
|
|
// recorded in axesMoving. This is a simple heuristic, and any
|
|
// small error will be forgotten with the very next step.
|
|
|
|
timeStep = move->stepDistances[axesMoving]/velocity;
|
|
|
|
return true;
|
|
}
|
|
|
|
void DDA::Start(boolean noTest)
|
|
{
|
|
for(char drive = 0; drive < DRIVES; drive++)
|
|
platform->SetDirection(drive, directions[drive]);
|
|
if(noTest)
|
|
platform->SetInterrupt((long)(1.0e6*timeStep));
|
|
active = true;
|
|
}
|
|
|
|
void DDA::Step(boolean noTest)
|
|
{
|
|
if(!active && noTest)
|
|
return;
|
|
|
|
char axesMoving = 0;
|
|
|
|
counter[X_AXIS] += delta[X_AXIS];
|
|
if(counter[X_AXIS] > 0)
|
|
{
|
|
if(noTest)
|
|
platform->Step(X_AXIS);
|
|
axesMoving |= 1;
|
|
counter[X_AXIS] -= totalSteps;
|
|
}
|
|
|
|
counter[Y_AXIS] += delta[Y_AXIS];
|
|
if(counter[Y_AXIS] > 0)
|
|
{
|
|
if(noTest)
|
|
platform->Step(Y_AXIS);
|
|
axesMoving |= 2;
|
|
counter[Y_AXIS] -= totalSteps;
|
|
}
|
|
|
|
counter[Z_AXIS] += delta[Z_AXIS];
|
|
if(counter[Z_AXIS] > 0)
|
|
{
|
|
if(noTest)
|
|
platform->Step(Z_AXIS);
|
|
axesMoving |= 4;
|
|
counter[Z_AXIS] -= totalSteps;
|
|
}
|
|
|
|
|
|
for(char drive = AXES; drive < DRIVES; drive++)
|
|
{
|
|
counter[drive] += delta[drive];
|
|
if(counter[drive] > 0)
|
|
{
|
|
if(noTest)
|
|
platform->Step(drive);
|
|
counter[drive] -= totalSteps;
|
|
}
|
|
}
|
|
|
|
if(axesMoving)
|
|
{
|
|
if(stepCount < stopAStep)
|
|
{
|
|
timeStep = move->stepDistances[axesMoving]/velocity;
|
|
if(axesMoving & 4)
|
|
velocity += platform->Acceleration(Z_AXIS)*timeStep;
|
|
else
|
|
velocity += platform->Acceleration(X_AXIS)*timeStep;
|
|
if(noTest)
|
|
platform->SetInterrupt((long)(1.0e6*timeStep));
|
|
}
|
|
if(stepCount >= startDStep)
|
|
{
|
|
timeStep = move->stepDistances[axesMoving]/velocity;
|
|
if(axesMoving & 4)
|
|
velocity -= platform->Acceleration(Z_AXIS)*timeStep;
|
|
else
|
|
velocity -= platform->Acceleration(X_AXIS)*timeStep;
|
|
if(noTest)
|
|
platform->SetInterrupt((long)(1.0e6*timeStep));
|
|
}
|
|
}
|
|
|
|
stepCount++;
|
|
active = stepCount < totalSteps;
|
|
if(!active && noTest)
|
|
platform->SetInterrupt(-1);
|
|
}
|