Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

EMG_calibration.h@37:9a5da4463982, 2018-11-01 (annotated)

Committer:
hermanindeput
Date:
Thu Nov 01 14:19:33 2018 +0000
Revision:
37:9a5da4463982
Child:
39:f119ca6fc821
EMG_calibration;

Who changed what in which revision?

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