Advanced Search

Unsupported HAL pin type (1) in mesa_modbus definition file

[attachment=67048]2025-01-1918_53_54-_scara-FreeCAD1.0.0.png[/attachment]

[code]import math
import linuxcnc

def forward_kinematics(joint):
    """
    Forward kinematics for SCARA with parallelogram mechanism and tension-controlled Z movement.
    joint[0] = Theta1 (Base rotation)
    joint[1] = Theta2 (Second arm rotation)
    joint[2] = Z movement via parallelogram
    """
    L1 = 1.2  # First arm length (m)
    L2 = 1.5  # Second arm length (m) to achieve 1.8m z reach
    Z_parallelogram = 0.6  # Parallelogram tension control length (m)
    Z_min, Z_max = -0.9, 0.9  # Define valid Z range
    
    # Clamp Z within limits
    z = max(Z_min, min(joint[2], Z_max))
    
    # Compute effective second arm length with bidirectional movement
    L2_projected = math.sqrt(L2**2 - z**2)
    
    # Compute correct tension-controlled parallelogram movement
   delta_z = math.sqrt((Z_parallelogram - z)**2 + L2_projected**2)
    
    # Compute end-effector position
    x = L1 * math.cos(joint[0]) + L2_projected * math.cos(joint[0] + joint[1])
    y = L1 * math.sin(joint[0]) + L2_projected * math.sin(joint[0] + joint[1])
    
    return (x, y, delta_z)

def inverse_kinematics(x, y, z):
    """
    Inverse kinematics for SCARA with parallelogram mechanism and tension-controlled Z movement.
    """
    L1 = 1.2  # First arm length (m)
    L2 = 1.5  # Second arm length (m) for 1.8m reach
    Z_parallelogram = 0.6  # Parallelogram tension control length (m)
    Z_min, Z_max = -0.9, 0.9  # Define valid Z range
    
    # Clamp Z within limits
    z = max(Z_min, min(z, Z_max))
    
    # Adjust for parallelogram tension effect using correct formula
    L2_projected = math.sqrt(L2**2 - z**2)
    adjusted_z = math.sqrt((Z_parallelogram - z)**2 + L2_projected**2)
    
    # Compute effective second arm length with bidirectional movement
    L2_eff = math.sqrt(L2**2 - adjusted_z**2)
    
    # Compute inverse kinematics
    D = (x**2 + y**2 - L1**2 - L2_eff**2) / (2 * L1 * L2_eff)
    
    if abs(D) > 1:
        raise ValueError("Position out of reach")
    
    theta2 = math.acos(D)
    theta1_1 = math.atan2(y, x) - math.atan2(L2_eff * math.sin(theta2), L1 + L2_eff * math.cos(theta2))
    theta1_2 = math.atan2(y, x) - math.atan2(-L2_eff * math.sin(theta2), L1 + L2_eff * math.cos(theta2))
    
    return [(theta1_1, theta2, adju [attachment=67048]2025-01-19 18_53_54-_ scara - FreeCAD 1.0.0.png[/attachment]sted_z), (theta1_2, theta2, adjusted_z)]  # Two possible solutions



And Z axis should be controlled by some rope wire tensioning paralelogram part.

 

        case 0:
            pos->tran.x = j[0];
            pos->tran.y = j[1];
            pos->tran.z = j[2];
            pos->a      = j[3];
            break;
        case 1:
            pos->tran.x = j[2];
            pos->tran.y = -j[1];
            pos->tran.z = j[0];
            pos->a      = j[3];
            break;
    }
    // unused coordinates:
    pos->b = 0;
    pos->c = 0;
    pos->u = 0;
    pos->v = 0;
    pos->w = 0;

        case 0:
            pos->tran.x = j[0];
            pos->tran.y = j[1];
            pos->tran.z = j[2];
            break;
        case 1:
            pos->tran.x = -j[0];
            pos->tran.y = j[2];
            pos->tran.z = j[1];
            break;
    }
    // unused coordinates:
    pos->a = 0;
    pos->b = 0;
    pos->c = 0;
    pos->u = 0;
    pos->v = 0;
    pos->w = 0;