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@5:07d15b93b361, 2015-10-16 (annotated)

Committer:
marijnstudent
Date:
Fri Oct 16 11:23:33 2015 +0000
Revision:
5:07d15b93b361
Parent:
4:833948d12d78 
losse functie getangles

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 3:c1d11616f252 4 #include "complex.h"
marijnstudent 0:121acb083576 5
marijnstudent 1:a93b870d3cf8 6 HIDScope scope(4);
marijnstudent 3:c1d11616f252 7 // Analoge input signalen defineren
marijnstudent 3:c1d11616f252 8 AnalogIn EMG_in(A0); // EMG_in.read kan je nu gebruiken om het analoge signaal A0 uit te lezen
marijnstudent 3:c1d11616f252 9 AnalogIn EMG_int(A2); // deze leest A2 uit
marijnstudent 3:c1d11616f252 10 // ticker met de naam tick om je control functie uit te lezen.
marijnstudent 3:c1d11616f252 11 Ticker tick;
marijnstudent 3:c1d11616f252 12
marijnstudent 3:c1d11616f252 13
marijnstudent 3:c1d11616f252 14
marijnstudent 1:a93b870d3cf8 15
marijnstudent 0:121acb083576 16 const double L = 36;
marijnstudent 0:121acb083576 17 const double pi = 3.1415926535897;
marijnstudent 3:c1d11616f252 18 double theta_one;
marijnstudent 3:c1d11616f252 19 double theta_two;
marijnstudent 5:07d15b93b361 20 double x_input;
marijnstudent 5:07d15b93b361 21 double y_input;
marijnstudent 5:07d15b93b361 22 void getangles(double &theta_one,double &theta_two,double x_input,double y_input)
marijnstudent 3:c1d11616f252 23 {
marijnstudent 5:07d15b93b361 24 double xx = x_input;
marijnstudent 5:07d15b93b361 25 double yy = y_input;
marijnstudent 3:c1d11616f252 26 double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
marijnstudent 3:c1d11616f252 27 double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
marijnstudent 0:121acb083576 28 double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
marijnstudent 0:121acb083576 29 // hoeken berekenen
marijnstudent 3:c1d11616f252 30 theta_one = atan2(yy,xx)+beta;
marijnstudent 0:121acb083576 31 theta_two = pi + alfa;
marijnstudent 5:07d15b93b361 32
marijnstudent 1:a93b870d3cf8 33 }
marijnstudent 1:a93b870d3cf8 34
marijnstudent 1:a93b870d3cf8 35 int main()
marijnstudent 1:a93b870d3cf8 36 {
marijnstudent 3:c1d11616f252 37
marijnstudent 5:07d15b93b361 38 getangles(theta_one,theta_two, x_input, y_input);
marijnstudent 3:c1d11616f252 39
marijnstudent 1:a93b870d3cf8 40 }