pid gecomment
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
main.cpp@19:fb98ff1d06ed, 2016-10-24 (annotated)
- Committer:
- daniQQue
- Date:
- Mon Oct 24 12:52:46 2016 +0000
- Revision:
- 19:fb98ff1d06ed
- Parent:
- 18:d7695ac04de3
- Child:
- 20:a0495210915b
unsuccesful try; to calibrate
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 | 19:fb98ff1d06ed | 5 | #include "MODSERIAL.h" |
daniQQue | 0:34c739fcc3e0 | 6 | |
daniQQue | 0:34c739fcc3e0 | 7 | //Define objects |
daniQQue | 18:d7695ac04de3 | 8 | AnalogIn emg_biceps_right_in( A0 ); //analog in to get EMG in to c++ |
daniQQue | 16:fd4521a4f0b3 | 9 | Ticker sample_timer; //ticker |
daniQQue | 18:d7695ac04de3 | 10 | HIDScope scope( 3); //open 3 channels in hidscope |
daniQQue | 8:cd0cb71b69f2 | 11 | DigitalOut richting_motor1(D4); //motor1 direction output |
daniQQue | 8:cd0cb71b69f2 | 12 | PwmOut pwm_motor1(D5); //motor1 velocity output |
daniQQue | 19:fb98ff1d06ed | 13 | MODSERIAL pc(USBTX,USBRX); //connection with computer |
daniQQue | 19:fb98ff1d06ed | 14 | DigitalOut led(LED_GREEN); //include led |
daniQQue | 19:fb98ff1d06ed | 15 | DigitalIn button (D9); //button |
daniQQue | 0:34c739fcc3e0 | 16 | |
daniQQue | 19:fb98ff1d06ed | 17 | //define constant |
daniQQue | 19:fb98ff1d06ed | 18 | const int freq=1000; //chosen sample frequency |
daniQQue | 0:34c739fcc3e0 | 19 | //define variables |
daniQQue | 19:fb98ff1d06ed | 20 | |
daniQQue | 18:d7695ac04de3 | 21 | double emg_biceps_right; |
daniQQue | 18:d7695ac04de3 | 22 | double emg_filtered_high_biceps_right; |
daniQQue | 18:d7695ac04de3 | 23 | double emg_abs_biceps_right; |
daniQQue | 18:d7695ac04de3 | 24 | double emg_filtered_biceps_right; |
daniQQue | 7:42d0e38196f1 | 25 | int onoffsignal=0; |
daniQQue | 18:d7695ac04de3 | 26 | double cut_off_value=0.20; |
daniQQue | 18:d7695ac04de3 | 27 | double max_right_biceps; |
daniQQue | 18:d7695ac04de3 | 28 | |
daniQQue | 15:bb4a6c7836d8 | 29 | BiQuad filterhigh(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); |
daniQQue | 15:bb4a6c7836d8 | 30 | BiQuad filterlow (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); |
daniQQue | 10:7255b59224cc | 31 | |
daniQQue | 15:bb4a6c7836d8 | 32 | //functions which are called in ticker |
daniQQue | 0:34c739fcc3e0 | 33 | void filter(){ |
daniQQue | 19:fb98ff1d06ed | 34 | emg_biceps_right = emg_biceps_right_in.read(); //read the emg value from the electrodes |
daniQQue | 19:fb98ff1d06ed | 35 | emg_filtered_high_biceps_right = filterhigh.step(emg_biceps_right); |
daniQQue | 19:fb98ff1d06ed | 36 | emg_abs_biceps_right =fabs(emg_filtered_high_biceps_right); //fabs because float |
daniQQue | 19:fb98ff1d06ed | 37 | emg_filtered_biceps_right =filterlow.step(emg_abs_biceps_right); //gefilterd met high, abs, low |
daniQQue | 18:d7695ac04de3 | 38 | led=!led; |
daniQQue | 7:42d0e38196f1 | 39 | |
daniQQue | 18:d7695ac04de3 | 40 | if (emg_filtered_biceps_right>cut_off_value) |
daniQQue | 7:42d0e38196f1 | 41 | {onoffsignal=1;} |
daniQQue | 7:42d0e38196f1 | 42 | |
daniQQue | 7:42d0e38196f1 | 43 | else |
daniQQue | 7:42d0e38196f1 | 44 | {onoffsignal=0;} |
daniQQue | 7:42d0e38196f1 | 45 | |
daniQQue | 0:34c739fcc3e0 | 46 | //send signals to scope |
daniQQue | 18:d7695ac04de3 | 47 | scope.set(0, emg_biceps_right ); //set emg signal to scope in channel 1 |
daniQQue | 18:d7695ac04de3 | 48 | scope.set(1, emg_filtered_biceps_right); |
daniQQue | 18:d7695ac04de3 | 49 | scope.set(2, onoffsignal); |
daniQQue | 0:34c739fcc3e0 | 50 | scope.send(); //send all the signals to the scope |
daniQQue | 0:34c739fcc3e0 | 51 | } |
daniQQue | 18:d7695ac04de3 | 52 | |
daniQQue | 18:d7695ac04de3 | 53 | void calibration(){ |
daniQQue | 19:fb98ff1d06ed | 54 | if(button==0) |
daniQQue | 19:fb98ff1d06ed | 55 | { |
daniQQue | 19:fb98ff1d06ed | 56 | for(int n =0; n<2000;n++) //read for 5000 samples as calibration |
daniQQue | 19:fb98ff1d06ed | 57 | { |
daniQQue | 18:d7695ac04de3 | 58 | emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes |
daniQQue | 18:d7695ac04de3 | 59 | emg_filtered_high_biceps_right= filterhigh.step(emg_biceps_right); //highpass |
daniQQue | 18:d7695ac04de3 | 60 | emg_abs_biceps_right=fabs(emg_filtered_high_biceps_right); //fabs because float |
daniQQue | 18:d7695ac04de3 | 61 | emg_filtered_biceps_right=filterlow.step(emg_abs_biceps_right); //lowpass to envelope |
daniQQue | 19:fb98ff1d06ed | 62 | |
daniQQue | 19:fb98ff1d06ed | 63 | if (emg_filtered_biceps_right > max_right_biceps) //determine what the highest reachable emg signal is |
daniQQue | 19:fb98ff1d06ed | 64 | { |
daniQQue | 19:fb98ff1d06ed | 65 | max_right_biceps = emg_filtered_biceps_right; |
daniQQue | 18:d7695ac04de3 | 66 | } |
daniQQue | 19:fb98ff1d06ed | 67 | } |
daniQQue | 19:fb98ff1d06ed | 68 | cut_off_value=0.2*max_right_biceps; |
daniQQue | 19:fb98ff1d06ed | 69 | pc.printf(" change of cv %f ",cut_off_value ); |
daniQQue | 18:d7695ac04de3 | 70 | } |
daniQQue | 19:fb98ff1d06ed | 71 | } |
daniQQue | 0:34c739fcc3e0 | 72 | //program |
daniQQue | 0:34c739fcc3e0 | 73 | |
daniQQue | 0:34c739fcc3e0 | 74 | int main() |
daniQQue | 18:d7695ac04de3 | 75 | { |
daniQQue | 19:fb98ff1d06ed | 76 | pc.baud(115200); //start pc connection baudrate 115200 |
daniQQue | 4:7d9ca9c1dcce | 77 | |
daniQQue | 19:fb98ff1d06ed | 78 | sample_timer.attach (&filter, 1/freq); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds |
daniQQue | 19:fb98ff1d06ed | 79 | sample_timer.attach (&calibration,1/freq); //continously execute callibration, only affects it when button is pressed for a while. |
daniQQue | 0:34c739fcc3e0 | 80 | //endless loop |
daniQQue | 18:d7695ac04de3 | 81 | while(1) { |
daniQQue | 19:fb98ff1d06ed | 82 | |
daniQQue | 8:cd0cb71b69f2 | 83 | if (onoffsignal==1) |
daniQQue | 8:cd0cb71b69f2 | 84 | { |
daniQQue | 8:cd0cb71b69f2 | 85 | richting_motor1 = 0; //motordirection (ccw) |
daniQQue | 10:7255b59224cc | 86 | pwm_motor1 = 1; //motorspeed 1 |
daniQQue | 8:cd0cb71b69f2 | 87 | |
daniQQue | 8:cd0cb71b69f2 | 88 | } |
daniQQue | 8:cd0cb71b69f2 | 89 | else if(onoffsignal==0) |
daniQQue | 8:cd0cb71b69f2 | 90 | { |
daniQQue | 8:cd0cb71b69f2 | 91 | richting_motor1 = 0; //motordirection (ccw) |
daniQQue | 8:cd0cb71b69f2 | 92 | pwm_motor1 = 0; //motorspeed 0 |
daniQQue | 18:d7695ac04de3 | 93 | } |
daniQQue | 8:cd0cb71b69f2 | 94 | |
daniQQue | 18:d7695ac04de3 | 95 | } |
daniQQue | 0:34c739fcc3e0 | 96 | } |