Diff: robot.cpp

Revision:

0:f15aa1706e16

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.cpp	Mon Dec 14 15:03:32 2015 +0000
@@ -0,0 +1,70 @@
+#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;
+}
+
+
+ 
\ No newline at end of file