Biorobotics Project / Mbed 2 deprecated motor_emg_xyz

state switch test, works but switches 2 times

Dependencies:   EMGFilter HIDScope MODSERIAL biquadFilter mbed

main.cpp@0:f49bda586fc5, 2017-10-26 (annotated)

Committer:
vera1
Date:
Thu Oct 26 10:09:33 2017 +0000
Revision:
0:f49bda586fc5
Child:
1:90041f90ecf2
Child:
2:810de22bca6a
switch works but somehow it switches 2 times when muscles are activated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vera1 0:f49bda586fc5 1 #include "mbed.h"
vera1 0:f49bda586fc5 2 #include "math.h"
vera1 0:f49bda586fc5 3 #include "HIDScope.h"
vera1 0:f49bda586fc5 4 #include "MODSERIAL.h"
vera1 0:f49bda586fc5 5 #include "BiQuad.h"
vera1 0:f49bda586fc5 6 #include "EMG_filter.h"
vera1 0:f49bda586fc5 7
vera1 0:f49bda586fc5 8 MODSERIAL pc(USBTX,USBRX);
vera1 0:f49bda586fc5 9 //HIDScope scope (2);
vera1 0:f49bda586fc5 10
vera1 0:f49bda586fc5 11 enum r_states {R_HORIZONTAL, R_VERTICAL, R_BASE};
vera1 0:f49bda586fc5 12 r_states state;
vera1 0:f49bda586fc5 13
vera1 0:f49bda586fc5 14 //Define EMG inputs
vera1 0:f49bda586fc5 15 EMG_filter emg1(A0);
vera1 0:f49bda586fc5 16 EMG_filter emg2(A1);
vera1 0:f49bda586fc5 17 DigitalOut ledred(LED_RED);
vera1 0:f49bda586fc5 18 DigitalOut ledgreen(LED_GREEN);
vera1 0:f49bda586fc5 19 DigitalOut ledblue(LED_BLUE);
vera1 0:f49bda586fc5 20 DigitalOut ledstateswitch(D8);
vera1 0:f49bda586fc5 21 DigitalOut ledstatedef(D11);
vera1 0:f49bda586fc5 22
vera1 0:f49bda586fc5 23
vera1 0:f49bda586fc5 24 //EMG_filter emg3(A2);
vera1 0:f49bda586fc5 25
vera1 0:f49bda586fc5 26 //Define ticker1
vera1 0:f49bda586fc5 27 Ticker mainticker; //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_EMG
vera1 0:f49bda586fc5 28 Timeout calibrationgo; // Timeout that will determine the duration of the calibration.
vera1 0:f49bda586fc5 29
vera1 0:f49bda586fc5 30 //Define ticker variables
vera1 0:f49bda586fc5 31 bool go_EMG; // Boolean to put on/off the EMG readout
vera1 0:f49bda586fc5 32 bool go_calibration; // Boolean to put on/off the calibration of the EMG
vera1 0:f49bda586fc5 33 bool go_switch;
vera1 0:f49bda586fc5 34
vera1 0:f49bda586fc5 35 //Function that reads out the raw EMG and filters the signal
vera1 0:f49bda586fc5 36
vera1 0:f49bda586fc5 37
vera1 0:f49bda586fc5 38 void calibrationGO() // Function for go or no go of calibration
vera1 0:f49bda586fc5 39 {
vera1 0:f49bda586fc5 40 go_calibration = false;
vera1 0:f49bda586fc5 41 }
vera1 0:f49bda586fc5 42
vera1 0:f49bda586fc5 43 /*
vera1 0:f49bda586fc5 44 Calibration of the robot works as follows:
vera1 0:f49bda586fc5 45 EMG signal is read out for 5 seconds, 1000 times a second. This signal is filtered and the maximum value is compared to the current value of the MVC.
vera1 0:f49bda586fc5 46 The value of the MVC is overwritten when the value of the filtered signal is bigger than the current value.
vera1 0:f49bda586fc5 47 During the calibration the user should exert maximum force.
vera1 0:f49bda586fc5 48 */
vera1 0:f49bda586fc5 49
vera1 0:f49bda586fc5 50 void calibrationEMG() // Function for calibrating the EMG signal
vera1 0:f49bda586fc5 51 {
vera1 0:f49bda586fc5 52 if (go_calibration) {
vera1 0:f49bda586fc5 53 emg1.calibration(); // Using the calibration function of the EMG_filter class
vera1 0:f49bda586fc5 54 }
vera1 0:f49bda586fc5 55 }
vera1 0:f49bda586fc5 56
vera1 0:f49bda586fc5 57
vera1 0:f49bda586fc5 58 void r_movehorizontal()
vera1 0:f49bda586fc5 59 {
vera1 0:f49bda586fc5 60
vera1 0:f49bda586fc5 61
vera1 0:f49bda586fc5 62 }
vera1 0:f49bda586fc5 63
vera1 0:f49bda586fc5 64 void r_movevertical()
vera1 0:f49bda586fc5 65 {
vera1 0:f49bda586fc5 66
vera1 0:f49bda586fc5 67 }
vera1 0:f49bda586fc5 68
vera1 0:f49bda586fc5 69 void r_movebase()
vera1 0:f49bda586fc5 70 {
vera1 0:f49bda586fc5 71
vera1 0:f49bda586fc5 72 }
vera1 0:f49bda586fc5 73
vera1 0:f49bda586fc5 74 void r_processStateSwitch()
vera1 0:f49bda586fc5 75 {
vera1 0:f49bda586fc5 76 while(go_switch) {
vera1 0:f49bda586fc5 77 switch(state) {
vera1 0:f49bda586fc5 78 case R_HORIZONTAL:
vera1 0:f49bda586fc5 79 state = R_VERTICAL;
vera1 0:f49bda586fc5 80 ledblue = 1;
vera1 0:f49bda586fc5 81 ledred = 1;
vera1 0:f49bda586fc5 82 ledgreen = 0;
vera1 0:f49bda586fc5 83 pc.printf("state is vertical");
vera1 0:f49bda586fc5 84 break;
vera1 0:f49bda586fc5 85 case R_VERTICAL:
vera1 0:f49bda586fc5 86 state = R_BASE;
vera1 0:f49bda586fc5 87 ledgreen = 1;
vera1 0:f49bda586fc5 88 ledblue = 1;
vera1 0:f49bda586fc5 89 ledred = 0;
vera1 0:f49bda586fc5 90 pc.printf("state is base");
vera1 0:f49bda586fc5 91 break;
vera1 0:f49bda586fc5 92 case R_BASE:
vera1 0:f49bda586fc5 93 state = R_HORIZONTAL;
vera1 0:f49bda586fc5 94 ledgreen = 1;
vera1 0:f49bda586fc5 95 ledred = 1;
vera1 0:f49bda586fc5 96 ledblue = 0;
vera1 0:f49bda586fc5 97 pc.printf("state is horizontal");
vera1 0:f49bda586fc5 98 break;
vera1 0:f49bda586fc5 99 }
vera1 0:f49bda586fc5 100 wait(3.0f);
vera1 0:f49bda586fc5 101 go_switch = false;
vera1 0:f49bda586fc5 102 ledstatedef = 1;
vera1 0:f49bda586fc5 103 ledstateswitch = 0;
vera1 0:f49bda586fc5 104 }
vera1 0:f49bda586fc5 105 }
vera1 0:f49bda586fc5 106
vera1 0:f49bda586fc5 107 void processEMG ()
vera1 0:f49bda586fc5 108 {
vera1 0:f49bda586fc5 109 r_processStateSwitch();
vera1 0:f49bda586fc5 110 if (go_EMG) {
vera1 0:f49bda586fc5 111 //go_EMG = false; //set the variable to false --> misschien nodig?
vera1 0:f49bda586fc5 112 emg1.filter(); //filter the incoming emg signal
vera1 0:f49bda586fc5 113 emg2.filter();
vera1 0:f49bda586fc5 114 //emg3.filter();
vera1 0:f49bda586fc5 115 /*
vera1 0:f49bda586fc5 116 scope.set(0, emg1.normalized); //views emg1.normalized on port 0 of HIDScope
vera1 0:f49bda586fc5 117 scope.set(1, emg1.emg0);
vera1 0:f49bda586fc5 118 scope.send();*/
vera1 0:f49bda586fc5 119 }
vera1 0:f49bda586fc5 120
vera1 0:f49bda586fc5 121 }
vera1 0:f49bda586fc5 122
vera1 0:f49bda586fc5 123
vera1 0:f49bda586fc5 124
vera1 0:f49bda586fc5 125 int main()
vera1 0:f49bda586fc5 126 {
vera1 0:f49bda586fc5 127 pc.baud(115200); // Set baudrate for proper communication
vera1 0:f49bda586fc5 128 go_EMG = true; // Setting ticker variables
vera1 0:f49bda586fc5 129 go_calibration = true; // Setting the timeout variable
vera1 0:f49bda586fc5 130 calibrationgo.attach(&calibrationGO, 5.0); // Attach calibration timeout to calibration function
vera1 0:f49bda586fc5 131 mainticker.attach(&calibrationEMG, 0.001f); // Attach ticker to the calibration function
vera1 0:f49bda586fc5 132 wait(5.0); // Wait for 5.0 seconds to make time for calibration
vera1 0:f49bda586fc5 133 mainticker.attach(&processEMG, 0.001f); // Attaching mainticker to function processEMG, calling the function 1000 times a second
vera1 0:f49bda586fc5 134
vera1 0:f49bda586fc5 135 while (true) {
vera1 0:f49bda586fc5 136 ledstatedef = 1;
vera1 0:f49bda586fc5 137 if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
vera1 0:f49bda586fc5 138 ledstateswitch = 1;
vera1 0:f49bda586fc5 139 ledstatedef = 0;
vera1 0:f49bda586fc5 140 go_switch = true;
vera1 0:f49bda586fc5 141
vera1 0:f49bda586fc5 142 }
vera1 0:f49bda586fc5 143
vera1 0:f49bda586fc5 144
vera1 0:f49bda586fc5 145 switch(state) {
vera1 0:f49bda586fc5 146 case R_HORIZONTAL:
vera1 0:f49bda586fc5 147 r_movehorizontal();
vera1 0:f49bda586fc5 148 break;
vera1 0:f49bda586fc5 149 case R_VERTICAL:
vera1 0:f49bda586fc5 150 r_movevertical();
vera1 0:f49bda586fc5 151 break;
vera1 0:f49bda586fc5 152 case R_BASE:
vera1 0:f49bda586fc5 153 r_movebase();
vera1 0:f49bda586fc5 154 break;
vera1 0:f49bda586fc5 155 }
vera1 0:f49bda586fc5 156
vera1 0:f49bda586fc5 157
vera1 0:f49bda586fc5 158 }
vera1 0:f49bda586fc5 159 }
vera1 0:f49bda586fc5 160
vera1 0:f49bda586fc5 161
vera1 0:f49bda586fc5 162
vera1 0:f49bda586fc5 163
vera1 0:f49bda586fc5 164
vera1 0:f49bda586fc5 165