marijn peters / Mbed 2 deprecated getangles_function

losse functie die aan de hand van een input x en y de hoeken theta_one en theta_two bepaald

Dependencies:   HIDScope mbed

Fork of emg2position2angles by marijn peters

main.cpp@1:a93b870d3cf8, 2015-10-14 (annotated)

Committer:
marijnstudent
Date:
Wed Oct 14 13:43:01 2015 +0000
Revision:
1:a93b870d3cf8
Parent:
0:121acb083576
Child:
2:e2af01393e16
Testen d.m.v. HIDScope

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 1:a93b870d3cf8 3 #include "HIDScope.h"
marijnstudent 0:121acb083576 4
marijnstudent 1:a93b870d3cf8 5 HIDScope scope(4);
marijnstudent 1:a93b870d3cf8 6 AnalogIn potright(A0);
marijnstudent 1:a93b870d3cf8 7 AnalogIn potleft(A1);
marijnstudent 0:121acb083576 8 // x & y zijn inputs L is lengte van de arm r is de vector naar de end effector
marijnstudent 1:a93b870d3cf8 9
marijnstudent 0:121acb083576 10 const double L = 36;
marijnstudent 0:121acb083576 11 const double pi = 3.1415926535897;
marijnstudent 1:a93b870d3cf8 12
marijnstudent 0:121acb083576 13
marijnstudent 0:121acb083576 14 // functies die de hoeken berekend
marijnstudent 1:a93b870d3cf8 15 void getangles(double &theta_one,double &theta_two) // xy inputs
marijnstudent 0:121acb083576 16 {
marijnstudent 1:a93b870d3cf8 17 double x = 60+ 20*potleft.read();
marijnstudent 1:a93b870d3cf8 18 double y = 40+40*potright.read();
marijnstudent 0:121acb083576 19 double r = sqrt(pow(L,2)+pow(L,2)); // vector naar end effector
marijnstudent 0:121acb083576 20 double alfa = acos((2*pow(L,2))/(4*L)); // alfa is de hoek tussen upper en lower arm
marijnstudent 0:121acb083576 21 double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
marijnstudent 0:121acb083576 22 // hoeken berekenen
marijnstudent 0:121acb083576 23 theta_one = atan2(y,x)+beta;
marijnstudent 0:121acb083576 24 theta_two = pi + alfa;
marijnstudent 1:a93b870d3cf8 25 scope.set(0,x);
marijnstudent 1:a93b870d3cf8 26 scope.set(1,y);
marijnstudent 1:a93b870d3cf8 27 double xt = L*cos(theta_one)+L*cos(theta_one+theta_two);
marijnstudent 1:a93b870d3cf8 28 double yt = L*cos(theta_one)+L*cos(theta_one+theta_two);
marijnstudent 1:a93b870d3cf8 29 scope.set(2,xt);
marijnstudent 1:a93b870d3cf8 30 scope.set(3,yt);
marijnstudent 1:a93b870d3cf8 31 scope.send();
marijnstudent 1:a93b870d3cf8 32 }
marijnstudent 1:a93b870d3cf8 33
marijnstudent 1:a93b870d3cf8 34 int main()
marijnstudent 1:a93b870d3cf8 35 {
marijnstudent 1:a93b870d3cf8 36 while(true) {
marijnstudent 1:a93b870d3cf8 37 getangles(theta1,theta2);
marijnstudent 1:a93b870d3cf8 38 }
marijnstudent 1:a93b870d3cf8 39 }