losse functie die aan de hand van een input x en y de hoeken theta_one en theta_two bepaald
Fork of emg2position2angles by
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?
User | Revision | Line number | New 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 | } |