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

Committer:
calamaridudeman
Date:
2013-12-02
Revision:
55:ff84fbdfd1d1
Parent:
46:4497e945de6b

File content as of revision 55:ff84fbdfd1d1:

#include "mbed.h"
#include "include/dynamics.hpp"


Joints invKinBody(Point &in){
    float x=in.x;
    float y=in.y;
    //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)) ));
    //float theta2 = (float) atan2(y,x) - atan2(l3*sin(theta1), l2 + l4 + l3*cos(theta1));
    float theta2 = (float) acos((x*x+y*y-(l2+l4)*(l2+l4)-l3*l3)/(-2*(l2+l4)*l3));
    float theta1 = atan2(y,x)+acos((l3*l3-x*x-y*y-(l2+l4)*(l2+l4))/(-2*(l2+l4)*sqrt(x*x+y*y)));
    return Joints(theta1, theta2);
}