Biorobotics 7 / Mbed 2 deprecated State_Machine

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?

UserRevisionLine numberNew 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 };