Diff: include/dynamics.hpp

Revision:

35:a4e89e78d034

Parent:

28:21773a6fb6df

Child:

37:bf257a0154db

diff -r 21773a6fb6df -r a4e89e78d034 include/dynamics.hpp
--- a/include/dynamics.hpp	Sun Nov 24 19:04:01 2013 +0000
+++ b/include/dynamics.hpp	Sun Nov 24 20:18:17 2013 +0000
@@ -1,8 +1,5 @@
 #include "mbed.h"
 
-#include "QEI.h"
-#include "include/motor.hpp"
-
 #ifndef DYNAMICS_HPP
 #define DYNAMICS_HPP
 
@@ -14,19 +11,18 @@
 class Kangaroo {
 
     public:
-        Kangaroo(Motor &M1, Motor &M2, QEI &e1, QEI &e2);
+        Kangaroo();
         void zero();
         void start();
         void run();
         void stop();
-        void updatePose();
-        void testEncoders(Serial &pc);
+        void updatePose() {
+        
+        // position of foot is (x,y)
         
-    private:
-        QEI enc1;
-        QEI enc2;
-        Motor m1;
-        Motor m2;
+        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