Revision 0:fdc9474c08b9, committed 2011-01-28
- Comitter:
- ms523
- Date:
- Fri Jan 28 19:45:20 2011 +0000
- Commit message:
- First test of a robotic leg
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12.lib Fri Jan 28 19:45:20 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/chris/code/AX12/#ced71d1b2558
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GetAngles.cpp Fri Jan 28 19:45:20 2011 +0000
@@ -0,0 +1,32 @@
+#include "ServoAngles.h"
+
+LegAngles GetAngles(int X, int Y) {
+// Begin by working out L
+ float L = (X*X) + (Y*Y);
+ L = sqrt(L);
+
+// Work out the Knee angle
+ float Knee = (FEMUR*FEMUR)+(TIBIA*TIBIA)-(L*L);
+ Knee = Knee / (2*FEMUR*TIBIA);
+ Knee = acos(Knee);
+
+// Work out Alpha
+ float Alpha = (FEMUR*FEMUR)+(L*L)-(TIBIA*TIBIA);
+ Alpha = Alpha / (2*FEMUR*L);
+ Alpha = acos(Alpha);
+
+// Work out Beta
+ float Beta = (float) X/(float) Y;
+ Beta = atan(Beta);
+
+// Finally work out the Hip angle
+ float Hip = PI - Alpha - Beta;
+
+// Convert the angles to degrees...
+ LegAngles Angles;
+ Angles.Knee = Knee * 180 / PI - 30;
+ Angles.Hip = 360 - (Hip * 180 / PI) - 30;
+
+// Return the hip and knee angles...
+ return(Angles);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Globals.cpp Fri Jan 28 19:45:20 2011 +0000
@@ -0,0 +1,6 @@
+#include "ServoAngles.h"
+
+/**************************************************************************
+Global variables
+**************************************************************************/
+Serial PC(USBTX, USBRX);
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ServoAngles.h Fri Jan 28 19:45:20 2011 +0000
@@ -0,0 +1,27 @@
+#include "mbed.h"
+#include "AX12.h"
+
+/**************************************************************************
+Global Defines
+**************************************************************************/
+#define FEMUR 100 //Femer length
+#define TIBIA 140 //Tibia length
+#define PI 3.1415926
+
+/**************************************************************************
+Global structures
+**************************************************************************/
+struct LegAngles {
+ float Hip;
+ float Knee;
+};
+
+/**************************************************************************
+Function Declarations
+**************************************************************************/
+LegAngles GetAngles(int X, int Y);
+
+/**************************************************************************
+Global variables
+**************************************************************************/
+extern Serial PC;
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Jan 28 19:45:20 2011 +0000
@@ -0,0 +1,32 @@
+#include "ServoAngles.h"
+
+int main() {
+ AX12 AX12_Hip(p9, p10, 1), AX12_Knee(p9, p10, 2);
+ Timer t;
+ float Time;
+ int x,y; // x & y positions for the foot
+ LegAngles Answer; // Struct containing joint angles
+
+ t.start();
+ while (1) {
+ Time = t.read();
+ if (Time>=2){ // Work on a 4 second cycle
+ t.reset();
+ Time = 0; // Reset Time
+ }
+ if (Time<=1) { // Leg on it's forward stroke
+ x = (200 * Time) - 100;
+ y = 200;
+ } else { // Leg on it's backward stroke
+ x = (200 * (2 - Time)) - 100;
+ y = 195;
+ }
+ Answer = GetAngles(x,y); // Work out the joint angles
+
+ // Print the answers to terminal to debug
+ PC.printf("\n\rX = %d, Hip = %.2f, Knee = %.2f",x,Answer.Hip,Answer.Knee);
+
+ AX12_Hip.SetGoal(Answer.Hip);
+ AX12_Knee.SetGoal(Answer.Knee);
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld Fri Jan 28 19:45:20 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9a9732ce53a1
Martin Smith