Martin Smith
/
ERIC_First_Steps
Walk_Forward.cpp
- Committer:
- ms523
- Date:
- 2011-08-21
- Revision:
- 0:0965dacb3caf
File content as of revision 0:0965dacb3caf:
#include "ERIC.h" #define Z_OFFSET 150 // Offset for Z-axis #define RATE 2 // Time in seconds to complete one step #define ELLIPSE_HGT 20 // b constant for ellipse calculation //Serial pc(USBTX,USBRX); // To communicate with TeraTerm void Walk_Forward(int stride) { Timer t; // Used to control the ellipse float RHS_Y, RHS_Z; // Variables for the right leg float LHS_Y, LHS_Z; // Variables for the left leg float a = stride / 2; // The a constant for ellipse calculation float angle = 360 / RATE; // The constant to make algorithm calculate for full 360° angle = angle * PI / 180; // Convert into radians float time = 0; // Variable to hold the timer value. // Create variables to hold the joint angles & current set points Joint_Angles LHS, RHS; // Variables for joint angles t.start(); // Start the timer while (t < RATE) { time = t.read(); // Read the timer // For the right leg... RHS_Y = sin(time*angle); // Calculate the Y position RHS_Y = a * RHS_Y; RHS_Z = cos(time*angle); // Calculate the Z position and correct for Z-axis offset RHS_Z = ELLIPSE_HGT * RHS_Z; RHS_Z = RHS_Z - Z_OFFSET; // For the left leg... LHS_Y = sin((time+RATE/2)*angle); // Calculate the Y position LHS_Y = a * LHS_Y; LHS_Z = cos((time+RATE/2)*angle); // Calculate the Z position and correct for Z-axis offset LHS_Z = ELLIPSE_HGT * LHS_Z; LHS_Z = LHS_Z - Z_OFFSET; // Calculate the required joint angles... RHS = IK_Engine(70,RHS_Y,RHS_Z); LHS = IK_Engine(-70,LHS_Y,LHS_Z); // Translate the joint angles into Servo Angles RHS.lateral = 165 - RHS.lateral; RHS.hip = 340 - RHS.hip; RHS.knee = 320 - RHS.knee; LHS.lateral = LHS.lateral + 135; LHS.hip = LHS.hip - 40; LHS.knee = LHS.knee - 20; // And finally move the leg to the current set point Right_Hip_Lateral.SetGoal((int)RHS.lateral); Right_Hip.SetGoal((int)RHS.hip); Right_Knee.SetGoal((int)RHS.knee); Left_Hip_Lateral.SetGoal((int)LHS.lateral); Left_Hip.SetGoal((int)LHS.hip); Left_Knee.SetGoal((int)LHS.knee); } }