Browse Source

MORGAN_SCARA kinematics

Scott Lahteine 9 years ago
parent
commit
e94cb7a380
1 changed files with 33 additions and 23 deletions
  1. 33
    23
      Marlin/Marlin_main.cpp

+ 33
- 23
Marlin/Marlin_main.cpp View File

@@ -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
 

Loading…
Cancel
Save