marijn peters / Mbed 2 deprecated get_angles

functie getangles bepaald de hoeken theta 1 en theta 2 bij een vooraf gegeven x en y

Dependencies:   mbed

main.cpp

Committer:
marijnstudent
Date:
2015-10-14
Revision:
0:121acb083576

File content as of revision 0:121acb083576:

#include "mbed.h"
#include "math.h"

// x & y zijn inputs L is lengte van de arm r is de vector naar de end effector
double x = 50;
double y = 60;
const double L = 36;
const double pi = 3.1415926535897;
double theta1= 0;
double theta2 = 0;

// functies die de hoeken berekend
void getangles(double x,double y,double &theta_one,double &theta_two) // xy inputs
{
    double r = sqrt(pow(L,2)+pow(L,2)); // vector naar end effector
    double alfa = acos((2*pow(L,2))/(4*L)); // alfa is de hoek tussen upper en lower arm
    double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
    // hoeken berekenen
    theta_one = atan2(y,x)+beta;
    theta_two = pi + alfa;
 }
 
 int main()
 {
     while(true)
     {
         getangles(x,y,theta1,theta2);
         }
     }