Roy van Zijl / Mbed 2 deprecated EMGFilter_copy_VOORMATTHIJS

Voor matthijs

Dependencies:   Encoder MODSERIAL Matrix MatrixMath QEI biquad-master mbed

Fork of frdm_gpio1 by Roy van Zijl

main.cpp@4:dfed3b98ffb1, 2017-11-01 (annotated)

Committer:
RoyvZ
Date:
Wed Nov 01 15:08:27 2017 +0000
Revision:
4:dfed3b98ffb1
Parent:
3:2ffbee47fb38
Child:
5:52badb8ee317
Voor matthijs;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RoyvZ 4:dfed3b98ffb1 1 /**
RoyvZ 4:dfed3b98ffb1 2 * Demo program for BiQuad and BiQuadChain classes
RoyvZ 4:dfed3b98ffb1 3 * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> and Matthijs and Roy and Dion
RoyvZ 4:dfed3b98ffb1 4 */
RoyvZ 0:b6c8d56842ce 5 #include "mbed.h"
RoyvZ 4:dfed3b98ffb1 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 4:dfed3b98ffb1 24 AnalogIn potMeterIn1(A1);
RoyvZ 4:dfed3b98ffb1 25
RoyvZ 3:2ffbee47fb38 26
RoyvZ 3:2ffbee47fb38 27 DigitalOut motorDirection(D4);
RoyvZ 3:2ffbee47fb38 28 PwmOut motorSpeed(D5);
RoyvZ 3:2ffbee47fb38 29
RoyvZ 3:2ffbee47fb38 30 MODSERIAL pc(USBTX, USBRX);
RoyvZ 0:b6c8d56842ce 31
RoyvZ 0:b6c8d56842ce 32 Ticker sample_timer;
RoyvZ 2:293665548183 33 Ticker cal_timer;
RoyvZ 4:dfed3b98ffb1 34 Ticker setpoint_timer;
RoyvZ 4:dfed3b98ffb1 35 Ticker m1_Ticker;
RoyvZ 4:dfed3b98ffb1 36 Ticker SP_m1_Ticker;
RoyvZ 4:dfed3b98ffb1 37 //HIDScope scope( 3 );
RoyvZ 0:b6c8d56842ce 38 DigitalOut led(LED1);
RoyvZ 4:dfed3b98ffb1 39 DigitalOut led2(LED_GREEN);
RoyvZ 3:2ffbee47fb38 40 const int size = 100;
RoyvZ 3:2ffbee47fb38 41 vector<double> S(size,0);
RoyvZ 3:2ffbee47fb38 42 double meanSum = 0;
RoyvZ 3:2ffbee47fb38 43
RoyvZ 3:2ffbee47fb38 44 double maxsignal = 0;
RoyvZ 2:293665548183 45
RoyvZ 4:dfed3b98ffb1 46 double emgX = 0;
RoyvZ 4:dfed3b98ffb1 47 double emgY = 1;
RoyvZ 4:dfed3b98ffb1 48
RoyvZ 3:2ffbee47fb38 49 double L0 = 0.123;
RoyvZ 3:2ffbee47fb38 50 double L1 = 0.37;
RoyvZ 3:2ffbee47fb38 51 double L2 = 0.41;
RoyvZ 3:2ffbee47fb38 52 double q1 = encoder1.getPosition()/131; //Calibreren nog toevoegen
RoyvZ 3:2ffbee47fb38 53 double q2 = encoder2.getPosition()/131; //Calibreren mist nog
RoyvZ 4:dfed3b98ffb1 54 double Periode = 0.002; //1000 is het aantal Hz
RoyvZ 4:dfed3b98ffb1 55
RoyvZ 4:dfed3b98ffb1 56
RoyvZ 4:dfed3b98ffb1 57 float M1_KP = 2.5;
RoyvZ 4:dfed3b98ffb1 58 float M1_KI = 1.0;
RoyvZ 4:dfed3b98ffb1 59 const float M1_TS = 0.01;
RoyvZ 4:dfed3b98ffb1 60 const float SP_TS = 0.01;
RoyvZ 4:dfed3b98ffb1 61 const float RAD_PER_PULSE = 0.000119;
RoyvZ 4:dfed3b98ffb1 62 float m1_err_int = 0;
RoyvZ 4:dfed3b98ffb1 63 int motorD = 0;
RoyvZ 4:dfed3b98ffb1 64 float motor1 = 0;
RoyvZ 4:dfed3b98ffb1 65 float reference = 0;
RoyvZ 4:dfed3b98ffb1 66 float position = 0;
RoyvZ 4:dfed3b98ffb1 67 int outOfEncod = 0;
RoyvZ 4:dfed3b98ffb1 68 int KP_changer = 1;
RoyvZ 4:dfed3b98ffb1 69 float RotSpeed = 0;
RoyvZ 4:dfed3b98ffb1 70
RoyvZ 4:dfed3b98ffb1 71
RoyvZ 4:dfed3b98ffb1 72 //Making matrices globaly
RoyvZ 4:dfed3b98ffb1 73 Matrix JAPPAPP(2,2);
RoyvZ 4:dfed3b98ffb1 74 Matrix qdot(2,1);
RoyvZ 4:dfed3b98ffb1 75 Matrix Vdes(2,1);
RoyvZ 4:dfed3b98ffb1 76 Matrix qsetpoint(2,1);
RoyvZ 2:293665548183 77
RoyvZ 2:293665548183 78 //Filter toevoegen, bestaande uit notch, high- en lowpass filter
RoyvZ 2:293665548183 79 BiQuadChain Notch;
RoyvZ 3:2ffbee47fb38 80 BiQuadChain Notch50;
RoyvZ 3:2ffbee47fb38 81 BiQuadChain bqcLP;
RoyvZ 3:2ffbee47fb38 82 BiQuadChain bqc;
RoyvZ 2:293665548183 83 //50 Hz Notch filter, for radiation from surroundings
RoyvZ 0:b6c8d56842ce 84
RoyvZ 2:293665548183 85 //Notch filter chain for 100 Hz
RoyvZ 2:293665548183 86 BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 );
RoyvZ 2:293665548183 87 BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 );
RoyvZ 2:293665548183 88 BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 );
RoyvZ 2:293665548183 89
RoyvZ 3:2ffbee47fb38 90
RoyvZ 2:293665548183 91 //Notch filter chain for 50 Hz
RoyvZ 3:2ffbee47fb38 92 BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 );
RoyvZ 3:2ffbee47fb38 93 BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 );
RoyvZ 3:2ffbee47fb38 94 BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 );
RoyvZ 2:293665548183 95
RoyvZ 2:293665548183 96 //2Hz Highpass
RoyvZ 3:2ffbee47fb38 97 BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01);
RoyvZ 0:b6c8d56842ce 98
RoyvZ 3:2ffbee47fb38 99 //80 Hz Lowpass
RoyvZ 3:2ffbee47fb38 100 BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 );
RoyvZ 3:2ffbee47fb38 101 BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 );
RoyvZ 3:2ffbee47fb38 102
RoyvZ 3:2ffbee47fb38 103
RoyvZ 0:b6c8d56842ce 104
RoyvZ 2:293665548183 105 //450 Hz Lowpass filter, to remove any noise (sample frequency is only 500 Hz, but still...)
RoyvZ 2:293665548183 106 //BiQuad Lowpass450(8.00592e-01, 1.60118e+00, 8.00592e-01, 1.56102e+00, 6.41352e-01);
RoyvZ 4:dfed3b98ffb1 107
RoyvZ 4:dfed3b98ffb1 108 float PI( float e, const float Kp, const float Ki, float Ts, float &e_int )
RoyvZ 4:dfed3b98ffb1 109 {
RoyvZ 4:dfed3b98ffb1 110 e_int += Ts * e; // e_int is changed globally because it’s ’by reference’ (&)
RoyvZ 4:dfed3b98ffb1 111 if (fabs(Kp * e + Ki * e_int) < 1) {
RoyvZ 4:dfed3b98ffb1 112 return fabs(Kp * e + Ki * e_int);
RoyvZ 4:dfed3b98ffb1 113 } else {
RoyvZ 4:dfed3b98ffb1 114 return 1;
RoyvZ 4:dfed3b98ffb1 115 }
RoyvZ 4:dfed3b98ffb1 116 }
RoyvZ 4:dfed3b98ffb1 117 // Next task, measure the error and apply the output to the plant
RoyvZ 4:dfed3b98ffb1 118 void m1_Controller()
RoyvZ 4:dfed3b98ffb1 119 {
RoyvZ 4:dfed3b98ffb1 120 reference = 5 * potMeterIn1.read();
RoyvZ 4:dfed3b98ffb1 121 //reference = qsetpoint(1,1);
RoyvZ 4:dfed3b98ffb1 122 outOfEncod = encoder1.getPosition();
RoyvZ 4:dfed3b98ffb1 123 position = RAD_PER_PULSE * outOfEncod;
RoyvZ 4:dfed3b98ffb1 124 float ref_pos = reference - position; // Don’t use magic numbers!
RoyvZ 4:dfed3b98ffb1 125 //motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );
RoyvZ 4:dfed3b98ffb1 126
RoyvZ 4:dfed3b98ffb1 127 if (ref_pos <= 0) { // Don’t use magic numbers!
RoyvZ 4:dfed3b98ffb1 128 motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );
RoyvZ 4:dfed3b98ffb1 129 } else {
RoyvZ 4:dfed3b98ffb1 130 motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );
RoyvZ 4:dfed3b98ffb1 131 }
RoyvZ 4:dfed3b98ffb1 132 if (ref_pos <= 0) {
RoyvZ 4:dfed3b98ffb1 133 //float motor1DirectionPin1 = 1;
RoyvZ 4:dfed3b98ffb1 134 motorD=1;
RoyvZ 4:dfed3b98ffb1 135 } else {
RoyvZ 4:dfed3b98ffb1 136 //float motor1DirectionPin1 = 0;
RoyvZ 4:dfed3b98ffb1 137 motorD=0;
RoyvZ 4:dfed3b98ffb1 138 }
RoyvZ 4:dfed3b98ffb1 139 motorDirection.write(motorD);
RoyvZ 4:dfed3b98ffb1 140 motorSpeed.write(motor1);
RoyvZ 4:dfed3b98ffb1 141 }
RoyvZ 4:dfed3b98ffb1 142
RoyvZ 4:dfed3b98ffb1 143
RoyvZ 3:2ffbee47fb38 144 double findRMS(vector<double> array)
RoyvZ 3:2ffbee47fb38 145 {
RoyvZ 4:dfed3b98ffb1 146 int i;
RoyvZ 4:dfed3b98ffb1 147 double sumsquared = 0.000;
RoyvZ 4:dfed3b98ffb1 148 double RMS;
RoyvZ 4:dfed3b98ffb1 149 //sumsquared = 0;
RoyvZ 4:dfed3b98ffb1 150 for (i = 0; i < size; i++) {
RoyvZ 3:2ffbee47fb38 151 sumsquared = sumsquared + array.at(i)*array.at(i);
RoyvZ 4:dfed3b98ffb1 152 }
RoyvZ 4:dfed3b98ffb1 153 RMS = sqrt((double(1)/size)*(sumsquared));
RoyvZ 4:dfed3b98ffb1 154 return RMS;
RoyvZ 3:2ffbee47fb38 155 }
RoyvZ 4:dfed3b98ffb1 156
RoyvZ 4:dfed3b98ffb1 157
RoyvZ 4:dfed3b98ffb1 158
RoyvZ 0:b6c8d56842ce 159 void sample()
RoyvZ 0:b6c8d56842ce 160 {
RoyvZ 3:2ffbee47fb38 161 S.erase(S.begin());
RoyvZ 3:2ffbee47fb38 162 double b = bqc.step(emg0.read()-0.4542);
RoyvZ 3:2ffbee47fb38 163 S.push_back(b);
RoyvZ 4:dfed3b98ffb1 164 emgX = findRMS(S);
RoyvZ 4:dfed3b98ffb1 165
RoyvZ 0:b6c8d56842ce 166 led = !led;
RoyvZ 4:dfed3b98ffb1 167
RoyvZ 4:dfed3b98ffb1 168 //motorSpeed.write(0.38080*(emgX/maxsignal));
RoyvZ 4:dfed3b98ffb1 169 //motorDirection.write(1);
RoyvZ 0:b6c8d56842ce 170 }
RoyvZ 0:b6c8d56842ce 171
RoyvZ 4:dfed3b98ffb1 172
RoyvZ 4:dfed3b98ffb1 173 void qSetpointSet()
RoyvZ 2:293665548183 174 {
RoyvZ 4:dfed3b98ffb1 175 // Fill Matrix with data.
RoyvZ 4:dfed3b98ffb1 176 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));
RoyvZ 4:dfed3b98ffb1 177 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));
RoyvZ 4:dfed3b98ffb1 178 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));
RoyvZ 4:dfed3b98ffb1 179 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));
RoyvZ 4:dfed3b98ffb1 180
RoyvZ 4:dfed3b98ffb1 181 // Fill Matrix with data.
RoyvZ 4:dfed3b98ffb1 182 Vdes(1,1) = emgX; //Goede code nog toevoegen, Vx
RoyvZ 4:dfed3b98ffb1 183 Vdes(2,1) = emgY; //goede code nog toevoegen, Vy
RoyvZ 4:dfed3b98ffb1 184
RoyvZ 4:dfed3b98ffb1 185 qdot = JAPPAPP*Vdes;
RoyvZ 4:dfed3b98ffb1 186
RoyvZ 4:dfed3b98ffb1 187 qsetpoint(1,1) = q1 + qdot(1,1)*Periode;
RoyvZ 4:dfed3b98ffb1 188 qsetpoint(2,1) = q2 + qdot(2,1)*Periode;
RoyvZ 4:dfed3b98ffb1 189
RoyvZ 4:dfed3b98ffb1 190 //motorSpeed.write(qsetpoint(1,1));
RoyvZ 4:dfed3b98ffb1 191 //motorDirection.write(1);
RoyvZ 2:293665548183 192 }
RoyvZ 2:293665548183 193
RoyvZ 4:dfed3b98ffb1 194
RoyvZ 0:b6c8d56842ce 195 int main()
RoyvZ 4:dfed3b98ffb1 196 {
RoyvZ 4:dfed3b98ffb1 197 pc.baud(115200);
RoyvZ 4:dfed3b98ffb1 198
RoyvZ 2:293665548183 199 //Constructing the notch filter chain
RoyvZ 3:2ffbee47fb38 200 Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 );
RoyvZ 3:2ffbee47fb38 201 Notch50.add( &bq1 ).add( &bq2 ).add( &bq3 );
RoyvZ 3:2ffbee47fb38 202 bqcLP.add( &bqLP1 ).add( &bqLP2 );
RoyvZ 3:2ffbee47fb38 203 bqc.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ).add( &bqLP1 ).add( &bqLP2 );
RoyvZ 0:b6c8d56842ce 204 /**Attach the 'sample' function to the timer 'sample_timer'.
RoyvZ 2:293665548183 205 * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz
RoyvZ 0:b6c8d56842ce 206 */
RoyvZ 3:2ffbee47fb38 207 sample_timer.attach(&sample, 0.002);
RoyvZ 4:dfed3b98ffb1 208 setpoint_timer.attach(&qSetpointSet, 0.002);
RoyvZ 3:2ffbee47fb38 209 led2 = 1;
RoyvZ 3:2ffbee47fb38 210 /*empty loop, sample() is executed periodically*/
RoyvZ 4:dfed3b98ffb1 211 m1_Ticker.attach( &m1_Controller, M1_TS );
RoyvZ 4:dfed3b98ffb1 212
RoyvZ 4:dfed3b98ffb1 213 m1_Controller();
RoyvZ 0:b6c8d56842ce 214
RoyvZ 0:b6c8d56842ce 215 while(true) {
RoyvZ 4:dfed3b98ffb1 216 pc.printf("%f \t",emgX);
RoyvZ 4:dfed3b98ffb1 217 pc.printf("%f \t",qdot(1,1));
RoyvZ 4:dfed3b98ffb1 218 pc.printf("%0.8f \t",qsetpoint(1,1));
RoyvZ 4:dfed3b98ffb1 219 pc.printf("%f \t",encoder1.getPosition());
RoyvZ 4:dfed3b98ffb1 220 pc.printf("%f \t",motorD);
RoyvZ 4:dfed3b98ffb1 221 pc.printf("%f \t",emgX);
RoyvZ 4:dfed3b98ffb1 222 pc.printf("%f \r\n",reference);
RoyvZ 4:dfed3b98ffb1 223
RoyvZ 0:b6c8d56842ce 224 }
RoyvZ 0:b6c8d56842ce 225
RoyvZ 0:b6c8d56842ce 226 }