Carlo Collodi / kangaroo

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.

Dependencies:   QEI mbed

Diff: include/dynamics.hpp

Revision:
34:f3a1273e3361
Parent:
23:112c0be5a7f3
--- a/include/dynamics.hpp	Tue Nov 19 19:12:42 2013 +0000
+++ b/include/dynamics.hpp	Tue Nov 19 20:22:14 2013 +0000
@@ -16,7 +16,13 @@
         void start();
         void run();
         void stop();
-        void updatePose();
+        void updatePose() {
+        
+        // position of foot is (x,y)
+        
+        double theta1 = 2 * atan(sqrt( ( (l2+l3+l4)^2 - x^2 - y^2) / (x^2 + y^2 - (l2+l4-l3)^2) ));
+        double theta2 = atan2(y,x) - atan2(l3*sin(theta1), l2 + l4 + l3*cost(theta1));
+        }
 };
 
 #endif
\ No newline at end of file