marijn peters / Mbed 2 deprecated test_HIDScope

testen of de juiste hoeken gevonden worden

Dependencies:   HIDScope mbed

Fork of get_angles by marijn peters

main.cpp@0:121acb083576, 2015-10-14 (annotated)

Committer:
marijnstudent
Date:
Wed Oct 14 13:15:29 2015 +0000
Revision:
0:121acb083576
Child:
1:a93b870d3cf8
werkende functie die de hoeken theta1 en theta2 berekend bij geen gewenste x en y

Who changed what in which revision?

UserRevisionLine numberNew contents of line
marijnstudent 0:121acb083576 1 #include "mbed.h"
marijnstudent 0:121acb083576 2 #include "math.h"
marijnstudent 0:121acb083576 3
marijnstudent 0:121acb083576 4 // x & y zijn inputs L is lengte van de arm r is de vector naar de end effector
marijnstudent 0:121acb083576 5 double x = 50;
marijnstudent 0:121acb083576 6 double y = 60;
marijnstudent 0:121acb083576 7 const double L = 36;
marijnstudent 0:121acb083576 8 const double pi = 3.1415926535897;
marijnstudent 0:121acb083576 9 double theta1= 0;
marijnstudent 0:121acb083576 10 double theta2 = 0;
marijnstudent 0:121acb083576 11
marijnstudent 0:121acb083576 12 // functies die de hoeken berekend
marijnstudent 0:121acb083576 13 void getangles(double x,double y,double &theta_one,double &theta_two) // xy inputs
marijnstudent 0:121acb083576 14 {
marijnstudent 0:121acb083576 15 double r = sqrt(pow(L,2)+pow(L,2)); // vector naar end effector
marijnstudent 0:121acb083576 16 double alfa = acos((2*pow(L,2))/(4*L)); // alfa is de hoek tussen upper en lower arm
marijnstudent 0:121acb083576 17 double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
marijnstudent 0:121acb083576 18 // hoeken berekenen
marijnstudent 0:121acb083576 19 theta_one = atan2(y,x)+beta;
marijnstudent 0:121acb083576 20 theta_two = pi + alfa;
marijnstudent 0:121acb083576 21 }
marijnstudent 0:121acb083576 22
marijnstudent 0:121acb083576 23 int main()
marijnstudent 0:121acb083576 24 {
marijnstudent 0:121acb083576 25 while(true)
marijnstudent 0:121acb083576 26 {
marijnstudent 0:121acb083576 27 getangles(x,y,theta1,theta2);
marijnstudent 0:121acb083576 28 }
marijnstudent 0:121acb083576 29 }