Diff: ArthropodIK.cpp

Revision:

1:031a0c78d8d6

Parent:

0:9805c9d36254

Child:

2:6214ab5b94cb

--- a/ArthropodIK.cpp	Thu Jun 25 07:08:02 2015 +0000
+++ b/ArthropodIK.cpp	Fri Jun 26 04:21:43 2015 +0000
@@ -41,12 +41,46 @@
     
 
 
-float * ArthropodSolver::SolveBody(sixDOF_t BodyTarget6D[], float LegPriorPos[], int LegNum) {
-        static float foot_pos_out[NUM_LEGS];
+float * ArthropodSolver::SolveBody(sixDOF_t BodyTarget6D, float LegPriorPos[], int LegNum) {
+        
+        static float foot_pos_out[3];
+        float xp= 0, yp = 0; 
         
-        // IK code 
+        float foot_pos_inter[3]; 
+        float foot_pos_new[3]; 
+        switch (LegNum) {                   // Get the coordinates relative to the robot center rather than the leg axis. 
+            case 0: 
+                xp = HIP_DISP_ORTHO;
+                yp = HIP_DISP_ORTHO;
+                break;
+            case 1: 
+                xp = -HIP_DISP_ORTHO;
+                yp = HIP_DISP_ORTHO;
+                break;
+            case 2: 
+                xp = -HIP_DISP_ORTHO;
+                yp = -HIP_DISP_ORTHO;
+                break;
+            case 3: 
+                xp = HIP_DISP_ORTHO;
+                yp = -HIP_DISP_ORTHO;
+                break;
+            }            
+        foot_pos_new[0] = LegPriorPos[0] + xp + BodyTarget6D.xyz[0];
+        foot_pos_new[1] = LegPriorPos[1] + yp + BodyTarget6D.xyz[1];
+        foot_pos_new[2] = LegPriorPos[2]  +  BodyTarget6D.xyz[2];
         
-        // Loop calls SolveLeg()
+        YawXform(foot_pos_new,foot_pos_inter,BodyTarget6D.ypr[0]); // apply yaw transformation first. 
+        PitchXform(foot_pos_inter, foot_pos_new,BodyTarget6D.ypr[1]);
+        RollXform(foot_pos_new, foot_pos_inter,BodyTarget6D.ypr[2]);
+        
+        foot_pos_out[0] = foot_pos_inter[0] - xp  - BodyTarget6D.xyz[0];
+        foot_pos_out[1] = foot_pos_inter[1] - yp - BodyTarget6D.xyz[0];
+        foot_pos_out[2] = foot_pos_inter[2] - BodyTarget6D.xyz[0];
+        
+       
+       
+       
         
         return foot_pos_out; 
 }