Diff: Set_Pose.cpp

Revision:

0:8a7c1e92d067

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Set_Pose.cpp	Sun Oct 16 16:01:08 2011 +0000
@@ -0,0 +1,86 @@
+/**********************************************************************
+ This function puts E.R.I.C. into a standard position
+ regardless of E.R.I.C.'s initial positioning
+ Notebook: http://mbed.org/users/ms523/notebook/eric-initialisation-/
+***********************************************************************/
+#include "FK.h"
+
+void Set_Pose(const int *pose, float duration) {
+    
+    float StartPos[12];         // Array to hold current servo pos
+    FK_Engine(StartPos);        // Get the initial foot positions...
+    
+    // Apply global offsets to allow for hip width
+    StartPos[0] = StartPos[0] + X_OFFSET;       // Fore right leg
+    StartPos[3] = StartPos[3] - X_OFFSET;       // Fore left leg
+    StartPos[6] = StartPos[6] + X_OFFSET;       // Hind right leg
+    StartPos[9] = StartPos[9] - X_OFFSET;       // Hind left leg
+    
+    // Create arrays to hold each servo's distance, velocity and set point
+    int distance[12];
+    float velocity[12];
+    int set_point[12];
+
+    // Calculate the distance to travel in each axis...
+    // Equation: Distance = End Position - Start Position
+    for(int i=0;i<12;i++){
+        distance[i] = pose[i] - StartPos[i];        // Calculate the move distance
+        velocity[i] = distance[i] / duration;       // Calculate the move velocity
+    }
+
+    // Create a timer to control the movement
+    Timer Movement_timer;
+
+    // Create structs to hold joint positions
+    Joint_Angles Hind_left, Hind_right;                 // Variables for joint angles
+    Joint_Angles Fore_left, Fore_right;                 // Variables for joint angles
+
+    Movement_timer.start();                             // Start the movement timer
+    float time = 0;                                     // Create a variable to read the time
+
+    while (time < duration) {
+        // Get the current time
+        time = Movement_timer.read();
+        if (time > duration)
+            time = duration;
+
+        // Calculate current set points
+        for(int i=0;i<12;i++){
+            set_point[i] = (int)(time * velocity[i]) + StartPos[i];
+        }
+   
+        // Calculate the required joint angles...
+        Fore_right = IK_Engine(set_point[0],set_point[1],set_point[2],FORE);
+        Fore_left = IK_Engine(set_point[3],set_point[4],set_point[5],FORE);
+        Hind_right = IK_Engine(set_point[6],set_point[7],set_point[8],HIND);
+        Hind_left = IK_Engine(set_point[9],set_point[10],set_point[11],HIND);
+
+        // Translate the joint angles into Servo Angles       
+        Fore_right.lateral = Fore_right.lateral + 135;
+        Fore_right.hip = 355 - Fore_right.hip;
+        Fore_right.knee = 305 - Fore_right.knee;
+        Fore_left.lateral = 165 - Fore_left.lateral;
+        Fore_left.hip = Fore_left.hip - 55;
+        Fore_left.knee = Fore_left.knee - 5;  
+        Hind_right.lateral = 165 - Hind_right.lateral;
+        Hind_right.hip = 340 - Hind_right.hip;
+        Hind_right.knee = 320 - Hind_right.knee;
+        Hind_left.lateral = Hind_left.lateral + 135;
+        Hind_left.hip = Hind_left.hip - 40;
+        Hind_left.knee = Hind_left.knee - 20;
+
+        // And finally move the leg to the current set point
+        Fore_Right_Hip_Lateral.SetGoal((int)Fore_right.lateral);
+        Fore_Right_Hip.SetGoal((int)Fore_right.hip);
+        Fore_Right_Knee.SetGoal((int)Fore_right.knee);
+        Fore_Left_Hip_Lateral.SetGoal((int)Fore_left.lateral);
+        Fore_Left_Hip.SetGoal((int)Fore_left.hip);
+        Fore_Left_Knee.SetGoal((int)Fore_left.knee);
+        Hind_Right_Hip_Lateral.SetGoal((int)Hind_right.lateral);       
+        Hind_Right_Hip.SetGoal((int)Hind_right.hip);
+        Hind_Right_Knee.SetGoal((int)Hind_right.knee);
+        Hind_Left_Hip_Lateral.SetGoal((int)Hind_left.lateral);
+        Hind_Left_Hip.SetGoal((int)Hind_left.hip);
+        Hind_Left_Knee.SetGoal((int)Hind_left.knee);
+    }
+}
\ No newline at end of file