Roy van Zijl / Mbed 2 deprecated TestSubjectA

Mbed bordje 1 -af

Dependencies:   Encoder HIDScope MODSERIAL Matrix MatrixMath biquad-master mbed

Fork of dsjklafjaslkjdfalkjfdaslkasdjklajadsflkjdasflkjdasflkadsflkasd by Dion de Greef

main.cpp@3:2ffbee47fb38, 2017-10-31 (annotated)

Committer:
RoyvZ
Date:
Tue Oct 31 21:50:46 2017 +0000
Revision:
3:2ffbee47fb38
Parent:
2:293665548183
Child:
4:afa85283eb18
Added Matrices

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RoyvZ 3:2ffbee47fb38 1 /**
RoyvZ 0:b6c8d56842ce 2 * Demo program for BiQuad and BiQuadChain classes
RoyvZ 0:b6c8d56842ce 3 * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> and Matthijs and Roy and Dion
RoyvZ 0:b6c8d56842ce 4 */
RoyvZ 0:b6c8d56842ce 5 #include "mbed.h"
RoyvZ 0:b6c8d56842ce 6 #include "HIDScope.h"
RoyvZ 0:b6c8d56842ce 7 #include <stdlib.h>
RoyvZ 0:b6c8d56842ce 8 #include <iostream>
RoyvZ 0:b6c8d56842ce 9 #include <iomanip>
RoyvZ 0:b6c8d56842ce 10 #include <complex>
RoyvZ 3:2ffbee47fb38 11 #include <vector>
RoyvZ 0:b6c8d56842ce 12 #include "BiQuad.h"
RoyvZ 3:2ffbee47fb38 13 #include "Matrix.h"
RoyvZ 3:2ffbee47fb38 14 #include "MatrixMath.h"
RoyvZ 3:2ffbee47fb38 15 #include "MODSERIAL.h"
RoyvZ 3:2ffbee47fb38 16 #include "encoder.h"
RoyvZ 2:293665548183 17
RoyvZ 0:b6c8d56842ce 18 AnalogIn emg0( A0 );
RoyvZ 0:b6c8d56842ce 19 AnalogIn emg1( A1 );
RoyvZ 3:2ffbee47fb38 20 DigitalIn button1(D11);
RoyvZ 3:2ffbee47fb38 21 Encoder encoder1(D13,D12);
RoyvZ 3:2ffbee47fb38 22 Encoder encoder2(D13,D12); //moet nog even aangepast worden
RoyvZ 3:2ffbee47fb38 23
RoyvZ 3:2ffbee47fb38 24
RoyvZ 3:2ffbee47fb38 25 DigitalOut motorDirection(D4);
RoyvZ 3:2ffbee47fb38 26 PwmOut motorSpeed(D5);
RoyvZ 3:2ffbee47fb38 27
RoyvZ 3:2ffbee47fb38 28 MODSERIAL pc(USBTX, USBRX);
RoyvZ 0:b6c8d56842ce 29
RoyvZ 0:b6c8d56842ce 30 Ticker sample_timer;
RoyvZ 2:293665548183 31 Ticker cal_timer;
RoyvZ 3:2ffbee47fb38 32 HIDScope scope( 4 );
RoyvZ 0:b6c8d56842ce 33 DigitalOut led(LED1);
RoyvZ 3:2ffbee47fb38 34 DigitalOut led2(LED_GREEN);
RoyvZ 3:2ffbee47fb38 35 const int size = 100;
RoyvZ 3:2ffbee47fb38 36 vector<double> S(size,0);
RoyvZ 3:2ffbee47fb38 37 double meanSum = 0;
RoyvZ 3:2ffbee47fb38 38
RoyvZ 3:2ffbee47fb38 39 double maxsignal = 0;
RoyvZ 2:293665548183 40
RoyvZ 3:2ffbee47fb38 41 double L0 = 0.123;
RoyvZ 3:2ffbee47fb38 42 double L1 = 0.37;
RoyvZ 3:2ffbee47fb38 43 double L2 = 0.41;
RoyvZ 3:2ffbee47fb38 44 double q1 = encoder1.getPosition()/131; //Calibreren nog toevoegen
RoyvZ 3:2ffbee47fb38 45 double q2 = encoder2.getPosition()/131; //Calibreren mist nog
RoyvZ 3:2ffbee47fb38 46 double Periode = 1/1000; //1000 is het aantal Hz
RoyvZ 2:293665548183 47
RoyvZ 2:293665548183 48 //Filter toevoegen, bestaande uit notch, high- en lowpass filter
RoyvZ 2:293665548183 49 BiQuadChain Notch;
RoyvZ 3:2ffbee47fb38 50 BiQuadChain Notch50;
RoyvZ 3:2ffbee47fb38 51 BiQuadChain bqcLP;
RoyvZ 3:2ffbee47fb38 52 BiQuadChain bqc;
RoyvZ 2:293665548183 53 //50 Hz Notch filter, for radiation from surroundings
RoyvZ 0:b6c8d56842ce 54
RoyvZ 2:293665548183 55 //Notch filter chain for 100 Hz
RoyvZ 2:293665548183 56 BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 );
RoyvZ 2:293665548183 57 BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 );
RoyvZ 2:293665548183 58 BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 );
RoyvZ 2:293665548183 59
RoyvZ 3:2ffbee47fb38 60 //BiQuad Notch(0.9179504645111498,-1.4852750515677946, 0.9179504645111498, -1.4852750515677946, 0.8359009290222995);
RoyvZ 3:2ffbee47fb38 61
RoyvZ 2:293665548183 62 //Notch filter chain for 50 Hz
RoyvZ 3:2ffbee47fb38 63 BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 );
RoyvZ 3:2ffbee47fb38 64 BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 );
RoyvZ 3:2ffbee47fb38 65 BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 );
RoyvZ 2:293665548183 66
RoyvZ 2:293665548183 67 //15 Hz Highpass filter, as recommended by multiple studies regarding EMG signals
RoyvZ 2:293665548183 68 //BiQuad Highpass15(9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01);
RoyvZ 2:293665548183 69
RoyvZ 2:293665548183 70 //2Hz Highpass
RoyvZ 3:2ffbee47fb38 71 BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01);
RoyvZ 0:b6c8d56842ce 72
RoyvZ 0:b6c8d56842ce 73
RoyvZ 2:293665548183 74 //20 Hz Lowpass
RoyvZ 3:2ffbee47fb38 75 //BiQuad Lowpass100( 3.62168e-03, 7.24336e-03, 3.62168e-03, -1.82269e+00, 8.37182e-01 );
RoyvZ 3:2ffbee47fb38 76
RoyvZ 3:2ffbee47fb38 77 //250 Hz Lowpass
RoyvZ 3:2ffbee47fb38 78 //BiQuad Lowpass100 (2.92893e-01, 5.85786e-01, 2.92893e-01, -3.60822e-16, 1.71573e-01 );
RoyvZ 3:2ffbee47fb38 79
RoyvZ 3:2ffbee47fb38 80 //80 Hz Lowpass
RoyvZ 3:2ffbee47fb38 81 BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 );
RoyvZ 3:2ffbee47fb38 82 BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 );
RoyvZ 3:2ffbee47fb38 83
RoyvZ 3:2ffbee47fb38 84
RoyvZ 0:b6c8d56842ce 85
RoyvZ 2:293665548183 86 //450 Hz Lowpass filter, to remove any noise (sample frequency is only 500 Hz, but still...)
RoyvZ 2:293665548183 87 //BiQuad Lowpass450(8.00592e-01, 1.60118e+00, 8.00592e-01, 1.56102e+00, 6.41352e-01);
RoyvZ 3:2ffbee47fb38 88
RoyvZ 3:2ffbee47fb38 89 double findRMS(vector<double> array)
RoyvZ 3:2ffbee47fb38 90 {
RoyvZ 3:2ffbee47fb38 91 int i;
RoyvZ 3:2ffbee47fb38 92 double sumsquared = 0.000;
RoyvZ 3:2ffbee47fb38 93 double RMS;
RoyvZ 3:2ffbee47fb38 94 //sumsquared = 0;
RoyvZ 3:2ffbee47fb38 95 for (i = 0; i < size; i++)
RoyvZ 3:2ffbee47fb38 96 {
RoyvZ 3:2ffbee47fb38 97 sumsquared = sumsquared + array.at(i)*array.at(i);
RoyvZ 3:2ffbee47fb38 98 }
RoyvZ 3:2ffbee47fb38 99 RMS = sqrt((double(1)/size)*(sumsquared));
RoyvZ 3:2ffbee47fb38 100 return RMS;
RoyvZ 3:2ffbee47fb38 101 }
RoyvZ 3:2ffbee47fb38 102
RoyvZ 3:2ffbee47fb38 103
RoyvZ 3:2ffbee47fb38 104
RoyvZ 0:b6c8d56842ce 105 void sample()
RoyvZ 0:b6c8d56842ce 106 {
RoyvZ 0:b6c8d56842ce 107 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
RoyvZ 2:293665548183 108 //scope.set(0, emg0.read() );
RoyvZ 2:293665548183 109 //scope.set(1, fabs(Notch.step(emg0.read())) );
RoyvZ 3:2ffbee47fb38 110 scope.set(0, emg0.read()-0.4542);
RoyvZ 3:2ffbee47fb38 111 //scope.set(1, bqcLP.step(Highpass1.step(emg0.read()-0.4542)));
RoyvZ 3:2ffbee47fb38 112 //scope.set(2, fabs(Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542)))));
RoyvZ 3:2ffbee47fb38 113 //scope.set(3, fabs(bqc.step(emg0.read()-0.4542)));
RoyvZ 3:2ffbee47fb38 114 //scope.set(3, Notch.step(Notch50.step(bqcLP.step(Highpass1.step(fabs(emg0.read()-0.4542))))));
RoyvZ 3:2ffbee47fb38 115
RoyvZ 3:2ffbee47fb38 116
RoyvZ 3:2ffbee47fb38 117 //scope.set(0, fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))) );
RoyvZ 3:2ffbee47fb38 118 /*
RoyvZ 3:2ffbee47fb38 119 for (int i = size-1; i > 1; i--) {
RoyvZ 2:293665548183 120 S[i] = S[i-1];
RoyvZ 2:293665548183 121 }
RoyvZ 3:2ffbee47fb38 122 */
RoyvZ 3:2ffbee47fb38 123 S.erase(S.begin());
RoyvZ 3:2ffbee47fb38 124 double b = bqc.step(emg0.read()-0.4542);
RoyvZ 3:2ffbee47fb38 125 S.push_back(b);
RoyvZ 3:2ffbee47fb38 126 //S[0] = bqc.step(emg0.read()-0.4542)));
RoyvZ 3:2ffbee47fb38 127 //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542)));
RoyvZ 3:2ffbee47fb38 128 double a = findRMS(S);
RoyvZ 3:2ffbee47fb38 129 scope.set(1, a);
RoyvZ 3:2ffbee47fb38 130 scope.set(2, S[0]);
RoyvZ 3:2ffbee47fb38 131 //meanSum = 0; */
RoyvZ 0:b6c8d56842ce 132 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
RoyvZ 0:b6c8d56842ce 133 * Ensure that enough channels are available (HIDScope scope( 2 ))
RoyvZ 0:b6c8d56842ce 134 * Finally, send all channels to the PC at once */
RoyvZ 0:b6c8d56842ce 135 scope.send();
RoyvZ 0:b6c8d56842ce 136 /* To indicate that the function is working, the LED is toggled */
RoyvZ 0:b6c8d56842ce 137 led = !led;
RoyvZ 3:2ffbee47fb38 138
RoyvZ 3:2ffbee47fb38 139 motorSpeed.write(0.38080*(a/maxsignal));
RoyvZ 3:2ffbee47fb38 140 motorDirection.write(1);
RoyvZ 0:b6c8d56842ce 141 }
RoyvZ 0:b6c8d56842ce 142
RoyvZ 2:293665548183 143 void calibration()
RoyvZ 2:293665548183 144 {
RoyvZ 2:293665548183 145 //function to determine maximal EMG input in order to allow motorcontrol to be activated
RoyvZ 3:2ffbee47fb38 146
RoyvZ 2:293665548183 147 if (button1.read() == false){
RoyvZ 3:2ffbee47fb38 148 led2 = !led2;
RoyvZ 2:293665548183 149 for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds
RoyvZ 3:2ffbee47fb38 150
RoyvZ 3:2ffbee47fb38 151 S.erase(S.begin());
RoyvZ 3:2ffbee47fb38 152 double b = bqc.step(emg0.read()-0.4542);
RoyvZ 3:2ffbee47fb38 153 S.push_back(b);
RoyvZ 3:2ffbee47fb38 154 //S[0] = bqc.step(emg0.read()-0.4542)));
RoyvZ 3:2ffbee47fb38 155 //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542)));
RoyvZ 3:2ffbee47fb38 156 double signalfinal = findRMS(S);
RoyvZ 2:293665548183 157 if (signalfinal >= maxsignal){
RoyvZ 2:293665548183 158 maxsignal = signalfinal; //keep changing the maximal signal
RoyvZ 3:2ffbee47fb38 159 pc.baud(9600);
RoyvZ 3:2ffbee47fb38 160 pc.printf("%d \n",maxsignal);
RoyvZ 2:293665548183 161 }
RoyvZ 2:293665548183 162 }
RoyvZ 3:2ffbee47fb38 163 led2 = 1;
RoyvZ 2:293665548183 164 }
RoyvZ 2:293665548183 165 }
RoyvZ 2:293665548183 166
RoyvZ 0:b6c8d56842ce 167 int main()
RoyvZ 0:b6c8d56842ce 168 {
RoyvZ 2:293665548183 169 //Constructing the notch filter chain
RoyvZ 3:2ffbee47fb38 170 Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 );
RoyvZ 3:2ffbee47fb38 171 Notch50.add( &bq1 ).add( &bq2 ).add( &bq3 );
RoyvZ 3:2ffbee47fb38 172 bqcLP.add( &bqLP1 ).add( &bqLP2 );
RoyvZ 3:2ffbee47fb38 173 bqc.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ).add( &bqLP1 ).add( &bqLP2 );
RoyvZ 0:b6c8d56842ce 174 /**Attach the 'sample' function to the timer 'sample_timer'.
RoyvZ 2:293665548183 175 * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz
RoyvZ 0:b6c8d56842ce 176 */
RoyvZ 3:2ffbee47fb38 177 sample_timer.attach(&sample, 0.002);
RoyvZ 2:293665548183 178 cal_timer.attach(&calibration, 0.002);//ticker to check if motor is being calibrated
RoyvZ 3:2ffbee47fb38 179 led2 = 1;
RoyvZ 3:2ffbee47fb38 180 /*empty loop, sample() is executed periodically*/
RoyvZ 3:2ffbee47fb38 181
RoyvZ 3:2ffbee47fb38 182 pc.baud(9600);
RoyvZ 3:2ffbee47fb38 183 DigitalOut myled(LED1);
RoyvZ 3:2ffbee47fb38 184
RoyvZ 3:2ffbee47fb38 185 //---
RoyvZ 3:2ffbee47fb38 186 Matrix JAPPAPP(2,2);
RoyvZ 3:2ffbee47fb38 187 Matrix qdot(2,1);
RoyvZ 3:2ffbee47fb38 188 Matrix Vdes(2,1);
RoyvZ 3:2ffbee47fb38 189 Matrix qsetpoint(2,1);
RoyvZ 0:b6c8d56842ce 190
RoyvZ 3:2ffbee47fb38 191 // Fill Matrix with data.
RoyvZ 3:2ffbee47fb38 192 JAPPAPP << -(L0 - L0*sin(q1) + L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)) << -(L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2))
RoyvZ 3:2ffbee47fb38 193 << (L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)) << (L1*cos(q1) + L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));
RoyvZ 3:2ffbee47fb38 194
RoyvZ 3:2ffbee47fb38 195
RoyvZ 3:2ffbee47fb38 196 // Fill Matrix with data.
RoyvZ 3:2ffbee47fb38 197 Vdes << emg0.read //Goede code nog toevoegen, Vx
RoyvZ 3:2ffbee47fb38 198 << emg1.read; //goede code nog toevoegen, Vy
RoyvZ 3:2ffbee47fb38 199
RoyvZ 3:2ffbee47fb38 200
RoyvZ 3:2ffbee47fb38 201 qdot = JAPPAPP*Vdes;
RoyvZ 3:2ffbee47fb38 202 pc.printf("\r\nJAPPAPPP: "); //Alleen visualisatie
RoyvZ 3:2ffbee47fb38 203 qdot.print(); //Alleen visualisatie
RoyvZ 3:2ffbee47fb38 204
RoyvZ 3:2ffbee47fb38 205 qsetpoint = q1 + qdot*Periode;
RoyvZ 3:2ffbee47fb38 206
RoyvZ 3:2ffbee47fb38 207
RoyvZ 3:2ffbee47fb38 208
RoyvZ 0:b6c8d56842ce 209 while(true) {
RoyvZ 0:b6c8d56842ce 210
RoyvZ 2:293665548183 211
RoyvZ 0:b6c8d56842ce 212 }
RoyvZ 0:b6c8d56842ce 213
RoyvZ 0:b6c8d56842ce 214 }