Diff: help_functions/kinematics.h

Revision:

1:ce487c9929dd

diff -r ef81b9f14f58 -r ce487c9929dd help_functions/kinematics.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/help_functions/kinematics.h	Mon Oct 22 11:10:41 2018 +0000
@@ -0,0 +1,24 @@
+#include "mbed.h"
+
+double L1 = 0.5;
+double L2 = 0.7;
+double x01 = 0.0;
+double y01 = 0.2;
+double q1,q2,x,y;
+
+float forwardkinematics_function(double motor_angle) {
+    // input are joint angles, output are x and y position of end effector
+    q1 = motor_angle;
+    q2 = motor_angle;
+    
+    x = x01 - L1 * sin(q1) + L2 * cos(q1 + q2);
+    y = y01 + L1 * cos(q1) + L2 * sin(q1 + q2);
+    return x;    
+}
+
+float inversekinematics_function(double reference) {
+    // pseudo inverse jacobian to get joint speeds
+    // input are desired vx and vy of end effector, output joint angle speeds
+    
+    return 5.5;    
+}
\ No newline at end of file