marijn peters
losse functie die aan de hand van een input x en y de hoeken theta_one en theta_two bepaald
Fork of
emg2position2angles
by 
marijn peters
main.cpp@3:c1d11616f252, 2015-10-16 (annotated)
- Committer:
- marijnstudent
- Date:
- Fri Oct 16 10:50:13 2015 +0000
- Revision:
- 3:c1d11616f252
- Parent:
- 2:e2af01393e16
- Child:
- 4:833948d12d78
testfunctie om positie te testen
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 | 0:121acb083576 | 15 | // x & y zijn inputs L is lengte van de arm r is de vector naar de end effector |
| marijnstudent | 1:a93b870d3cf8 | 16 | |
| marijnstudent | 0:121acb083576 | 17 | const double L = 36; |
| marijnstudent | 0:121acb083576 | 18 | const double pi = 3.1415926535897; |
| marijnstudent | 3:c1d11616f252 | 19 | double theta_one; |
| marijnstudent | 3:c1d11616f252 | 20 | double theta_two; |
| marijnstudent | 3:c1d11616f252 | 21 | |
| marijnstudent | 3:c1d11616f252 | 22 | // tweede orde notch filter 50 Hz |
| marijnstudent | 3:c1d11616f252 | 23 | // biquad 1 coefficienten |
| marijnstudent | 3:c1d11616f252 | 24 | const double numnotch50biq1_1 = 1; |
| marijnstudent | 3:c1d11616f252 | 25 | const double numnotch50biq1_2 = -1.61816178466632; |
| marijnstudent | 3:c1d11616f252 | 26 | const double numnotch50biq1_3 = 1.00000006127058; |
| marijnstudent | 3:c1d11616f252 | 27 | const double dennotch50biq1_2 = -1.59325742941798; |
| marijnstudent | 3:c1d11616f252 | 28 | const double dennotch50biq1_3 = 0.982171881701431; |
| marijnstudent | 3:c1d11616f252 | 29 | // biquad 2 coefficienten |
| marijnstudent | 3:c1d11616f252 | 30 | const double numnotch50biq2_1 = 1; |
| marijnstudent | 3:c1d11616f252 | 31 | const double numnotch50biq2_2 = -1.61816171933244; |
| marijnstudent | 3:c1d11616f252 | 32 | const double numnotch50biq2_3 = 0.999999938729428; |
| marijnstudent | 3:c1d11616f252 | 33 | const double dennotch50biq2_2 = -1.61431180968071; |
| marijnstudent | 3:c1d11616f252 | 34 | const double dennotch50biq2_3 = 0.982599066293075; |
| marijnstudent | 3:c1d11616f252 | 35 | |
| marijnstudent | 3:c1d11616f252 | 36 | // highpass filter 20 Hz coefficienten |
| marijnstudent | 3:c1d11616f252 | 37 | const double numhigh20_1 = 0.837089190566345; |
| marijnstudent | 3:c1d11616f252 | 38 | const double numhigh20_2 = -1.67417838113269; |
| marijnstudent | 3:c1d11616f252 | 39 | const double numhigh20_3 = 0.837089190566345; |
| marijnstudent | 3:c1d11616f252 | 40 | const double denhigh20_2 = -1.64745998107698; |
| marijnstudent | 3:c1d11616f252 | 41 | const double denhigh20_3 = 0.700896781188403; |
| marijnstudent | 3:c1d11616f252 | 42 | |
| marijnstudent | 3:c1d11616f252 | 43 | // lowpass 5 Hz coefficienten |
| marijnstudent | 3:c1d11616f252 | 44 | const double numlow5_1 =0.000944691843840162; |
| marijnstudent | 3:c1d11616f252 | 45 | const double numlow5_2 =0.00188938368768032; |
| marijnstudent | 3:c1d11616f252 | 46 | const double numlow5_3 =0.000944691843840162; |
| marijnstudent | 3:c1d11616f252 | 47 | const double denlow5_2 =-1.91119706742607; |
| marijnstudent | 3:c1d11616f252 | 48 | const double denlow5_3 =0.914975834801434; |
| marijnstudent | 3:c1d11616f252 | 49 | |
| marijnstudent | 3:c1d11616f252 | 50 | // storage variables voor 8 filters aanmaken. Voor een signaal zijn dit 4 biquads, namelijk notch(2), highpass(1) & lowpass(1) |
| marijnstudent | 3:c1d11616f252 | 51 | double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0,f4_v1 = 0, f4_v2 = 0; |
| marijnstudent | 3:c1d11616f252 | 52 | double f1_v1t = 0, f1_v2t = 0, f2_v1t = 0, f2_v2t = 0, f3_v1t = 0, f3_v2t = 0,f4_v1t = 0, f4_v2t = 0; |
| marijnstudent | 0:121acb083576 | 53 | |
| marijnstudent | 3:c1d11616f252 | 54 | // biquadfilters die bij het filteren van signaal 1 horen |
| marijnstudent | 3:c1d11616f252 | 55 | double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) |
| marijnstudent | 3:c1d11616f252 | 56 | { |
| marijnstudent | 3:c1d11616f252 | 57 | double v = u- a1*v1-a2*v2; |
| marijnstudent | 3:c1d11616f252 | 58 | double y = b0*v+b1*v1+b2*v2; |
| marijnstudent | 3:c1d11616f252 | 59 | v2 = v1; |
| marijnstudent | 3:c1d11616f252 | 60 | v1 = v; |
| marijnstudent | 3:c1d11616f252 | 61 | return y; |
| marijnstudent | 3:c1d11616f252 | 62 | } |
| marijnstudent | 3:c1d11616f252 | 63 | |
| marijnstudent | 3:c1d11616f252 | 64 | // biquadfilters die bij het filteren van signaal 2 horen, copy paste, alle waardes zijn veranderd naar +t (t van two of twee) |
| marijnstudent | 3:c1d11616f252 | 65 | double biquadfiltert(double ut, double &v1t, double &v2t, const double a1t, const double a2t, const double b0t, const double b1t, const double b2t) |
| marijnstudent | 3:c1d11616f252 | 66 | { |
| marijnstudent | 3:c1d11616f252 | 67 | double vt = ut- a1t*v1t-a2t*v2t; |
| marijnstudent | 3:c1d11616f252 | 68 | double yt = b0t*vt+b1t*v1t+b2t*v2t; |
| marijnstudent | 3:c1d11616f252 | 69 | v2t = v1t; |
| marijnstudent | 3:c1d11616f252 | 70 | v1t = vt; |
| marijnstudent | 3:c1d11616f252 | 71 | return yt; |
| marijnstudent | 3:c1d11616f252 | 72 | } |
| marijnstudent | 3:c1d11616f252 | 73 | |
| marijnstudent | 3:c1d11616f252 | 74 | |
| marijnstudent | 3:c1d11616f252 | 75 | |
| marijnstudent | 3:c1d11616f252 | 76 | // myController vraagt de analoge waardes op (A0 en A2, d.m.v. AnalogIn[t].read()) |
| marijnstudent | 3:c1d11616f252 | 77 | // deze waardes worden gefilterd en naar HIDScope gestuurd |
| marijnstudent | 3:c1d11616f252 | 78 | void myController() |
| marijnstudent | 0:121acb083576 | 79 | { |
| marijnstudent | 3:c1d11616f252 | 80 | // filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass |
| marijnstudent | 3:c1d11616f252 | 81 | double u1 = EMG_in.read(); |
| marijnstudent | 3:c1d11616f252 | 82 | double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); |
| marijnstudent | 3:c1d11616f252 | 83 | double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); |
| marijnstudent | 3:c1d11616f252 | 84 | double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3); |
| marijnstudent | 3:c1d11616f252 | 85 | double y4 = abs(y3); |
| marijnstudent | 3:c1d11616f252 | 86 | double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3); |
| marijnstudent | 3:c1d11616f252 | 87 | // versturen van het input signaal u1 en het gefilterde signaal y5 naar HIDScope channel 0 en 1 |
| marijnstudent | 3:c1d11616f252 | 88 | //scope.set(0,u1); |
| marijnstudent | 3:c1d11616f252 | 89 | //scope.set(1,y5); |
| marijnstudent | 3:c1d11616f252 | 90 | // filteren van EMG signaal 2 (A2), zelfde proces als signaal 1 |
| marijnstudent | 3:c1d11616f252 | 91 | double u1t = EMG_int.read(); |
| marijnstudent | 3:c1d11616f252 | 92 | double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); |
| marijnstudent | 3:c1d11616f252 | 93 | double y2t = biquadfiltert( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); |
| marijnstudent | 3:c1d11616f252 | 94 | double y3t = biquadfiltert( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3); |
| marijnstudent | 3:c1d11616f252 | 95 | double y4t = abs(y3t); |
| marijnstudent | 3:c1d11616f252 | 96 | double y5t = biquadfiltert( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3); |
| marijnstudent | 3:c1d11616f252 | 97 | // versturen van input signaal2; u1t en het gefilterde signaal; y5t naar HIDScope channel 2 en 3 |
| marijnstudent | 3:c1d11616f252 | 98 | //scope.set(2,u1t); |
| marijnstudent | 3:c1d11616f252 | 99 | //scope.set(3,y5t); |
| marijnstudent | 3:c1d11616f252 | 100 | // verzenden van de verstuurde signalen |
| marijnstudent | 3:c1d11616f252 | 101 | //scope.send(); |
| marijnstudent | 3:c1d11616f252 | 102 | |
| marijnstudent | 3:c1d11616f252 | 103 | double xx = 7*50*y5; |
| marijnstudent | 3:c1d11616f252 | 104 | double yy = 7*50*y5t; |
| marijnstudent | 3:c1d11616f252 | 105 | double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector |
| marijnstudent | 3:c1d11616f252 | 106 | 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 | 107 | double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r |
| marijnstudent | 0:121acb083576 | 108 | // hoeken berekenen |
| marijnstudent | 3:c1d11616f252 | 109 | theta_one = atan2(yy,xx)+beta; |
| marijnstudent | 0:121acb083576 | 110 | theta_two = pi + alfa; |
| marijnstudent | 3:c1d11616f252 | 111 | scope.set(0,xx); |
| marijnstudent | 3:c1d11616f252 | 112 | scope.set(1,yy); |
| marijnstudent | 1:a93b870d3cf8 | 113 | double xt = L*cos(theta_one)+L*cos(theta_one+theta_two); |
| marijnstudent | 3:c1d11616f252 | 114 | double yt = L*sin(theta_one)+L*sin(theta_one+theta_two); |
| marijnstudent | 1:a93b870d3cf8 | 115 | scope.set(2,xt); |
| marijnstudent | 1:a93b870d3cf8 | 116 | scope.set(3,yt); |
| marijnstudent | 1:a93b870d3cf8 | 117 | scope.send(); |
| marijnstudent | 1:a93b870d3cf8 | 118 | } |
| marijnstudent | 1:a93b870d3cf8 | 119 | |
| marijnstudent | 1:a93b870d3cf8 | 120 | int main() |
| marijnstudent | 1:a93b870d3cf8 | 121 | { |
| marijnstudent | 3:c1d11616f252 | 122 | |
| marijnstudent | 3:c1d11616f252 | 123 | tick.attach(&myController,0.01f); |
| marijnstudent | 3:c1d11616f252 | 124 | // roep elke 0.01 seconde de functie myController aan, dus elke 0.01 seconde wordt er gesamplet en dit sample gefilterd. |
| marijnstudent | 3:c1d11616f252 | 125 | |
| marijnstudent | 3:c1d11616f252 | 126 | |
| marijnstudent | 1:a93b870d3cf8 | 127 | } |