Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

EMG_calibration.h@51:e0e4d7e3de93, 2018-11-05 (annotated)

Committer:
MAHCSnijders
Date:
Mon Nov 05 16:03:39 2018 +0000
Revision:
51:e0e4d7e3de93
Parent:
46:0be634ee10e8 
Fixed comments

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