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

diff -r 000000000000 -r fdc9474c08b9 AX12.lib
--- /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

diff -r 000000000000 -r fdc9474c08b9 GetAngles.cpp
--- /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

diff -r 000000000000 -r fdc9474c08b9 Globals.cpp
--- /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

diff -r 000000000000 -r fdc9474c08b9 ServoAngles.h
--- /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

diff -r 000000000000 -r fdc9474c08b9 main.cpp
--- /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

diff -r 000000000000 -r fdc9474c08b9 mbed.bld
--- /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