|
@ -12003,7 +12003,7 @@ void prepare_move_to_destination() { |
|
|
else |
|
|
else |
|
|
C2 = (HYPOT2(sx, sy) - (L1_2 + L2_2)) / (2.0 * L1 * L2); |
|
|
C2 = (HYPOT2(sx, sy) - (L1_2 + L2_2)) / (2.0 * L1 * L2); |
|
|
|
|
|
|
|
|
S2 = sqrt(sq(C2) - 1); |
|
|
S2 = sqrt(1 - sq(C2)); |
|
|
|
|
|
|
|
|
// Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
|
|
|
// Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
|
|
|
SK1 = L1 + L2 * C2; |
|
|
SK1 = L1 + L2 * C2; |
|
|