pid gecomment
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
main.cpp@37:60dd2e42bf8f, 2016-10-28 (annotated)
- Committer:
- daniQQue
- Date:
- Fri Oct 28 12:53:42 2016 +0000
- Revision:
- 37:60dd2e42bf8f
- Parent:
- 15:bb4a6c7836d8
- Child:
- 38:23601b26bb84
filter notch werkt niet; dat is het probleem; simpel script alleen biceps;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
daniQQue | 0:34c739fcc3e0 | 1 | //libraries |
daniQQue | 0:34c739fcc3e0 | 2 | #include "mbed.h" |
daniQQue | 0:34c739fcc3e0 | 3 | #include "HIDScope.h" |
daniQQue | 14:5b17697cf775 | 4 | #include "BiQuad.h" |
daniQQue | 0:34c739fcc3e0 | 5 | |
daniQQue | 0:34c739fcc3e0 | 6 | //Define objects |
daniQQue | 15:bb4a6c7836d8 | 7 | AnalogIn emg_biceps_right_in( A0 ); //analog in to get EMG in to c++ |
daniQQue | 0:34c739fcc3e0 | 8 | Ticker sample_timer; //ticker |
daniQQue | 14:5b17697cf775 | 9 | HIDScope scope( 3); //open 3 channels in hidscope |
daniQQue | 8:cd0cb71b69f2 | 10 | DigitalOut richting_motor1(D4); //motor1 direction output |
daniQQue | 8:cd0cb71b69f2 | 11 | PwmOut pwm_motor1(D5); //motor1 velocity output |
daniQQue | 0:34c739fcc3e0 | 12 | DigitalOut led(LED_GREEN); |
daniQQue | 0:34c739fcc3e0 | 13 | |
daniQQue | 0:34c739fcc3e0 | 14 | //define variables |
daniQQue | 15:bb4a6c7836d8 | 15 | double emg_biceps_right; |
daniQQue | 15:bb4a6c7836d8 | 16 | double emg_filtered_high_biceps_right; |
daniQQue | 15:bb4a6c7836d8 | 17 | double emg_abs_biceps_right; |
daniQQue | 15:bb4a6c7836d8 | 18 | double emg_filtered_biceps_right; |
daniQQue | 7:42d0e38196f1 | 19 | int onoffsignal=0; |
daniQQue | 15:bb4a6c7836d8 | 20 | double cut_off_value=0.08; //gespecifeerd door floortje |
daniQQue | 5:688b1b5530d8 | 21 | |
daniQQue | 37:60dd2e42bf8f | 22 | BiQuad filterhigh(9.1497e-01, -1.8299e+00, 9.1497e-01, -1.8227e+00, 8.3718e-01); // |
daniQQue | 37:60dd2e42bf8f | 23 | BiQuad filterlow ( 3.9130e-05 , 7.8260e-05 , 3.9130e-05, -1.9822e+00 , 9.8239e-01); // |
daniQQue | 37:60dd2e42bf8f | 24 | BiQuad notch1(9.9115e-01 ,-1.8853e+00 , 9.9115e-01 , -1.8909e+00 , 9.9103e-01); |
daniQQue | 37:60dd2e42bf8f | 25 | BiQuad notch2( 1.0000e+00 , -1.9022e+00, 1.0000e+00 , -1.8965e+00 , 9.9127e-01); |
daniQQue | 10:7255b59224cc | 26 | |
daniQQue | 15:bb4a6c7836d8 | 27 | //functions which are called in ticker |
daniQQue | 0:34c739fcc3e0 | 28 | void filter(){ |
daniQQue | 15:bb4a6c7836d8 | 29 | emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes |
daniQQue | 15:bb4a6c7836d8 | 30 | emg_filtered_high_biceps_right= filterhigh.step(emg_biceps_right); |
daniQQue | 37:60dd2e42bf8f | 31 | double emg_filtered_notch1=notch1.step(emg_filtered_high_biceps_right); |
daniQQue | 37:60dd2e42bf8f | 32 | double emg_filtered_notch2=notch2.step(emg_filtered_notch1); |
daniQQue | 37:60dd2e42bf8f | 33 | emg_abs_biceps_right=fabs(emg_filtered_notch2); //fabs because float |
daniQQue | 15:bb4a6c7836d8 | 34 | emg_filtered_biceps_right=filterlow.step(emg_abs_biceps_right); |
daniQQue | 0:34c739fcc3e0 | 35 | led=!led; |
daniQQue | 7:42d0e38196f1 | 36 | |
daniQQue | 15:bb4a6c7836d8 | 37 | if (emg_filtered_biceps_right>cut_off_value) |
daniQQue | 7:42d0e38196f1 | 38 | {onoffsignal=1;} |
daniQQue | 7:42d0e38196f1 | 39 | |
daniQQue | 7:42d0e38196f1 | 40 | else |
daniQQue | 37:60dd2e42bf8f | 41 | {onoffsignal=0;} |
daniQQue | 7:42d0e38196f1 | 42 | |
daniQQue | 0:34c739fcc3e0 | 43 | //send signals to scope |
daniQQue | 37:60dd2e42bf8f | 44 | scope.set(0, emg_filtered_notch2 ); //set emg signal to scope in channel 1 |
daniQQue | 37:60dd2e42bf8f | 45 | scope.set(1, emg_abs_biceps_right); |
daniQQue | 37:60dd2e42bf8f | 46 | scope.set(2, emg_filtered_biceps_right); |
daniQQue | 37:60dd2e42bf8f | 47 | scope.send(); //send all the signals to the scope |
daniQQue | 0:34c739fcc3e0 | 48 | } |
daniQQue | 0:34c739fcc3e0 | 49 | |
daniQQue | 0:34c739fcc3e0 | 50 | //program |
daniQQue | 0:34c739fcc3e0 | 51 | |
daniQQue | 0:34c739fcc3e0 | 52 | int main() |
daniQQue | 0:34c739fcc3e0 | 53 | { |
daniQQue | 0:34c739fcc3e0 | 54 | sample_timer.attach(&filter, 0.001); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds |
daniQQue | 4:7d9ca9c1dcce | 55 | |
daniQQue | 0:34c739fcc3e0 | 56 | //endless loop |
daniQQue | 0:34c739fcc3e0 | 57 | |
daniQQue | 0:34c739fcc3e0 | 58 | while(1) |
daniQQue | 8:cd0cb71b69f2 | 59 | { |
daniQQue | 8:cd0cb71b69f2 | 60 | if (onoffsignal==1) |
daniQQue | 8:cd0cb71b69f2 | 61 | { |
daniQQue | 8:cd0cb71b69f2 | 62 | richting_motor1 = 0; //motordirection (ccw) |
daniQQue | 10:7255b59224cc | 63 | pwm_motor1 = 1; //motorspeed 1 |
daniQQue | 8:cd0cb71b69f2 | 64 | |
daniQQue | 8:cd0cb71b69f2 | 65 | } |
daniQQue | 8:cd0cb71b69f2 | 66 | else if(onoffsignal==0) |
daniQQue | 8:cd0cb71b69f2 | 67 | { |
daniQQue | 8:cd0cb71b69f2 | 68 | richting_motor1 = 0; //motordirection (ccw) |
daniQQue | 8:cd0cb71b69f2 | 69 | pwm_motor1 = 0; //motorspeed 0 |
daniQQue | 8:cd0cb71b69f2 | 70 | |
daniQQue | 8:cd0cb71b69f2 | 71 | } |
daniQQue | 8:cd0cb71b69f2 | 72 | |
daniQQue | 8:cd0cb71b69f2 | 73 | } |
daniQQue | 0:34c739fcc3e0 | 74 | |
daniQQue | 0:34c739fcc3e0 | 75 | } |