Results 1 to 5 of 5

Thread: Delta robot kinematic

  1. Delta robot kinematic

    Hi

    Any onde please help, I need help to write this C code in Structured Text

    // robot geometry
    // (look at pics above for explanation)
    const float e = 115.0; // end effector
    const float f = 457.3; // base
    const float re = 232.0;
    const float rf = 112.0;

    // trigonometric constants
    const float sqrt3 = sqrt(3.0);
    const float pi = 3.141592653; // PI
    const float sin120 = sqrt3/2.0;
    const float cos120 = -0.5;
    const float tan60 = sqrt3;
    const float sin30 = 0.5;
    const float tan30 = 1/sqrt3;

    // forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0)
    // returned status: 0=OK, -1=non-existing position
    int delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) {
    float t = (f-e)*tan30/2;
    float dtr = pi/(float)180.0;

    theta1 *= dtr;
    theta2 *= dtr;
    theta3 *= dtr;

    float y1 = -(t + rf*cos(theta1));
    float z1 = -rf*sin(theta1);

    float y2 = (t + rf*cos(theta2))*sin30;
    float x2 = y2*tan60;
    float z2 = -rf*sin(theta2);

    float y3 = (t + rf*cos(theta3))*sin30;
    float x3 = -y3*tan60;
    float z3 = -rf*sin(theta3);

    float dnm = (y2-y1)*x3-(y3-y1)*x2;

    float w1 = y1*y1 + z1*z1;
    float w2 = x2*x2 + y2*y2 + z2*z2;
    float w3 = x3*x3 + y3*y3 + z3*z3;

    // x = (a1*z + b1)/dnm
    float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
    float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;

    // y = (a2*z + b2)/dnm;
    float a2 = -(z2-z1)*x3+(z3-z1)*x2;
    float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;

    // a*z^2 + b*z + c = 0
    float a = a1*a1 + a2*a2 + dnm*dnm;
    float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
    float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);

    // discriminant
    float d = b*b - (float)4.0*a*c;
    if (d < 0) return -1; // non-existing point

    z0 = -(float)0.5*(b+sqrt(d))/a;
    x0 = (a1*z0 + b1)/dnm;
    y0 = (a2*z0 + b2)/dnm;
    return 0;
    }

    // inverse kinematics
    // helper functions, calculates angle theta1 (for YZ-pane)
    int delta_calcAngleYZ(float x0, float y0, float z0, float &theta) {
    float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
    y0 -= 0.5 * 0.57735 * e; // shift center to edge
    // z = a + b*y
    float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
    float b = (y1-y0)/z0;
    // discriminant
    float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
    if (d < 0) return -1; // non-existing point
    float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point
    float zj = a + b*yj;
    theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0);
    return 0;
    }

    // inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3)
    // returned status: 0=OK, -1=non-existing position
    int delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) {
    theta1 = theta2 = theta3 = 0;
    int status = delta_calcAngleYZ(x0, y0, z0, theta1);
    if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2); // rotate coords to +120 deg
    if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3); // rotate coords to -120 deg
    return status;
    }

  2. #2

    Re: Delta robot kinematic

    Yes that looks like some C code.

    What do you mean by "structured text"?
    If you mean that you want the code to look nicer in the forum post, then use [ code ] and [ /code ] tags (but without the spaces)
    Or click "Advanced" edit button, select the code, and press the hash tag tool in the toolbar (#)

    Code:
    // inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3)
    // returned status: 0=OK, -1=non-existing position
    int delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) {
        theta1 = theta2 = theta3 = 0;
        int status = delta_calcAngleYZ(x0, y0, z0, theta1);
        if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2); // rotate coords to +120 deg
        if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3); // rotate coords to -120 deg
        return status;
    }
    






  3. Re: Delta robot kinematic

    Hi
    Structured text is a high level language that is block structured and syntactically resembles Pascal, on which it is based. It is used when you program a PLC for controling a machine.

    I am used to program Structured text but not C or C++ syntax

    Can you Help?

  4. #4

    Re: Delta robot kinematic

    Then it sounds to me like you need to translate the mathematical equations of the C code above to Structural Text.
    The math is very straightforward. If you have general programming experience, and understand the math of the inverse kinematics, you'll do fine!
    If there's anything in particular you have trouble with, please post a more specific question.

  5. Re: Delta robot kinematic

    Yes your are right, yesterday evening i looked a little more on the code and googled syntax for the C , and have now rewrite the code in ST and it is working well now I will start making the vision and motion planing

Thread Information

Users Browsing this Thread

There are currently 1 users browsing this thread. (0 members and 1 guests)

Similar Threads

  1. Discussion Delta Parallel Robot Project
    By Rens67 in forum Robotics General Discussion
    Replies: 4
    Last Post: 11-15-2015, 11:24 AM
  2. Question(s) My Delta Robot
    By Scope in forum Software and Programming
    Replies: 8
    Last Post: 07-31-2013, 03:26 PM
  3. Delta Robot
    By EdrZero in forum Robotics General Discussion
    Replies: 7
    Last Post: 03-14-2013, 05:09 AM
  4. Project My Inverse kinematic software for controlling robot arms.
    By Pinter75 in forum Project Showcase
    Replies: 1
    Last Post: 09-10-2010, 11:01 PM
  5. Project Delta Robot for Light Manufacturing
    By Intechmr in forum Projects For Request and/or Sale
    Replies: 1
    Last Post: 10-17-2009, 09:42 PM

Tags for this Thread

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •