Added back tool angle for solve, I was trying to solve it for 3 joints...

Author: henriksod

src/FABRIK2D.cpp

@@ -278,7 +278,7 @@ uint8_t Fabrik2D::solve2(
 }
 
 uint8_t Fabrik2D::solve(float x, float y, float toolAngle, int lengths[]) {
-    return 0; // Makes no sense to solve this, remove in the future
+    return solve2(x, y, 0, toolAngle, 0, lengths);
 }
 
 uint8_t Fabrik2D::solve(
@@ -287,7 +287,7 @@ uint8_t Fabrik2D::solve(
     float grippingOffset,
     int lengths[]
 ) {
-    return 0; // Makes no sense to solve this, remove in the future
+    return solve2(x, y, 0, toolAngle, grippingOffset, lengths);
 }
 
 uint8_t Fabrik2D::solve2(float x, float y, float z, int lengths[]) {

test/unit_tests.cpp

@@ -146,6 +146,30 @@ unittest(test_solve)
     assertEqualFloat(100, fabrik2D_3_2DOF.getX(2), fabrik2D_3_2DOF.getTolerance());
     assertEqualFloat(100, fabrik2D_3_2DOF.getY(2), fabrik2D_3_2DOF.getTolerance());
 
+    // Solve 4 joints, 3DOF
+    fprintf(stderr, "Solve 4 joints, 3DOF\n");
+    Fabrik2D fabrik2D_3_3DOF(3, lengths_4_joints, 10);
+    success = fabrik2D_3_3DOF.solve(150, 50, -HALF_PI, lengths_4_joints);
+    assertEqual(1, success);
+    
+    assertEqualFloat(150, fabrik2D_3_3DOF.getX(3), fabrik2D_3_3DOF.getTolerance());
+    assertEqualFloat(50, fabrik2D_3_3DOF.getY(3), fabrik2D_3_3DOF.getTolerance());
+    
+    assertEqualFloat(150, fabrik2D_3_3DOF.getX(2), fabrik2D_3_3DOF.getTolerance());
+    assertEqualFloat(250, fabrik2D_3_3DOF.getY(2), fabrik2D_3_3DOF.getTolerance());
+    
+    // Solve 4 joints, 3DOF, Gripping offset
+    fprintf(stderr, "Solve 4 joints, 3DOF, Gripping offset\n");
+    Fabrik2D fabrik2D_3_3DOF_GO(3, lengths_4_joints, 1);
+    success = fabrik2D_3_3DOF_GO.solve(150, 50, -HALF_PI, 10, lengths_4_joints);
+    assertEqual(1, success);
+    
+    assertEqualFloat(150, fabrik2D_3_3DOF_GO.getX(3), fabrik2D_3_3DOF_GO.getTolerance());
+    assertEqualFloat(60, fabrik2D_3_3DOF_GO.getY(3), fabrik2D_3_3DOF_GO.getTolerance());
+    
+    assertEqualFloat(150, fabrik2D_3_3DOF_GO.getX(2), fabrik2D_3_3DOF_GO.getTolerance());
+    assertEqualFloat(260, fabrik2D_3_3DOF_GO.getY(2), fabrik2D_3_3DOF_GO.getTolerance());
+    
     // Solve 4 joints, 3DOF
     fprintf(stderr, "Solve 4 joints, 3DOF\n");
     Fabrik2D fabrik2D_4_3DOF(4, lengths_4_joints, 1);