NA

Dependencies:   AX12 mbed

Fork of InverseKinematics2 by Martin Smith

Files at this revision

API Documentation at this revision

Comitter:
ms523
Date:
Fri Jan 28 19:45:20 2011 +0000
Commit message:
First test of a robotic leg

Changed in this revision

AX12.lib Show annotated file Show diff for this revision Revisions of this file
GetAngles.cpp Show annotated file Show diff for this revision Revisions of this file
Globals.cpp Show annotated file Show diff for this revision Revisions of this file
ServoAngles.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
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