Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

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

Committer:
hermanindeput
Date:
Thu Nov 01 08:50:20 2018 +0000
Revision:
35:38a5af0afee8
Child:
36:691ea4660f29
EMG_calibration toevoegen;

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 35:38a5af0afee8 8 //Define objects
hermanindeput 35:38a5af0afee8 9 InterruptIn Button1(D2); // Button on interface
hermanindeput 35:38a5af0afee8 10 Ticker calibration_timer;
hermanindeput 35:38a5af0afee8 11 Timeout calibration_bicep; // Timeout to get to relax state
hermanindeput 35:38a5af0afee8 12 Timeout end_of_calibration; // Timeout to get to final state
hermanindeput 35:38a5af0afee8 13 Screen screen(D14, D15, D9);
hermanindeput 35:38a5af0afee8 14
hermanindeput 35:38a5af0afee8 15 //Variables
hermanindeput 35:38a5af0afee8 16 volatile float EMG_signal1;
hermanindeput 35:38a5af0afee8 17 volatile float EMG_min_relax=1.000;
hermanindeput 35:38a5af0afee8 18 volatile float EMG_min_tense=1.000;
hermanindeput 35:38a5af0afee8 19 volatile float EMG_max_relax=0.000;
hermanindeput 35:38a5af0afee8 20 volatile float EMG_max_tense=0.000;
hermanindeput 35:38a5af0afee8 21 volatile float EMG_threshold;
hermanindeput 35:38a5af0afee8 22 // IN STATE MACHINE:
hermanindeput 35:38a5af0afee8 23 EMGFilter emg_input = EMGFilter(A0); // Voor kalibratie van bicep 1
hermanindeput 35:38a5af0afee8 24 //EMGFilter emg_input = EMGFilter(A1); // Voor kalibratie van bicep 2
hermanindeput 35:38a5af0afee8 25
hermanindeput 35:38a5af0afee8 26 void tense_calibration()
hermanindeput 35:38a5af0afee8 27 {
hermanindeput 35:38a5af0afee8 28 EMG_signal1 = emg_input.get_envelope_emg();
hermanindeput 35:38a5af0afee8 29 if (EMG_max_tense < EMG_signal1)
hermanindeput 35:38a5af0afee8 30 {
hermanindeput 35:38a5af0afee8 31 EMG_max_tense=EMG_signal1;
hermanindeput 35:38a5af0afee8 32 }
hermanindeput 35:38a5af0afee8 33 if (EMG_min_tense > EMG_signal1)
hermanindeput 35:38a5af0afee8 34 {
hermanindeput 35:38a5af0afee8 35 EMG_min_tense=EMG_signal1;
hermanindeput 35:38a5af0afee8 36 }
hermanindeput 35:38a5af0afee8 37 screen.get_screen_handle()->setTextCursor(0, 0);
hermanindeput 35:38a5af0afee8 38 screen.get_screen_handle()->printf("Tense ");
hermanindeput 35:38a5af0afee8 39 screen.get_screen_handle()->printf("EMG maximum= %.6f",EMG_max_tense);
hermanindeput 35:38a5af0afee8 40 screen.get_screen_handle()->printf("EMG minimum= %.6f",EMG_min_tense);
hermanindeput 35:38a5af0afee8 41 screen.get_screen_handle()->printf(" ",EMG_min_tense);
hermanindeput 35:38a5af0afee8 42 screen.display();
hermanindeput 35:38a5af0afee8 43 }
hermanindeput 35:38a5af0afee8 44
hermanindeput 35:38a5af0afee8 45 void relax_calibration()
hermanindeput 35:38a5af0afee8 46 {
hermanindeput 35:38a5af0afee8 47 EMG_signal1 = emg_input.get_envelope_emg();
hermanindeput 35:38a5af0afee8 48 if (EMG_max_relax < EMG_signal1)
hermanindeput 35:38a5af0afee8 49 {
hermanindeput 35:38a5af0afee8 50 EMG_max_relax=EMG_signal1;
hermanindeput 35:38a5af0afee8 51 }
hermanindeput 35:38a5af0afee8 52 if (EMG_min_relax > EMG_signal1)
hermanindeput 35:38a5af0afee8 53 {
hermanindeput 35:38a5af0afee8 54 EMG_min_relax=EMG_signal1;
hermanindeput 35:38a5af0afee8 55 }
hermanindeput 35:38a5af0afee8 56 EMG_threshold=(((EMG_min_tense-EMG_max_relax)/2)+EMG_max_relax);
hermanindeput 35:38a5af0afee8 57 screen.get_screen_handle()->setTextCursor(0, 0);
hermanindeput 35:38a5af0afee8 58 screen.get_screen_handle()->printf("Relax ");
hermanindeput 35:38a5af0afee8 59 screen.get_screen_handle()->printf("EMG maximum= %.6f",EMG_max_relax);
hermanindeput 35:38a5af0afee8 60 screen.get_screen_handle()->printf("EMG minimum= %.6f",EMG_min_relax);
hermanindeput 35:38a5af0afee8 61 screen.get_screen_handle()->printf("threshold= %.6f",EMG_threshold);
hermanindeput 35:38a5af0afee8 62 screen.display();
hermanindeput 35:38a5af0afee8 63 }
hermanindeput 35:38a5af0afee8 64
hermanindeput 35:38a5af0afee8 65 void TheFinalCalibration()
hermanindeput 35:38a5af0afee8 66 {
hermanindeput 35:38a5af0afee8 67 calibration_bicep.detach();
hermanindeput 35:38a5af0afee8 68 calibration_timer.detach();
hermanindeput 35:38a5af0afee8 69 screen.get_screen_handle()->setTextCursor(0, 0);
hermanindeput 35:38a5af0afee8 70 screen.get_screen_handle()->printf("threshold= %.6f",EMG_threshold);
hermanindeput 35:38a5af0afee8 71 screen.get_screen_handle()->printf(" ");
hermanindeput 35:38a5af0afee8 72 screen.get_screen_handle()->printf(" ");
hermanindeput 35:38a5af0afee8 73 screen.get_screen_handle()->printf(" ");
hermanindeput 35:38a5af0afee8 74 screen.display();
hermanindeput 35:38a5af0afee8 75 emg_input.set_threshold(EMG_threshold);
hermanindeput 35:38a5af0afee8 76 }
hermanindeput 35:38a5af0afee8 77
hermanindeput 35:38a5af0afee8 78 void relax_measure()
hermanindeput 35:38a5af0afee8 79 {
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("Relax muscle ",EMG_max_relax);
hermanindeput 35:38a5af0afee8 83 screen.get_screen_handle()->printf(" ",EMG_min_tense);
hermanindeput 35:38a5af0afee8 84 screen.get_screen_handle()->printf(" ",EMG_min_tense);
hermanindeput 35:38a5af0afee8 85 screen.get_screen_handle()->printf(" ",EMG_min_tense);
hermanindeput 35:38a5af0afee8 86 screen.display();
hermanindeput 35:38a5af0afee8 87 wait(2.5f);
hermanindeput 35:38a5af0afee8 88 calibration_timer.attach(&relax_calibration, 0.1);
hermanindeput 35:38a5af0afee8 89 end_of_calibration.attach(TheFinalCalibration,5.0f);
hermanindeput 35:38a5af0afee8 90 }
hermanindeput 35:38a5af0afee8 91
hermanindeput 35:38a5af0afee8 92 void tense_measure()
hermanindeput 35:38a5af0afee8 93 {
hermanindeput 35:38a5af0afee8 94 calibration_timer.detach();
hermanindeput 35:38a5af0afee8 95 screen.get_screen_handle()->setTextCursor(0, 0);
hermanindeput 35:38a5af0afee8 96 screen.get_screen_handle()->printf("Tense muscle ",EMG_max_relax);
hermanindeput 35:38a5af0afee8 97 screen.get_screen_handle()->printf(" ",EMG_min_tense);
hermanindeput 35:38a5af0afee8 98 screen.get_screen_handle()->printf(" ",EMG_min_tense);
hermanindeput 35:38a5af0afee8 99 screen.get_screen_handle()->printf(" ",EMG_min_tense);
hermanindeput 35:38a5af0afee8 100 screen.display();
hermanindeput 35:38a5af0afee8 101 wait(2.5f);
hermanindeput 35:38a5af0afee8 102 calibration_timer.attach(&tense_calibration, 0.1);
hermanindeput 35:38a5af0afee8 103 calibration_bicep.attach(&relax_measure,5);
hermanindeput 35:38a5af0afee8 104 }
hermanindeput 35:38a5af0afee8 105
hermanindeput 35:38a5af0afee8 106 int main()
hermanindeput 35:38a5af0afee8 107 {
hermanindeput 35:38a5af0afee8 108 emg_input.start(0.005);
hermanindeput 35:38a5af0afee8 109 while(1)
hermanindeput 35:38a5af0afee8 110 {
hermanindeput 35:38a5af0afee8 111 Button1.fall(&tense_measure);
hermanindeput 35:38a5af0afee8 112 }
hermanindeput 35:38a5af0afee8 113 // return 0;
hermanindeput 35:38a5af0afee8 114 }