State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
EMG_calibration.h@36:691ea4660f29, 2018-11-01 (annotated)
- Committer:
- hermanindeput
- Date:
- Thu Nov 01 11:10:34 2018 +0000
- Revision:
- 36:691ea4660f29
- Parent:
- 35:38a5af0afee8
Timeouts and everything (heeft nog errors);
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hermanindeput | 35:38a5af0afee8 | 1 | #pragma once |
hermanindeput | 35:38a5af0afee8 | 2 | |
hermanindeput | 35:38a5af0afee8 | 3 | #include "mbed.h" |
hermanindeput | 35:38a5af0afee8 | 4 | #include "MODSERIAL.h" |
hermanindeput | 35:38a5af0afee8 | 5 | #include "Screen.h" |
hermanindeput | 35:38a5af0afee8 | 6 | #include "EMGFilter.h" |
hermanindeput | 35:38a5af0afee8 | 7 | |
hermanindeput | 36:691ea4660f29 | 8 | class EMG_calibration |
hermanindeput | 36:691ea4660f29 | 9 | { |
hermanindeput | 36:691ea4660f29 | 10 | private: |
hermanindeput | 35:38a5af0afee8 | 11 | Ticker calibration_timer; |
hermanindeput | 36:691ea4660f29 | 12 | timeout adjustment_time; |
hermanindeput | 35:38a5af0afee8 | 13 | Timeout calibration_bicep; // Timeout to get to relax state |
hermanindeput | 35:38a5af0afee8 | 14 | Timeout end_of_calibration; // Timeout to get to final state |
hermanindeput | 36:691ea4660f29 | 15 | EMG_Filter* EMG_filter |
hermanindeput | 36:691ea4660f29 | 16 | Screen* screen; |
hermanindeput | 35:38a5af0afee8 | 17 | |
hermanindeput | 36:691ea4660f29 | 18 | //variables |
hermanindeput | 35:38a5af0afee8 | 19 | volatile float EMG_signal1; |
hermanindeput | 36:691ea4660f29 | 20 | volatile float EMG_min_relax; |
hermanindeput | 36:691ea4660f29 | 21 | volatile float EMG_min_tense; |
hermanindeput | 36:691ea4660f29 | 22 | volatile float EMG_max_relax; |
hermanindeput | 36:691ea4660f29 | 23 | volatile float EMG_max_tense; |
hermanindeput | 35:38a5af0afee8 | 24 | volatile float EMG_threshold; |
hermanindeput | 35:38a5af0afee8 | 25 | // IN STATE MACHINE: |
hermanindeput | 36:691ea4660f29 | 26 | EMGFilter emg_input *EMGFilter; // Voor kalibratie van bicep 1 |
hermanindeput | 35:38a5af0afee8 | 27 | //EMGFilter emg_input = EMGFilter(A1); // Voor kalibratie van bicep 2 |
hermanindeput | 35:38a5af0afee8 | 28 | |
hermanindeput | 36:691ea4660f29 | 29 | EMG_calibration(Screen *new_screen,EMGFilter *EMG_filter){ |
hermanindeput | 36:691ea4660f29 | 30 | screen= new_screen; |
hermanindeput | 36:691ea4660f29 | 31 | EMG_min_relax=1.000; |
hermanindeput | 36:691ea4660f29 | 32 | EMG_min_tense=1.000; |
hermanindeput | 36:691ea4660f29 | 33 | EMG_max_relax=0.000; |
hermanindeput | 36:691ea4660f29 | 34 | EMG_max_tense=0.000; |
hermanindeput | 36:691ea4660f29 | 35 | relax_calibration(); |
hermanindeput | 36:691ea4660f29 | 36 | } |
hermanindeput | 36:691ea4660f29 | 37 | |
hermanindeput | 35:38a5af0afee8 | 38 | void tense_calibration() |
hermanindeput | 35:38a5af0afee8 | 39 | { |
hermanindeput | 35:38a5af0afee8 | 40 | EMG_signal1 = emg_input.get_envelope_emg(); |
hermanindeput | 35:38a5af0afee8 | 41 | if (EMG_max_tense < EMG_signal1) |
hermanindeput | 35:38a5af0afee8 | 42 | { |
hermanindeput | 35:38a5af0afee8 | 43 | EMG_max_tense=EMG_signal1; |
hermanindeput | 35:38a5af0afee8 | 44 | } |
hermanindeput | 35:38a5af0afee8 | 45 | if (EMG_min_tense > EMG_signal1) |
hermanindeput | 35:38a5af0afee8 | 46 | { |
hermanindeput | 35:38a5af0afee8 | 47 | EMG_min_tense=EMG_signal1; |
hermanindeput | 35:38a5af0afee8 | 48 | } |
hermanindeput | 35:38a5af0afee8 | 49 | screen.get_screen_handle()->setTextCursor(0, 0); |
hermanindeput | 35:38a5af0afee8 | 50 | screen.get_screen_handle()->printf("Tense "); |
hermanindeput | 35:38a5af0afee8 | 51 | screen.get_screen_handle()->printf("EMG maximum= %.6f",EMG_max_tense); |
hermanindeput | 35:38a5af0afee8 | 52 | screen.get_screen_handle()->printf("EMG minimum= %.6f",EMG_min_tense); |
hermanindeput | 35:38a5af0afee8 | 53 | screen.get_screen_handle()->printf(" ",EMG_min_tense); |
hermanindeput | 35:38a5af0afee8 | 54 | screen.display(); |
hermanindeput | 35:38a5af0afee8 | 55 | } |
hermanindeput | 35:38a5af0afee8 | 56 | |
hermanindeput | 35:38a5af0afee8 | 57 | void relax_calibration() |
hermanindeput | 35:38a5af0afee8 | 58 | { |
hermanindeput | 35:38a5af0afee8 | 59 | EMG_signal1 = emg_input.get_envelope_emg(); |
hermanindeput | 35:38a5af0afee8 | 60 | if (EMG_max_relax < EMG_signal1) |
hermanindeput | 35:38a5af0afee8 | 61 | { |
hermanindeput | 35:38a5af0afee8 | 62 | EMG_max_relax=EMG_signal1; |
hermanindeput | 35:38a5af0afee8 | 63 | } |
hermanindeput | 35:38a5af0afee8 | 64 | if (EMG_min_relax > EMG_signal1) |
hermanindeput | 35:38a5af0afee8 | 65 | { |
hermanindeput | 35:38a5af0afee8 | 66 | EMG_min_relax=EMG_signal1; |
hermanindeput | 35:38a5af0afee8 | 67 | } |
hermanindeput | 35:38a5af0afee8 | 68 | EMG_threshold=(((EMG_min_tense-EMG_max_relax)/2)+EMG_max_relax); |
hermanindeput | 35:38a5af0afee8 | 69 | screen.get_screen_handle()->setTextCursor(0, 0); |
hermanindeput | 35:38a5af0afee8 | 70 | screen.get_screen_handle()->printf("Relax "); |
hermanindeput | 35:38a5af0afee8 | 71 | screen.get_screen_handle()->printf("EMG maximum= %.6f",EMG_max_relax); |
hermanindeput | 35:38a5af0afee8 | 72 | screen.get_screen_handle()->printf("EMG minimum= %.6f",EMG_min_relax); |
hermanindeput | 35:38a5af0afee8 | 73 | screen.get_screen_handle()->printf("threshold= %.6f",EMG_threshold); |
hermanindeput | 35:38a5af0afee8 | 74 | screen.display(); |
hermanindeput | 35:38a5af0afee8 | 75 | } |
hermanindeput | 35:38a5af0afee8 | 76 | |
hermanindeput | 35:38a5af0afee8 | 77 | void TheFinalCalibration() |
hermanindeput | 35:38a5af0afee8 | 78 | { |
hermanindeput | 35:38a5af0afee8 | 79 | calibration_bicep.detach(); |
hermanindeput | 35:38a5af0afee8 | 80 | calibration_timer.detach(); |
hermanindeput | 35:38a5af0afee8 | 81 | screen.get_screen_handle()->setTextCursor(0, 0); |
hermanindeput | 35:38a5af0afee8 | 82 | screen.get_screen_handle()->printf("threshold= %.6f",EMG_threshold); |
hermanindeput | 35:38a5af0afee8 | 83 | screen.get_screen_handle()->printf(" "); |
hermanindeput | 35:38a5af0afee8 | 84 | screen.get_screen_handle()->printf(" "); |
hermanindeput | 35:38a5af0afee8 | 85 | screen.get_screen_handle()->printf(" "); |
hermanindeput | 35:38a5af0afee8 | 86 | screen.display(); |
hermanindeput | 35:38a5af0afee8 | 87 | emg_input.set_threshold(EMG_threshold); |
hermanindeput | 35:38a5af0afee8 | 88 | } |
hermanindeput | 35:38a5af0afee8 | 89 | |
hermanindeput | 35:38a5af0afee8 | 90 | void relax_measure() |
hermanindeput | 35:38a5af0afee8 | 91 | { |
hermanindeput | 35:38a5af0afee8 | 92 | calibration_timer.detach(); |
hermanindeput | 35:38a5af0afee8 | 93 | screen.get_screen_handle()->setTextCursor(0, 0); |
hermanindeput | 35:38a5af0afee8 | 94 | screen.get_screen_handle()->printf("Relax muscle ",EMG_max_relax); |
hermanindeput | 35:38a5af0afee8 | 95 | screen.get_screen_handle()->printf(" ",EMG_min_tense); |
hermanindeput | 35:38a5af0afee8 | 96 | screen.get_screen_handle()->printf(" ",EMG_min_tense); |
hermanindeput | 35:38a5af0afee8 | 97 | screen.get_screen_handle()->printf(" ",EMG_min_tense); |
hermanindeput | 35:38a5af0afee8 | 98 | screen.display(); |
hermanindeput | 36:691ea4660f29 | 99 | adjustment_time.attach(callback(this, &EMG_calibration::start_relax),2.5f); |
hermanindeput | 36:691ea4660f29 | 100 | } |
hermanindeput | 36:691ea4660f29 | 101 | void start_relax(){ |
hermanindeput | 35:38a5af0afee8 | 102 | calibration_timer.attach(&relax_calibration, 0.1); |
hermanindeput | 36:691ea4660f29 | 103 | end_of_calibration.attach(&relax_measure,5.0f); |
hermanindeput | 35:38a5af0afee8 | 104 | } |
hermanindeput | 35:38a5af0afee8 | 105 | |
hermanindeput | 35:38a5af0afee8 | 106 | void tense_measure() |
hermanindeput | 35:38a5af0afee8 | 107 | { |
hermanindeput | 35:38a5af0afee8 | 108 | calibration_timer.detach(); |
hermanindeput | 35:38a5af0afee8 | 109 | screen.get_screen_handle()->setTextCursor(0, 0); |
hermanindeput | 35:38a5af0afee8 | 110 | screen.get_screen_handle()->printf("Tense muscle ",EMG_max_relax); |
hermanindeput | 35:38a5af0afee8 | 111 | screen.get_screen_handle()->printf(" ",EMG_min_tense); |
hermanindeput | 35:38a5af0afee8 | 112 | screen.get_screen_handle()->printf(" ",EMG_min_tense); |
hermanindeput | 35:38a5af0afee8 | 113 | screen.get_screen_handle()->printf(" ",EMG_min_tense); |
hermanindeput | 35:38a5af0afee8 | 114 | screen.display(); |
hermanindeput | 36:691ea4660f29 | 115 | adjustment_time.attach(callback(this, &EMG_calibration::start_tense),2.5f); |
hermanindeput | 35:38a5af0afee8 | 116 | } |
hermanindeput | 36:691ea4660f29 | 117 | void start_tense(){ |
hermanindeput | 36:691ea4660f29 | 118 | calibration_timer.attach(&tense_calibration,0.1); |
hermanindeput | 36:691ea4660f29 | 119 | calibration_bicep.attach(&TheFinalCalibration,5); |
hermanindeput | 36:691ea4660f29 | 120 | } |
hermanindeput | 36:691ea4660f29 | 121 | }; |