public static void calculateAcceleration()
{
Logger.logProgress("Calculating Acceleration");
//We need to keep a refernce to the previous segment so that we can adjust it based on the current one
MoveSegment prevMove = new MoveSegment();
double prev_vA = 0;
double prev_vB = 0;
foreach (LayerComponent layer in Global.Values.layerComponentList)
{
foreach (MoveSegment move in layer.intialLayerMoves)
{
acclerateMoves(ref prevMove, move, ref prev_vA, ref prev_vB);
}
foreach (Island island in layer.islandList)
{
foreach (LayerSegment segment in island.segmentList)
{
foreach (MoveSegment move in segment.moveSegments)
{
acclerateMoves(ref prevMove, move, ref prev_vA, ref prev_vB);
}
}
}
}
//Finally we need to make the last move accelerate to rest
MoveSegment stopMove = new MoveSegment(prevMove.endPoint, prevMove.endPoint, 0); //It is not needed to have positions here with the current algorithm but it might become neccesary
acclerateMoves(ref prevMove, stopMove, ref prev_vA, ref prev_vB);
}
private static void writeMoveSegment(MoveSegment segment)
{
//Write the startpoint
binaryWriter.Write(segment.startPoint.X);
binaryWriter.Write(segment.startPoint.Y);
//Write the endpoint
binaryWriter.Write(segment.endPoint.X);
binaryWriter.Write(segment.endPoint.Y);
//Write the start velocity
binaryWriter.Write(segment.startVelocity.X);
binaryWriter.Write(segment.startVelocity.Y);
//Write the peak velocity
binaryWriter.Write(segment.peakVelocity.X);
binaryWriter.Write(segment.peakVelocity.Y);
//Write the end velocity
binaryWriter.Write(segment.endVelocity.X);
binaryWriter.Write(segment.endVelocity.Y);
//Write if isExtruded
binaryWriter.Write(segment.isExtruded);
}
private static void acclerateMoves(ref MoveSegment prevMove, MoveSegment move, ref double prev_vA, ref double prev_vB)
{
//prevMove.gedoen = true;
//prevMove.doen();
//Calculate the change in X and Y positions
long dX = move.endPoint.X - move.startPoint.X;
long dY = move.endPoint.Y - move.startPoint.Y;
/*if (dX == 0 && dY == 0)
* {
* prevMove.convertToCoreXY();
* prevMove = move;
* prev_vA = 0;
* prev_vB = 0;
* return;
* }*/
//Calculate the target velocities in the X and Y axis
double vX = (double)move.feedrate / move.moveDistance * dX;
double vY = (double)move.feedrate / move.moveDistance * dY;
//Calculate the target velocities for the A and B motors
double vA = vX + vY;
double vB = vX - vY;
//Convert the coordinates to CoreXY
prevMove.convertToCoreXY();
//Calculate the change in position on the A and B axis
double dA = prevMove.endPoint.X - prevMove.startPoint.X;
double dB = prevMove.endPoint.Y - prevMove.startPoint.Y;
double max_vA, max_vB, dif_vA, dif_vB;
if (dA == 0 && dB == 0)
{
dif_vA = 0;
dif_vB = 0;
max_vA = 0;
max_vB = 0;
}
else if (Math.Abs(dA) > Math.Abs(dB))
{
var squared = (prevMove.startVelocity.X > 0) ? (prevMove.startVelocity.X * prevMove.startVelocity.X) : -(prevMove.startVelocity.X * prevMove.startVelocity.X);
var other = (2 * Global.Values.maxAccel * dA);
var result = Math.Sqrt(Math.Abs(squared + other));
if (prev_vA > prevMove.startVelocity.X)
{
prevMove.acceleration = new Vector2(Global.Values.maxAccel, (long)(dB / dA * Global.Values.maxAccel));
max_vA = Math.Min(result, prev_vA);
}
else
{
prevMove.acceleration = new Vector2(-Global.Values.maxAccel, (long)(dB / dA * -Global.Values.maxAccel));
max_vA = Math.Max(-result, prev_vA);
}
//Calculate the maximum achieveable velocity in each axis or the target velocity if it can be reached
max_vB = dB / dA * max_vA;
//Calculate the difference between the target velocities for this move and the max achievable ones for the previous one
dif_vA = Math.Abs(vA - max_vA);
dif_vB = Math.Abs(vB - max_vB);
}
else
{
var squared = (prevMove.startVelocity.Y > 0) ? (prevMove.startVelocity.Y * prevMove.startVelocity.Y) : -(prevMove.startVelocity.Y * prevMove.startVelocity.Y);
var other = (2 * Global.Values.maxAccel * dB);
var result = Math.Sqrt(Math.Abs(squared + other));
if (prev_vB > prevMove.startVelocity.Y)
{
prevMove.acceleration = new Vector2(Global.Values.maxAccel, (long)(dA / dB * Global.Values.maxAccel));
max_vB = Math.Min(result, prev_vB);
}
else
{
prevMove.acceleration = new Vector2(-Global.Values.maxAccel, (long)(dA / dB * -Global.Values.maxAccel));
max_vB = Math.Max(-result, prev_vB);
}
//Calculate the maximum achieveable velocity in each axis or the target velocity if it can be reached
max_vA = dA / dB * max_vB;
//Calculate the difference between the target velocities for this move and the max achievable ones for the previous one
dif_vA = Math.Abs(vA - max_vA);
dif_vB = Math.Abs(vB - max_vB);
}
//Calculate the jumping point by scaling the max velocities
double maxDif = Math.Max(dif_vA, dif_vB);
double scaler = (maxDif != 0) ? (Global.Values.maxJump / maxDif) : 0;
prevMove.endVelocity = new Vector2((long)(max_vA * scaler), (long)(max_vB * scaler));
move.startVelocity = new Vector2((long)(vA * scaler), (long)(vB * scaler));
//Now calculate the peak velocity by calculating the intesection between the velocity line from the start and the invert one from the jumping point
///Lines defined in terms of Ax + By = C where:
///A = y2 - y1
///B = x1 - x2
///C = A * x1 + B * y1
if (max_vA == 0 && max_vB == 0)
{
prevMove.peakVelocity = new Vector2(0, 0);
}
else if (Math.Abs(max_vA) > Math.Abs(max_vB))
{
var A1 = max_vA - prevMove.startVelocity.X;
var B1 = -dA; //0 - dA;
var C1 = B1 * prevMove.startVelocity.X; //A1 * 0 + B1 * prevMove.startVelocity.X;
var A2 = max_vA; //(max_vA + prevMove.endVelocity.X) - prevMove.endVelocity.X;
var B2 = dA; //dA - 0;
var C2 = A2 * dA + B2 * prevMove.endVelocity.X;
var det = A1 * B2 - A2 * B1;
var peak_vA = (A1 * C2 - A2 * C1) / det;
//Because the A and B points are in a fixed ratio to each other, we only do the long calculation for A and then us the ratio for B
prevMove.peakVelocity = new Vector2((long)(peak_vA), (long)(dB / dA * peak_vA));
}
else
{
var A1 = max_vB - prevMove.startVelocity.Y;
var B1 = -dB; //0 - dB;
var C1 = B1 * prevMove.startVelocity.Y; //A1 * 0 + B1 * prevMove.startVelocity.Y;
var A2 = max_vB; //(max_vA + prevMove.endVelocity.X) - prevMove.endVelocity.X;
var B2 = dB; //dA - 0;
var C2 = A2 * dB + B2 * prevMove.endVelocity.Y;
var det = A1 * B2 - A2 * B1;
var peak_vB = (A1 * C2 - A2 * C1) / det;
//Because the A and B points are in a fixed ratio to each other, we only do the long calculation for A and then us the ratio for B
prevMove.peakVelocity = new Vector2((long)(dA / dB * peak_vB), (long)(peak_vB));
}
prevMove = move;
prev_vA = vA;
prev_vB = vB;
}
private static void writeMove(MoveSegment segment)
{
if (segment.isRetraction)
{
//If in plotting mode we want to lift up the pen rather than retract
if (Global.Values.materialType == MaterialType.Pen)
{
zLifted = true;
streamWriter.WriteLine("G0 Z1");
return;
}
//The e position should always change so there is no need to check if it changed
var e = " E" + (currentE - (float)segment.extrusionDistance / 1000000f).ToString(CultureInfo.InvariantCulture);
var f = "";
if (segment.feedrate != prev1F)
{
prev1F = segment.feedrate;
f = " F" + prev1F / 1000000f * 60f;
}
streamWriter.WriteLine("G1" + e + f);
retracted = true;
}
else if (segment.moveDistance > 0)
{
if (!segment.isExtruded)
{
var x = "";
var y = "";
var z = "";
var f = "";
var newX = (float)segment.endPoint.X / 1000000f;
var newY = (float)segment.endPoint.Y / 1000000f;
var newZ = (float)segment.endPoint.Z / 1000000f;
if (newX != prevX)
{
prevX = newX;
x = " X" + prevX.ToString(CultureInfo.InvariantCulture);
}
if (newY != prevY)
{
prevY = newY;
y = " Y" + prevY.ToString(CultureInfo.InvariantCulture);
}
if (newZ != prevZ && !zLifted)
{
prevZ = newZ;
z = " Z" + prevZ.ToString(CultureInfo.InvariantCulture);
}
if (segment.feedrate != prev0F)
{
prev0F = segment.feedrate;
f = " F" + prev0F / 1000000f * 60f;
}
streamWriter.WriteLine("G0" + x + y + z + f);
}
else
{
//If the printhead has retracted then we first need to get it back at the correct e before continuing
if (retracted)
{
streamWriter.WriteLine("G1 E" + currentE);
retracted = false;
}
//The e position should always change so there is no need to check if it changed
int layerNumber = (int)(segment.startPoint.Z / Global.Values.layerHeight) - 1;
currentE += (float)segment.extrusionDistance / 1000000f * Global.Values.layerComponentList[layerNumber].flowrate;//Global.Values.flowrates[layerNumber];
var x = "";
var y = "";
var z = "";
var e = " E" + currentE.ToString(CultureInfo.InvariantCulture);
var f = "";
var newX = (float)segment.endPoint.X / 1000000f;
var newY = (float)segment.endPoint.Y / 1000000f;
var newZ = (float)segment.endPoint.Z / 1000000f;
//Put the pen down again if it was lifted
if (zLifted)
{
streamWriter.WriteLine("G1 Z" + newZ.ToString(CultureInfo.InvariantCulture));
zLifted = false;
}
if (newX != prevX)
{
prevX = newX;
x = " X" + prevX.ToString(CultureInfo.InvariantCulture);
}
if (newY != prevY)
{
prevY = newY;
y = " Y" + prevY.ToString(CultureInfo.InvariantCulture);
}
if (newZ != prevZ /* || zLifted*/)
{
prevZ = newZ;
z = " Z" + prevZ.ToString(CultureInfo.InvariantCulture);
}
if (segment.feedrate != prev1F)
{
prev1F = segment.feedrate;
f = " F" + prev1F / 1000000f * 60f;
}
streamWriter.WriteLine("G1" + x + y + z + e + f);
//zLifted = false;
}
}
}
internal abstract void writeMove(MoveSegment segment);
