Martin Smith
/
ERIC_First_Steps
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Walk_Forward.cpp@0:0965dacb3caf, 2011-08-21 (annotated)
- Committer:
- ms523
- Date:
- Sun Aug 21 08:25:00 2011 +0000
- Revision:
- 0:0965dacb3caf
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| ms523 | 0:0965dacb3caf | 1 | #include "ERIC.h" |
| ms523 | 0:0965dacb3caf | 2 | |
| ms523 | 0:0965dacb3caf | 3 | #define Z_OFFSET 150 // Offset for Z-axis |
| ms523 | 0:0965dacb3caf | 4 | #define RATE 2 // Time in seconds to complete one step |
| ms523 | 0:0965dacb3caf | 5 | #define ELLIPSE_HGT 20 // b constant for ellipse calculation |
| ms523 | 0:0965dacb3caf | 6 | |
| ms523 | 0:0965dacb3caf | 7 | //Serial pc(USBTX,USBRX); // To communicate with TeraTerm |
| ms523 | 0:0965dacb3caf | 8 | |
| ms523 | 0:0965dacb3caf | 9 | void Walk_Forward(int stride) { |
| ms523 | 0:0965dacb3caf | 10 | Timer t; // Used to control the ellipse |
| ms523 | 0:0965dacb3caf | 11 | |
| ms523 | 0:0965dacb3caf | 12 | float RHS_Y, RHS_Z; // Variables for the right leg |
| ms523 | 0:0965dacb3caf | 13 | float LHS_Y, LHS_Z; // Variables for the left leg |
| ms523 | 0:0965dacb3caf | 14 | float a = stride / 2; // The a constant for ellipse calculation |
| ms523 | 0:0965dacb3caf | 15 | float angle = 360 / RATE; // The constant to make algorithm calculate for full 360° |
| ms523 | 0:0965dacb3caf | 16 | angle = angle * PI / 180; // Convert into radians |
| ms523 | 0:0965dacb3caf | 17 | float time = 0; // Variable to hold the timer value. |
| ms523 | 0:0965dacb3caf | 18 | |
| ms523 | 0:0965dacb3caf | 19 | // Create variables to hold the joint angles & current set points |
| ms523 | 0:0965dacb3caf | 20 | Joint_Angles LHS, RHS; // Variables for joint angles |
| ms523 | 0:0965dacb3caf | 21 | |
| ms523 | 0:0965dacb3caf | 22 | t.start(); // Start the timer |
| ms523 | 0:0965dacb3caf | 23 | |
| ms523 | 0:0965dacb3caf | 24 | while (t < RATE) { |
| ms523 | 0:0965dacb3caf | 25 | time = t.read(); // Read the timer |
| ms523 | 0:0965dacb3caf | 26 | |
| ms523 | 0:0965dacb3caf | 27 | // For the right leg... |
| ms523 | 0:0965dacb3caf | 28 | RHS_Y = sin(time*angle); // Calculate the Y position |
| ms523 | 0:0965dacb3caf | 29 | RHS_Y = a * RHS_Y; |
| ms523 | 0:0965dacb3caf | 30 | RHS_Z = cos(time*angle); // Calculate the Z position and correct for Z-axis offset |
| ms523 | 0:0965dacb3caf | 31 | RHS_Z = ELLIPSE_HGT * RHS_Z; |
| ms523 | 0:0965dacb3caf | 32 | RHS_Z = RHS_Z - Z_OFFSET; |
| ms523 | 0:0965dacb3caf | 33 | |
| ms523 | 0:0965dacb3caf | 34 | // For the left leg... |
| ms523 | 0:0965dacb3caf | 35 | LHS_Y = sin((time+RATE/2)*angle); // Calculate the Y position |
| ms523 | 0:0965dacb3caf | 36 | LHS_Y = a * LHS_Y; |
| ms523 | 0:0965dacb3caf | 37 | LHS_Z = cos((time+RATE/2)*angle); // Calculate the Z position and correct for Z-axis offset |
| ms523 | 0:0965dacb3caf | 38 | LHS_Z = ELLIPSE_HGT * LHS_Z; |
| ms523 | 0:0965dacb3caf | 39 | LHS_Z = LHS_Z - Z_OFFSET; |
| ms523 | 0:0965dacb3caf | 40 | |
| ms523 | 0:0965dacb3caf | 41 | // Calculate the required joint angles... |
| ms523 | 0:0965dacb3caf | 42 | RHS = IK_Engine(70,RHS_Y,RHS_Z); |
| ms523 | 0:0965dacb3caf | 43 | LHS = IK_Engine(-70,LHS_Y,LHS_Z); |
| ms523 | 0:0965dacb3caf | 44 | |
| ms523 | 0:0965dacb3caf | 45 | // Translate the joint angles into Servo Angles |
| ms523 | 0:0965dacb3caf | 46 | RHS.lateral = 165 - RHS.lateral; |
| ms523 | 0:0965dacb3caf | 47 | RHS.hip = 340 - RHS.hip; |
| ms523 | 0:0965dacb3caf | 48 | RHS.knee = 320 - RHS.knee; |
| ms523 | 0:0965dacb3caf | 49 | LHS.lateral = LHS.lateral + 135; |
| ms523 | 0:0965dacb3caf | 50 | LHS.hip = LHS.hip - 40; |
| ms523 | 0:0965dacb3caf | 51 | LHS.knee = LHS.knee - 20; |
| ms523 | 0:0965dacb3caf | 52 | |
| ms523 | 0:0965dacb3caf | 53 | // And finally move the leg to the current set point |
| ms523 | 0:0965dacb3caf | 54 | Right_Hip_Lateral.SetGoal((int)RHS.lateral); |
| ms523 | 0:0965dacb3caf | 55 | Right_Hip.SetGoal((int)RHS.hip); |
| ms523 | 0:0965dacb3caf | 56 | Right_Knee.SetGoal((int)RHS.knee); |
| ms523 | 0:0965dacb3caf | 57 | Left_Hip_Lateral.SetGoal((int)LHS.lateral); |
| ms523 | 0:0965dacb3caf | 58 | Left_Hip.SetGoal((int)LHS.hip); |
| ms523 | 0:0965dacb3caf | 59 | Left_Knee.SetGoal((int)LHS.knee); |
| ms523 | 0:0965dacb3caf | 60 | } |
| ms523 | 0:0965dacb3caf | 61 | } |