Biorobotics Project / Mbed 2 deprecated motor_emg_xyz

state switch test, works but switches 2 times

Dependencies:   EMGFilter HIDScope MODSERIAL biquadFilter mbed

main.cpp@2:810de22bca6a, 2017-10-27 (annotated)

Committer:
vera1
Date:
Fri Oct 27 09:10:21 2017 +0000
Revision:
2:810de22bca6a
Parent:
0:f49bda586fc5 
working version.

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 2:810de22bca6a 54 emg2.calibration();
vera1 2:810de22bca6a 55 //emg3.calibration();
vera1 0:f49bda586fc5 56 }
vera1 0:f49bda586fc5 57 }
vera1 0:f49bda586fc5 58
vera1 0:f49bda586fc5 59
vera1 0:f49bda586fc5 60 void r_movehorizontal()
vera1 0:f49bda586fc5 61 {
vera1 0:f49bda586fc5 62
vera1 0:f49bda586fc5 63
vera1 0:f49bda586fc5 64 }
vera1 0:f49bda586fc5 65
vera1 0:f49bda586fc5 66 void r_movevertical()
vera1 0:f49bda586fc5 67 {
vera1 0:f49bda586fc5 68
vera1 0:f49bda586fc5 69 }
vera1 0:f49bda586fc5 70
vera1 0:f49bda586fc5 71 void r_movebase()
vera1 0:f49bda586fc5 72 {
vera1 0:f49bda586fc5 73
vera1 0:f49bda586fc5 74 }
vera1 0:f49bda586fc5 75
vera1 0:f49bda586fc5 76 void r_processStateSwitch()
vera1 0:f49bda586fc5 77 {
vera1 2:810de22bca6a 78
vera1 0:f49bda586fc5 79 while(go_switch) {
vera1 2:810de22bca6a 80 go_switch = false;
vera1 0:f49bda586fc5 81 switch(state) {
vera1 0:f49bda586fc5 82 case R_HORIZONTAL:
vera1 0:f49bda586fc5 83 state = R_VERTICAL;
vera1 0:f49bda586fc5 84 ledblue = 1;
vera1 0:f49bda586fc5 85 ledred = 1;
vera1 0:f49bda586fc5 86 ledgreen = 0;
vera1 0:f49bda586fc5 87 pc.printf("state is vertical");
vera1 0:f49bda586fc5 88 break;
vera1 0:f49bda586fc5 89 case R_VERTICAL:
vera1 0:f49bda586fc5 90 state = R_BASE;
vera1 0:f49bda586fc5 91 ledgreen = 1;
vera1 0:f49bda586fc5 92 ledblue = 1;
vera1 0:f49bda586fc5 93 ledred = 0;
vera1 0:f49bda586fc5 94 pc.printf("state is base");
vera1 0:f49bda586fc5 95 break;
vera1 0:f49bda586fc5 96 case R_BASE:
vera1 0:f49bda586fc5 97 state = R_HORIZONTAL;
vera1 0:f49bda586fc5 98 ledgreen = 1;
vera1 0:f49bda586fc5 99 ledred = 1;
vera1 0:f49bda586fc5 100 ledblue = 0;
vera1 0:f49bda586fc5 101 pc.printf("state is horizontal");
vera1 0:f49bda586fc5 102 break;
vera1 0:f49bda586fc5 103 }
vera1 2:810de22bca6a 104 wait(1.0f);
vera1 0:f49bda586fc5 105 ledstatedef = 1;
vera1 0:f49bda586fc5 106 ledstateswitch = 0;
vera1 0:f49bda586fc5 107 }
vera1 0:f49bda586fc5 108 }
vera1 0:f49bda586fc5 109
vera1 0:f49bda586fc5 110 void processEMG ()
vera1 0:f49bda586fc5 111 {
vera1 0:f49bda586fc5 112 r_processStateSwitch();
vera1 0:f49bda586fc5 113 if (go_EMG) {
vera1 0:f49bda586fc5 114 //go_EMG = false; //set the variable to false --> misschien nodig?
vera1 0:f49bda586fc5 115 emg1.filter(); //filter the incoming emg signal
vera1 0:f49bda586fc5 116 emg2.filter();
vera1 0:f49bda586fc5 117 //emg3.filter();
vera1 0:f49bda586fc5 118 /*
vera1 0:f49bda586fc5 119 scope.set(0, emg1.normalized); //views emg1.normalized on port 0 of HIDScope
vera1 0:f49bda586fc5 120 scope.set(1, emg1.emg0);
vera1 0:f49bda586fc5 121 scope.send();*/
vera1 0:f49bda586fc5 122 }
vera1 0:f49bda586fc5 123
vera1 0:f49bda586fc5 124 }
vera1 0:f49bda586fc5 125
vera1 0:f49bda586fc5 126
vera1 0:f49bda586fc5 127
vera1 0:f49bda586fc5 128 int main()
vera1 0:f49bda586fc5 129 {
vera1 2:810de22bca6a 130 pc.baud(9600); // Set baudrate for proper communication
vera1 0:f49bda586fc5 131 go_EMG = true; // Setting ticker variables
vera1 0:f49bda586fc5 132 go_calibration = true; // Setting the timeout variable
vera1 0:f49bda586fc5 133 calibrationgo.attach(&calibrationGO, 5.0); // Attach calibration timeout to calibration function
vera1 0:f49bda586fc5 134 mainticker.attach(&calibrationEMG, 0.001f); // Attach ticker to the calibration function
vera1 0:f49bda586fc5 135 wait(5.0); // Wait for 5.0 seconds to make time for calibration
vera1 0:f49bda586fc5 136 mainticker.attach(&processEMG, 0.001f); // Attaching mainticker to function processEMG, calling the function 1000 times a second
vera1 0:f49bda586fc5 137
vera1 0:f49bda586fc5 138 while (true) {
vera1 0:f49bda586fc5 139 ledstatedef = 1;
vera1 0:f49bda586fc5 140 if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
vera1 0:f49bda586fc5 141 ledstateswitch = 1;
vera1 0:f49bda586fc5 142 ledstatedef = 0;
vera1 0:f49bda586fc5 143 go_switch = true;
vera1 0:f49bda586fc5 144
vera1 0:f49bda586fc5 145 }
vera1 2:810de22bca6a 146 pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
vera1 0:f49bda586fc5 147
vera1 0:f49bda586fc5 148 switch(state) {
vera1 0:f49bda586fc5 149 case R_HORIZONTAL:
vera1 0:f49bda586fc5 150 r_movehorizontal();
vera1 0:f49bda586fc5 151 break;
vera1 0:f49bda586fc5 152 case R_VERTICAL:
vera1 0:f49bda586fc5 153 r_movevertical();
vera1 0:f49bda586fc5 154 break;
vera1 0:f49bda586fc5 155 case R_BASE:
vera1 0:f49bda586fc5 156 r_movebase();
vera1 0:f49bda586fc5 157 break;
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
vera1 0:f49bda586fc5 166
vera1 0:f49bda586fc5 167
vera1 0:f49bda586fc5 168