Voor matthijs
Dependencies: Encoder MODSERIAL Matrix MatrixMath QEI biquad-master mbed
Fork of frdm_gpio1 by
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?
User | Revision | Line number | New 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 | } |