|
@@ -8345,25 +8345,26 @@ void prepare_move_to_destination() {
|
8345
|
8345
|
|
8346
|
8346
|
#endif // HAS_CONTROLLERFAN
|
8347
|
8347
|
|
8348
|
|
-#if IS_SCARA
|
|
8348
|
+#if ENABLED(MORGAN_SCARA)
|
8349
|
8349
|
|
|
8350
|
+ /**
|
|
8351
|
+ * Morgan SCARA Forward Kinematics. Results in cartes[].
|
|
8352
|
+ * Maths and first version by QHARLEY.
|
|
8353
|
+ * Integrated into Marlin and slightly restructured by Joachim Cerny.
|
|
8354
|
+ */
|
8350
|
8355
|
void forward_kinematics_SCARA(const float &a, const float &b) {
|
8351
|
|
- // Perform forward kinematics, and place results in cartes[]
|
8352
|
|
- // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014
|
8353
|
8356
|
|
8354
|
|
- float a_sin, a_cos, b_sin, b_cos;
|
8355
|
|
-
|
8356
|
|
- a_sin = sin(RADIANS(a)) * L1;
|
8357
|
|
- a_cos = cos(RADIANS(a)) * L1;
|
8358
|
|
- b_sin = sin(RADIANS(b)) * L2;
|
8359
|
|
- b_cos = cos(RADIANS(b)) * L2;
|
|
8357
|
+ float a_sin = sin(RADIANS(a)) * L1,
|
|
8358
|
+ a_cos = cos(RADIANS(a)) * L1,
|
|
8359
|
+ b_sin = sin(RADIANS(b)) * L2,
|
|
8360
|
+ b_cos = cos(RADIANS(b)) * L2;
|
8360
|
8361
|
|
8361
|
8362
|
cartes[X_AXIS] = a_cos + b_cos + SCARA_OFFSET_X; //theta
|
8362
|
8363
|
cartes[Y_AXIS] = a_sin + b_sin + SCARA_OFFSET_Y; //theta+phi
|
8363
|
8364
|
|
8364
|
8365
|
/*
|
8365
|
|
- SERIAL_ECHOPAIR("f_delta x=", a);
|
8366
|
|
- SERIAL_ECHOPAIR(" y=", b);
|
|
8366
|
+ SERIAL_ECHOPAIR("Angle a=", a);
|
|
8367
|
+ SERIAL_ECHOPAIR(" b=", b);
|
8367
|
8368
|
SERIAL_ECHOPAIR(" a_sin=", a_sin);
|
8368
|
8369
|
SERIAL_ECHOPAIR(" a_cos=", a_cos);
|
8369
|
8370
|
SERIAL_ECHOPAIR(" b_sin=", b_sin);
|
|
@@ -8373,29 +8374,38 @@ void prepare_move_to_destination() {
|
8373
|
8374
|
//*/
|
8374
|
8375
|
}
|
8375
|
8376
|
|
|
8377
|
+ /**
|
|
8378
|
+ * Morgan SCARA Inverse Kinematics. Results in delta[].
|
|
8379
|
+ *
|
|
8380
|
+ * See http://forums.reprap.org/read.php?185,283327
|
|
8381
|
+ *
|
|
8382
|
+ * Maths and first version by QHARLEY.
|
|
8383
|
+ * Integrated into Marlin and slightly restructured by Joachim Cerny.
|
|
8384
|
+ */
|
8376
|
8385
|
void inverse_kinematics(const float logical[XYZ]) {
|
8377
|
|
- // Inverse kinematics.
|
8378
|
|
- // Perform SCARA IK and place results in delta[].
|
8379
|
|
- // The maths and first version were done by QHARLEY.
|
8380
|
|
- // Integrated, tweaked by Joachim Cerny in June 2014.
|
8381
|
8386
|
|
8382
|
8387
|
static float C2, S2, SK1, SK2, THETA, PSI;
|
8383
|
8388
|
|
8384
|
8389
|
float sx = RAW_X_POSITION(logical[X_AXIS]) - SCARA_OFFSET_X, // Translate SCARA to standard X Y
|
8385
|
8390
|
sy = RAW_Y_POSITION(logical[Y_AXIS]) - SCARA_OFFSET_Y; // With scaling factor.
|
8386
|
8391
|
|
8387
|
|
- #if (L1 == L2)
|
8388
|
|
- C2 = HYPOT2(sx, sy) / (2 * L1_2) - 1;
|
8389
|
|
- #else
|
8390
|
|
- C2 = (HYPOT2(sx, sy) - L1_2 - L2_2) / 45000;
|
8391
|
|
- #endif
|
|
8392
|
+ if (L1 == L2)
|
|
8393
|
+ C2 = HYPOT2(sx, sy) / L1_2_2 - 1;
|
|
8394
|
+ else
|
|
8395
|
+ C2 = (HYPOT2(sx, sy) - (L1_2 + L2_2)) / (2.0 * L1 * L2);
|
8392
|
8396
|
|
8393
|
|
- S2 = sqrt(1 - sq(C2));
|
|
8397
|
+ S2 = sqrt(sq(C2) - 1);
|
8394
|
8398
|
|
|
8399
|
+ // Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
|
8395
|
8400
|
SK1 = L1 + L2 * C2;
|
|
8401
|
+
|
|
8402
|
+ // Rotated Arm2 gives the distance from Arm1 to Arm2
|
8396
|
8403
|
SK2 = L2 * S2;
|
8397
|
8404
|
|
8398
|
|
- THETA = (atan2(sx, sy) - atan2(SK1, SK2)) * -1;
|
|
8405
|
+ // Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
|
|
8406
|
+ THETA = atan2(SK1, SK2) - atan2(sx, sy);
|
|
8407
|
+
|
|
8408
|
+ // Angle of Arm2
|
8399
|
8409
|
PSI = atan2(S2, C2);
|
8400
|
8410
|
|
8401
|
8411
|
delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle
|
|
@@ -8414,7 +8424,7 @@ void prepare_move_to_destination() {
|
8414
|
8424
|
//*/
|
8415
|
8425
|
}
|
8416
|
8426
|
|
8417
|
|
-#endif // IS_SCARA
|
|
8427
|
+#endif // MORGAN_SCARA
|
8418
|
8428
|
|
8419
|
8429
|
#if ENABLED(TEMP_STAT_LEDS)
|
8420
|
8430
|
|