File content as of revision 0:f15aa1706e16:

#include <math.h>
#include "robot.h"

//Define dimension of robot
Kinematic::Kinematic(float t1x, float t1z, float t2, float t3, float t4x, float t4z) {
    T1X = t1x;
    T1Z = t1z;
    T2  = t2;
    T3  = t3;
    T4X = t4x;
    T4Z = t4z;
}

// Mapping from linear posittion to angular position
void Kinematic::inverse(float x, float y, float z, unsigned int index){
    float k = (630.0/11.0); // radian to degree
    float R31, A1, A2, C1, X1, Y1, Z1, X3, Y3, Z3;
    
    A[index] = atan2(y,x);
    
    X1 = T1X*cos(A[index]);
    Y1 = T1X*sin(A[index]);
    Z1 = T1Z;
    X3 = x - (T4X*cos(A[index]));
    Y3 = y - (T4X*sin(A[index]));
    Z3 = T4Z;
    
    R31 = sqrt(pow((X3-X1),2)+ pow((Y3-Y1),2) + pow((Z3-Z1),2));
    A1 = acos((pow(T2,2) + pow(R31,2) - pow(T3,2))/(2*T2*R31));
    A2 = atan((Z3-Z1)/(sqrt(pow((X3-X1),2) + pow((Y3 - Y1),2)))); 
    C1 = acos((pow(T2,2) + pow(T3,2) - pow(R31,2))/(2*T2*T3));
    
    //printf("%f %f %f %f",R31,A1,A2,C1); 
    B[index] = A1 + A2;
    C[index] = C1;
    
    A[index] = (float) (A[index]*k);          // A in degree
    B[index] = (float) 90.0f - (B[index]*k);  // B in degree
    C[index] = (float) 180.0f - (C[index]*k); // C in degree
    
}

// Mapping from angular posittion to linear position
void Kinematic::forward(float am, float bm, float cm){
    Xm = 0+am;
    Ym = 0+bm;
    Zm = 0+cm;
}
// Data Linker
float Kinematic::GetA(unsigned int i){
    return (float) A[i];
}
float Kinematic::GetB(unsigned int i){
    return (float) B[i];
}
float Kinematic::GetC(unsigned int i){
    return (float) C[i];
}
float Kinematic::GetXm(void){
    return (float) Xm;
}
float Kinematic::GetYm(void){
    return (float) Ym;
}
float Kinematic::GetZm(void){
    return (float) Zm;
}