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

src/dynamics.cpp@37:bf257a0154db, 2013-11-24 (annotated)

Committer:
calamaridudeman
Date:
Sun Nov 24 20:38:59 2013 +0000
Revision:
37:bf257a0154db
Parent:
35:a4e89e78d034
Child:
38:922f2584bdfd
Child:
39:b765b6dd01c3
integrated sams inverse kinematics code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
calamaridudeman 21:a9569c7b4379 1 #include "mbed.h"
calamaridudeman 23:112c0be5a7f3 2 #include "include/dynamics.hpp"
calamaridudeman 16:c21df8c0c458 3
calamaridudeman 37:bf257a0154db 4
calamaridudeman 37:bf257a0154db 5 Joints invKinBody(Point &in){
calamaridudeman 37:bf257a0154db 6 float x=in.x;
calamaridudeman 37:bf257a0154db 7 float y=in.y;
calamaridudeman 37:bf257a0154db 8 float theta1 = (float) 2 * atan(sqrt( ( (l2+l3+l4)*(l2+l3+l4) - x*x - y*y) / (x*x + y*y - (l2+l4-l3)*(l2+l4-l3)) ));
calamaridudeman 37:bf257a0154db 9 float theta2 = (float) atan2(y,x) - atan2(l3*sin(theta1), l2 + l4 + l3*cos(theta1));
calamaridudeman 37:bf257a0154db 10 return Joints(theta1, theta2);
calamaridudeman 37:bf257a0154db 11 }