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@4:afa85283eb18, 2017-11-01 (annotated)

Committer:
DiondeGreef
Date:
Wed Nov 01 11:50:05 2017 +0000
Revision:
4:afa85283eb18
Parent:
3:2ffbee47fb38
Child:
5:3562c205d001
afdd

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