Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

EMG_calibration.h@39:f119ca6fc821, 2018-11-01 (annotated)

Committer:
brass_phoenix
Date:
Thu Nov 01 15:41:34 2018 +0000
Revision:
39:f119ca6fc821
Parent:
38:fd89e1301734
Child:
46:0be634ee10e8
+ EMG possibly calibratable.

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