Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

EMG_calibration.h@38:fd89e1301734, 2018-11-01 (annotated)

Committer:
brass_phoenix
Date:
Thu Nov 01 15:09:10 2018 +0000
Revision:
38:fd89e1301734
Child:
39:f119ca6fc821
+ Broken emg calibration.

Who changed what in which revision?

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