Arduino Code: Kinematic Function for Servo Arm

Here is the arduino function that determines the location of the tip of the robot arm in 3D cartesian space using kinematics.

Note: While this function was designed to be used with the 3D printed microservo arm being built in this project. You may change the physical parameters of the arm to match those of any servo arm and the code will work perfectly.

The code below has the structure definition needed to setup the function and then the function itself. Soon an example program will be posted with this function implemented.


struct xyzPos{                //a struct to hold multple returned arguments for the 3D position
  float x;
  float y;
  float z;
};

struct xyzPos currentPos;     //current location of tip of arm
struct xyzPos desiredPos;     //desired location of tip of arm

//+++++++++++++++FUNCTION DECLARATIONS+++++++++++++++++++++++++++

void servoControl (int thePos, int theSpeed, Servo theServo);
struct xyzPos kinematics(float baseAng, float shoulderAng, float wristAng);

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

IN THIS AREA IS WHERE THE OTHER ARDUINO LOOP CODE WOULD BE
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++


struct xyzPos kinematics(float baseAng, float shoulderAng, float wristAng){
  
    // this function takes the servo angles as input and computes the locaiton of the tip of the arm in 3D space
    Serial.println("in kinematics");
    
    struct xyzPos POS;

    // the physical parameters of the arm.
    float baseHeight   = 3;
    float upperArm     = 5;
    float foreArm      = 0;       // left at 0 since current arm does not have wriist
    double gripper     = .5;

    //convert servo angles to radians for trig functions
    baseAng = (baseAng * PI /180);
    shoulderAng = (shoulderAng * PI /180);
    wristAng = (wristAng * PI /180);

    // kinematic equations of the robot arm. These equations describe where it is based on the angle of the arm. 
    POS.x = (upperArm*cos(baseAng)*cos(shoulderAng))   +    (cos(baseAng)*(foreArm*cos(shoulderAng+wristAng))   );
    POS.y = baseHeight + (upperArm*sin(shoulderAng)) + (foreArm*sin(shoulderAng+wristAng)) ;
    POS.z = (upperArm *(-sin(baseAng))*(cos(shoulderAng))) - (sin(baseAng) * (      (foreArm*cos(shoulderAng+wristAng))          ));

    //Prints used for error checking if necessary
    /* 
    Serial.print("xpos = ");
    Serial.println(POS.x);
    Serial.print("ypos = ");
    Serial.println(POS.y);
    Serial.print("zpos = ");
    Serial.println(POS.z); */
    
    Serial.println ("done with function");
    
    return POS; //return the #D position of the arm
  
}

If you would like to learn more about using structures in c here are some explanations.