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
|
/*
* CoreXYUKinematics.cpp
*
* Created on: 4 Jun 2017
* Author: Lars
*/
#include "CoreXYUKinematics.h"
#include "GCodes/GCodes.h"
#include "GCodes/GCodeBuffer.h"
#include "Movement/DDA.h"
CoreXYUKinematics::CoreXYUKinematics() : CoreBaseKinematics(KinematicsType::coreXYU)
{
}
// Return the name of the current kinematics
const char *CoreXYUKinematics::GetName(bool forStatusReport) const
{
return (forStatusReport) ? "coreXYU" : "CoreXYU";
}
// Set the parameters from a M665, M666 or M669 command
// Return true if we changed any parameters. Set 'error' true if there was an error, otherwise leave it alone.
bool CoreXYUKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const StringRef& reply, bool& error) /*override*/
{
if (mCode == 669)
{
bool seen = false;
for (size_t axis = 0; axis < CoreXYU_AXES; ++axis)
{
if (gb.Seen(reprap.GetGCodes().GetAxisLetters()[axis]))
{
axisFactors[axis] = gb.GetFValue();
seen = true;
}
}
if (!seen && !gb.Seen('K'))
{
reply.printf("Kinematics is %s with axis factors", GetName(false));
for (size_t axis = 0; axis < CoreXYU_AXES; ++axis)
{
reply.catf(" %c:%3f", reprap.GetGCodes().GetAxisLetters()[axis], (double)axisFactors[axis]);
}
}
return seen;
}
return CoreBaseKinematics::Configure(mCode, gb, reply, error);
}
// Convert Cartesian coordinates to motor coordinates
bool CoreXYUKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const
{
motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[X_AXIS]);
motorPos[Y_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) - (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[Y_AXIS]);
motorPos[Z_AXIS] = lrintf(machinePos[Z_AXIS] * stepsPerMm[Z_AXIS]);
motorPos[U_AXIS] = lrintf(((machinePos[U_AXIS] * axisFactors[U_AXIS]) + (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[U_AXIS]);
motorPos[V_AXIS] = lrintf(((machinePos[U_AXIS] * axisFactors[U_AXIS]) - (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[V_AXIS]);
for (size_t axis = CoreXYU_AXES; axis < numVisibleAxes; ++axis)
{
motorPos[axis] = lrintf(machinePos[axis] * stepsPerMm[axis]);
}
return true;
}
// Convert motor coordinates to machine coordinates. Used after homing and after individual motor moves.
void CoreXYUKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const
{
// Convert the main axes
const float xyStepsMm = stepsPerMm[X_AXIS] * stepsPerMm[Y_AXIS];
const float uvStepsMm = stepsPerMm[U_AXIS] * stepsPerMm[V_AXIS];
machinePos[X_AXIS] = ((motorPos[X_AXIS] * stepsPerMm[Y_AXIS]) + (motorPos[Y_AXIS] * stepsPerMm[X_AXIS]))
/(2 * axisFactors[X_AXIS] * xyStepsMm);
machinePos[Y_AXIS] = ((motorPos[X_AXIS] * stepsPerMm[Y_AXIS]) - (motorPos[Y_AXIS] * stepsPerMm[X_AXIS]))
/(2 * axisFactors[Y_AXIS] * xyStepsMm);
machinePos[U_AXIS] = ((motorPos[U_AXIS] * stepsPerMm[V_AXIS]) + (motorPos[V_AXIS] * stepsPerMm[U_AXIS]))
/(2 * axisFactors[V_AXIS] * uvStepsMm);
machinePos[V_AXIS] = ((motorPos[U_AXIS] * stepsPerMm[V_AXIS]) - (motorPos[V_AXIS] * stepsPerMm[U_AXIS]))
/(2 * axisFactors[V_AXIS] * uvStepsMm);
machinePos[Z_AXIS] = motorPos[Z_AXIS]/stepsPerMm[Z_AXIS];
// Convert any additional axes
for (size_t drive = CoreXYU_AXES; drive < numVisibleAxes; ++drive)
{
machinePos[drive] = motorPos[drive]/stepsPerMm[drive];
}
}
// Return true if the specified endstop axis uses shared motors.
// Used to determine whether to abort the whole move or just one motor when an endstop switch is triggered.
bool CoreXYUKinematics::DriveIsShared(size_t drive) const
{
return drive == X_AXIS || drive == Y_AXIS || drive == U_AXIS
|| drive == V_AXIS; // V doesn't have endstop switches, but include it here just in case
}
// Limit the speed and acceleration of a move to values that the mechanics can handle.
// The speeds along individual Cartesian axes have already been limited before this is called.
void CoreXYUKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const
{
const float vecX = normalisedDirectionVector[0];
const float vecY = normalisedDirectionVector[1];
// Limit the XY motor accelerations
const float vecMaxXY = max<float>(fabs(vecX + vecY), fabs(vecX - vecY)); // pick the case for the motor that is working hardest
if (vecMaxXY > 0.01) // avoid division by zero or near-zero
{
const Platform& platform = reprap.GetPlatform();
const float aX = platform.Acceleration(0);
const float aY = platform.Acceleration(1);
const float vX = platform.MaxFeedrate(0);
const float vY = platform.MaxFeedrate(1);
const float aMax = (fabs(vecX) + fabs(vecY)) * aX * aY/(vecMaxXY * (fabs(vecX) * aY + fabs(vecY) * aX));
const float vMax = (fabs(vecX) + fabs(vecY)) * vX * vY/(vecMaxXY * (fabs(vecX) * vY + fabs(vecY) * vX));
dda.LimitSpeedAndAcceleration(vMax, aMax);
}
// Limit the UV motor accelerations
const float vecU = normalisedDirectionVector[3];
const float vecMaxUV = max<float>(fabs(vecU + vecY), fabs(vecU - vecY)); // pick the case for the motor that is working hardest
if (vecMaxUV > 0.01) // avoid division by zero or near-zero
{
const Platform& platform = reprap.GetPlatform();
const float aU = platform.Acceleration(3);
const float aY = platform.Acceleration(1);
const float vU = platform.MaxFeedrate(3);
const float vY = platform.MaxFeedrate(1);
const float aMax = (fabs(vecU) + fabs(vecY)) * aU * aY/(vecMaxUV * (fabs(vecU) * aY + fabs(vecY) * aU));
const float vMax = (fabs(vecU) + fabs(vecY)) * vU * vY/(vecMaxUV * (fabs(vecU) * vY + fabs(vecY) * vU));
dda.LimitSpeedAndAcceleration(vMax, aMax);
}
}
// End
|
