3d-print/anadyr-marlin
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Add custom types for position (#15204)

Browse Source
This commit is contained in:
Scott Lahteine
2019-09-29 04:25:39 -05:00
committed by GitHub
parent 43d6e9fa43
commit 50e4545255
227 changed files with 3147 additions and 3264 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
Marlin/src/module/scara.cpp
View File

@@ -36,25 +36,25 @@ float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND;
void scara_set_axis_is_at_home(const AxisEnum axis) {
if (axis == Z_AXIS)
current_position[Z_AXIS] = Z_HOME_POS;
current_position.z = Z_HOME_POS;
else {
/**
* SCARA homes XY at the same time
*/
float homeposition[XYZ];
xyz_pos_t homeposition;
LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
// SERIAL_ECHOLNPAIR("homeposition X:", homeposition[X_AXIS], " Y:", homeposition[Y_AXIS]);
// SERIAL_ECHOLNPAIR("homeposition X:", homeposition.x, " Y:", homeposition.y);
/**
* Get Home position SCARA arm angles using inverse kinematics,
* and calculate homing offset using forward kinematics
*/
inverse_kinematics(homeposition);
forward_kinematics_SCARA(delta[A_AXIS], delta[B_AXIS]);
forward_kinematics_SCARA(delta.a, delta.b);
// SERIAL_ECHOLNPAIR("Cartesian X:", cartes[X_AXIS], " Y:", cartes[Y_AXIS]);
// SERIAL_ECHOLNPAIR("Cartesian X:", cartes.x, " Y:", cartes.y);
current_position[axis] = cartes[axis];
@@ -62,8 +62,10 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
}
}
static constexpr xy_pos_t scara_offset = { SCARA_OFFSET_X, SCARA_OFFSET_Y };
/**
* Morgan SCARA Forward Kinematics. Results in cartes[].
* Morgan SCARA Forward Kinematics. Results in 'cartes'.
* Maths and first version by QHARLEY.
* Integrated into Marlin and slightly restructured by Joachim Cerny.
*/
@@ -74,8 +76,8 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
b_sin = sin(RADIANS(b)) * L2,
b_cos = cos(RADIANS(b)) * L2;
cartes[X_AXIS] = a_cos + b_cos + SCARA_OFFSET_X; //theta
cartes[Y_AXIS] = a_sin + b_sin + SCARA_OFFSET_Y; //theta+phi
cartes.set(a_cos + b_cos + scara_offset.x, // theta
a_sin + b_sin + scara_offset.y); // theta+phi
/*
SERIAL_ECHOLNPAIR(
@@ -86,31 +88,32 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
" b_sin=", b_sin,
" b_cos=", b_cos
);
SERIAL_ECHOLNPAIR(" cartes (X,Y) = "(cartes[X_AXIS], ", ", cartes[Y_AXIS], ")");
SERIAL_ECHOLNPAIR(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")");
//*/
}
/**
* Morgan SCARA Inverse Kinematics. Results in delta[].
* Morgan SCARA Inverse Kinematics. Results in 'delta'.
*
* See http://forums.reprap.org/read.php?185,283327
*
* Maths and first version by QHARLEY.
* Integrated into Marlin and slightly restructured by Joachim Cerny.
*/
void inverse_kinematics(const float (&raw)[XYZ]) {
void inverse_kinematics(const xyz_pos_t &raw) {
static float C2, S2, SK1, SK2, THETA, PSI;
float C2, S2, SK1, SK2, THETA, PSI;
float sx = raw[X_AXIS] - SCARA_OFFSET_X, // Translate SCARA to standard X Y
sy = raw[Y_AXIS] - SCARA_OFFSET_Y; // With scaling factor.
// Translate SCARA to standard XY with scaling factor
const xy_pos_t spos = raw - scara_offset;
const float H2 = HYPOT2(spos.x, spos.y);
if (L1 == L2)
C2 = HYPOT2(sx, sy) / L1_2_2 - 1;
C2 = H2 / L1_2_2 - 1;
else
C2 = (HYPOT2(sx, sy) - (L1_2 + L2_2)) / (2.0 * L1 * L2);
C2 = (H2 - (L1_2 + L2_2)) / (2.0 * L1 * L2);
S2 = SQRT(1 - sq(C2));
S2 = SQRT(1.0f - sq(C2));
// Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
SK1 = L1 + L2 * C2;
@@ -119,14 +122,12 @@ void inverse_kinematics(const float (&raw)[XYZ]) {
SK2 = L2 * S2;
// Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
THETA = ATAN2(SK1, SK2) - ATAN2(sx, sy);
THETA = ATAN2(SK1, SK2) - ATAN2(spos.x, spos.y);
// Angle of Arm2
PSI = ATAN2(S2, C2);
delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle
delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor)
delta[C_AXIS] = raw[Z_AXIS];
delta.set(DEGREES(THETA), DEGREES(THETA + PSI), raw.z);
/*
DEBUG_POS("SCARA IK", raw);