![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
..
Dependencies: HIDScope MODSERIAL biquadFilter mbed
main.cpp@12:eaed305a76c3, 2018-10-19 (annotated)
- Committer:
- MarijkeZondag
- Date:
- Fri Oct 19 14:39:35 2018 +0000
- Revision:
- 12:eaed305a76c3
- Parent:
- 11:b95b0e9e1b89
- Child:
- 13:a3d4b4daf5b4
Alleen nog moving average erin pleuren;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:c8f15874531b | 1 | #include "mbed.h" |
vsluiter | 0:c8f15874531b | 2 | #include "MODSERIAL.h" |
MarijkeZondag | 10:39ec51206c8b | 3 | #include "BiQuad.h" |
MarijkeZondag | 10:39ec51206c8b | 4 | #include "HIDScope.h" |
MarijkeZondag | 10:39ec51206c8b | 5 | #include <math.h> |
vsluiter | 0:c8f15874531b | 6 | |
MarijkeZondag | 10:39ec51206c8b | 7 | AnalogIn emg0_in (A0); |
MarijkeZondag | 10:39ec51206c8b | 8 | AnalogIn emg1_in (A1); |
MarijkeZondag | 10:39ec51206c8b | 9 | AnalogIn emg2_in (A2); |
MarijkeZondag | 10:39ec51206c8b | 10 | |
MarijkeZondag | 9:c722418997b5 | 11 | DigitalIn button2 (D10); //Let op, is deze niet bezet? |
MarijkeZondag | 9:c722418997b5 | 12 | InterruptIn encoderA (D9); |
MarijkeZondag | 9:c722418997b5 | 13 | InterruptIn encoderB (D8); |
MarijkeZondag | 6:f4bbb73f3989 | 14 | |
MarijkeZondag | 9:c722418997b5 | 15 | DigitalOut directionpin1 (D4); |
MarijkeZondag | 9:c722418997b5 | 16 | DigitalOut directionpin2 (D7); |
MarijkeZondag | 9:c722418997b5 | 17 | PwmOut pwmpin1 (D5); |
MarijkeZondag | 9:c722418997b5 | 18 | PwmOut pwmpin2 (D6); |
MarijkeZondag | 9:c722418997b5 | 19 | |
MarijkeZondag | 6:f4bbb73f3989 | 20 | |
vsluiter | 0:c8f15874531b | 21 | MODSERIAL pc(USBTX, USBRX); |
vsluiter | 0:c8f15874531b | 22 | |
MarijkeZondag | 9:c722418997b5 | 23 | |
MarijkeZondag | 9:c722418997b5 | 24 | //Global variables |
MarijkeZondag | 12:eaed305a76c3 | 25 | int encoder = 0; |
MarijkeZondag | 12:eaed305a76c3 | 26 | const float T = 0.001f; //Ticker period |
MarijkeZondag | 10:39ec51206c8b | 27 | |
MarijkeZondag | 10:39ec51206c8b | 28 | //Biquad |
MarijkeZondag | 10:39ec51206c8b | 29 | BiQuadChain emg0band; |
MarijkeZondag | 10:39ec51206c8b | 30 | BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 31 | BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 32 | BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 33 | |
MarijkeZondag | 10:39ec51206c8b | 34 | BiQuadChain emg1band; |
MarijkeZondag | 10:39ec51206c8b | 35 | BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 36 | BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 37 | BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 38 | |
MarijkeZondag | 10:39ec51206c8b | 39 | BiQuadChain emg2band; |
MarijkeZondag | 10:39ec51206c8b | 40 | BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 41 | BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 42 | BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 43 | |
MarijkeZondag | 10:39ec51206c8b | 44 | BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter |
MarijkeZondag | 10:39ec51206c8b | 45 | |
MarijkeZondag | 10:39ec51206c8b | 46 | |
MarijkeZondag | 10:39ec51206c8b | 47 | //Tickers |
MarijkeZondag | 12:eaed305a76c3 | 48 | Ticker filter_tick; |
MarijkeZondag | 12:eaed305a76c3 | 49 | Ticker MovAg_tick; |
MarijkeZondag | 9:c722418997b5 | 50 | |
MarijkeZondag | 9:c722418997b5 | 51 | //Functions |
MarijkeZondag | 10:39ec51206c8b | 52 | void EMGFilter0() |
MarijkeZondag | 10:39ec51206c8b | 53 | { |
MarijkeZondag | 11:b95b0e9e1b89 | 54 | double emg0 = emg0_in.read(); |
MarijkeZondag | 11:b95b0e9e1b89 | 55 | double bandpass0 = emg0band.step(emg0); |
MarijkeZondag | 11:b95b0e9e1b89 | 56 | double absolute0 = fabs(bandpass0); |
MarijkeZondag | 11:b95b0e9e1b89 | 57 | double notch0 = notch1.step(absolute0); |
MarijkeZondag | 10:39ec51206c8b | 58 | } |
MarijkeZondag | 10:39ec51206c8b | 59 | |
MarijkeZondag | 10:39ec51206c8b | 60 | void EMGFilter1() |
MarijkeZondag | 10:39ec51206c8b | 61 | { |
MarijkeZondag | 11:b95b0e9e1b89 | 62 | double emg1 = emg1_in.read(); |
MarijkeZondag | 11:b95b0e9e1b89 | 63 | double bandpass1 = emg1band.step(emg1); |
MarijkeZondag | 11:b95b0e9e1b89 | 64 | double absolute1 = fabs(bandpass1); |
MarijkeZondag | 11:b95b0e9e1b89 | 65 | double notch1 = notch1.step(absolute1); |
MarijkeZondag | 10:39ec51206c8b | 66 | } |
MarijkeZondag | 10:39ec51206c8b | 67 | |
MarijkeZondag | 10:39ec51206c8b | 68 | void EMGFilter2() |
MarijkeZondag | 10:39ec51206c8b | 69 | { |
MarijkeZondag | 11:b95b0e9e1b89 | 70 | double emg2 = emg2_in.read(); |
MarijkeZondag | 11:b95b0e9e1b89 | 71 | double bandpass2 = emg2band.step(emg2); |
MarijkeZondag | 11:b95b0e9e1b89 | 72 | double absolute2 = fabs(bandpass2); |
MarijkeZondag | 11:b95b0e9e1b89 | 73 | double notch2 = notch1.step(absolute2); |
MarijkeZondag | 10:39ec51206c8b | 74 | } |
MarijkeZondag | 10:39ec51206c8b | 75 | |
MarijkeZondag | 12:eaed305a76c3 | 76 | void emg_filtered() //call all filter functions |
MarijkeZondag | 12:eaed305a76c3 | 77 | { |
MarijkeZondag | 12:eaed305a76c3 | 78 | EMGFilter0(); |
MarijkeZondag | 12:eaed305a76c3 | 79 | EMGFilter1(); |
MarijkeZondag | 12:eaed305a76c3 | 80 | EMGFilter2(); |
MarijkeZondag | 12:eaed305a76c3 | 81 | } |
MarijkeZondag | 12:eaed305a76c3 | 82 | |
MarijkeZondag | 12:eaed305a76c3 | 83 | void MovAg() //calculate moving average |
MarijkeZondag | 12:eaed305a76c3 | 84 | { |
MarijkeZondag | 12:eaed305a76c3 | 85 | for i = |
MarijkeZondag | 12:eaed305a76c3 | 86 | } |
MarijkeZondag | 12:eaed305a76c3 | 87 | |
MarijkeZondag | 8:895d941a5910 | 88 | void encoderA_rise() |
MarijkeZondag | 8:895d941a5910 | 89 | { |
MarijkeZondag | 8:895d941a5910 | 90 | if(encoderB==false) |
MarijkeZondag | 8:895d941a5910 | 91 | { |
MarijkeZondag | 8:895d941a5910 | 92 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 93 | } |
MarijkeZondag | 8:895d941a5910 | 94 | else |
MarijkeZondag | 8:895d941a5910 | 95 | { |
MarijkeZondag | 8:895d941a5910 | 96 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 97 | } |
MarijkeZondag | 8:895d941a5910 | 98 | } |
MarijkeZondag | 8:895d941a5910 | 99 | |
MarijkeZondag | 8:895d941a5910 | 100 | void encoderA_fall() |
MarijkeZondag | 8:895d941a5910 | 101 | { |
MarijkeZondag | 8:895d941a5910 | 102 | if(encoderB==true) |
MarijkeZondag | 8:895d941a5910 | 103 | { |
MarijkeZondag | 8:895d941a5910 | 104 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 105 | } |
MarijkeZondag | 8:895d941a5910 | 106 | else |
MarijkeZondag | 8:895d941a5910 | 107 | { |
MarijkeZondag | 8:895d941a5910 | 108 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 109 | } |
MarijkeZondag | 8:895d941a5910 | 110 | } |
MarijkeZondag | 8:895d941a5910 | 111 | |
MarijkeZondag | 8:895d941a5910 | 112 | void encoderB_rise() |
MarijkeZondag | 8:895d941a5910 | 113 | { |
MarijkeZondag | 8:895d941a5910 | 114 | if(encoderA==true) |
MarijkeZondag | 8:895d941a5910 | 115 | { |
MarijkeZondag | 8:895d941a5910 | 116 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 117 | } |
MarijkeZondag | 8:895d941a5910 | 118 | else |
MarijkeZondag | 8:895d941a5910 | 119 | { |
MarijkeZondag | 8:895d941a5910 | 120 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 121 | } |
MarijkeZondag | 8:895d941a5910 | 122 | } |
MarijkeZondag | 8:895d941a5910 | 123 | |
MarijkeZondag | 8:895d941a5910 | 124 | void encoderB_fall() |
MarijkeZondag | 8:895d941a5910 | 125 | { |
MarijkeZondag | 8:895d941a5910 | 126 | if(encoderA==false) |
MarijkeZondag | 8:895d941a5910 | 127 | { |
MarijkeZondag | 8:895d941a5910 | 128 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 129 | } |
MarijkeZondag | 8:895d941a5910 | 130 | else |
MarijkeZondag | 8:895d941a5910 | 131 | { |
MarijkeZondag | 8:895d941a5910 | 132 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 133 | } |
MarijkeZondag | 8:895d941a5910 | 134 | } |
MarijkeZondag | 8:895d941a5910 | 135 | |
MarijkeZondag | 8:895d941a5910 | 136 | |
MarijkeZondag | 9:c722418997b5 | 137 | // Main function start. |
MarijkeZondag | 8:895d941a5910 | 138 | |
vsluiter | 0:c8f15874531b | 139 | int main() |
MarijkeZondag | 3:a3ad58758696 | 140 | { |
vsluiter | 0:c8f15874531b | 141 | pc.baud(115200); |
MarijkeZondag | 6:f4bbb73f3989 | 142 | pc.printf("hello\n\r"); |
MarijkeZondag | 10:39ec51206c8b | 143 | |
MarijkeZondag | 10:39ec51206c8b | 144 | //EMG signaal filteren |
MarijkeZondag | 10:39ec51206c8b | 145 | |
MarijkeZondag | 12:eaed305a76c3 | 146 | filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec. |
MarijkeZondag | 12:eaed305a76c3 | 147 | MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec. |
MarijkeZondag | 12:eaed305a76c3 | 148 | |
MarijkeZondag | 10:39ec51206c8b | 149 | while(true){/*do not return from main()*/} |
MarijkeZondag | 10:39ec51206c8b | 150 | |
MarijkeZondag | 10:39ec51206c8b | 151 | |
MarijkeZondag | 6:f4bbb73f3989 | 152 | pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz |
MarijkeZondag | 6:f4bbb73f3989 | 153 | |
MarijkeZondag | 8:895d941a5910 | 154 | encoderA.rise(&encoderA_rise); |
MarijkeZondag | 8:895d941a5910 | 155 | encoderA.fall(&encoderA_fall); |
MarijkeZondag | 8:895d941a5910 | 156 | encoderB.rise(&encoderB_rise); |
MarijkeZondag | 8:895d941a5910 | 157 | encoderB.fall(&encoderB_fall); |
vsluiter | 0:c8f15874531b | 158 | |
MarijkeZondag | 2:2f4444f1504d | 159 | while (true) |
MarijkeZondag | 2:2f4444f1504d | 160 | { |
MarijkeZondag | 10:39ec51206c8b | 161 | //Motor aansturen en encoder uitlezen |
MarijkeZondag | 6:f4bbb73f3989 | 162 | float u1 = potmetervalue1; |
MarijkeZondag | 6:f4bbb73f3989 | 163 | float u2 = potmetervalue2; |
MarijkeZondag | 6:f4bbb73f3989 | 164 | |
MarijkeZondag | 6:f4bbb73f3989 | 165 | float m1 = ((u1*2.0f)-1.0f); |
MarijkeZondag | 6:f4bbb73f3989 | 166 | float m2 = ((u2*2.0f)-1.0f); |
MarijkeZondag | 7:f32005d13749 | 167 | |
MarijkeZondag | 7:f32005d13749 | 168 | pwmpin1 = fabs(m1*0.6f)+0.4f; //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling. |
MarijkeZondag | 7:f32005d13749 | 169 | directionpin1.write(m1>0); //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom. |
MarijkeZondag | 7:f32005d13749 | 170 | wait(0.01f); //zodat de code niet oneindig doorgaat. |
MarijkeZondag | 7:f32005d13749 | 171 | pwmpin2 = fabs(m2*0.6f)+0.4f; |
MarijkeZondag | 8:895d941a5910 | 172 | directionpin2.write(m2>0); |
MarijkeZondag | 10:39ec51206c8b | 173 | |
MarijkeZondag | 10:39ec51206c8b | 174 | float encoderDegrees = float(encoder)*(360.0/8400.0); |
MarijkeZondag | 9:c722418997b5 | 175 | |
MarijkeZondag | 10:39ec51206c8b | 176 | pc.printf("Encoder count: %f \n\r",encoderDegrees); |
MarijkeZondag | 10:39ec51206c8b | 177 | |
vsluiter | 0:c8f15874531b | 178 | } |
vsluiter | 0:c8f15874531b | 179 | } |
MarijkeZondag | 4:fbea05c8fe2b | 180 |