Biorobotics Project / Mbed 2 deprecated motor_emg_xyz

state switch test, works but switches 2 times

Dependencies:   EMGFilter HIDScope MODSERIAL biquadFilter mbed

main.cpp@1:90041f90ecf2, 2017-10-27 (annotated)

Committer:
vera1
Date:
Fri Oct 27 08:13:15 2017 +0000
Revision:
1:90041f90ecf2
Parent:
0:f49bda586fc5 
trial didn't work out

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 1:90041f90ecf2 14
vera1 0:f49bda586fc5 15 //Define EMG inputs
vera1 0:f49bda586fc5 16 EMG_filter emg1(A0);
vera1 0:f49bda586fc5 17 EMG_filter emg2(A1);
vera1 0:f49bda586fc5 18 DigitalOut ledred(LED_RED);
vera1 0:f49bda586fc5 19 DigitalOut ledgreen(LED_GREEN);
vera1 0:f49bda586fc5 20 DigitalOut ledblue(LED_BLUE);
vera1 0:f49bda586fc5 21 DigitalOut ledstateswitch(D8);
vera1 0:f49bda586fc5 22 DigitalOut ledstatedef(D11);
vera1 0:f49bda586fc5 23
vera1 0:f49bda586fc5 24
vera1 0:f49bda586fc5 25 //EMG_filter emg3(A2);
vera1 0:f49bda586fc5 26
vera1 0:f49bda586fc5 27 //Define ticker1
vera1 0:f49bda586fc5 28 Ticker mainticker; //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_EMG
vera1 1:90041f90ecf2 29 Timeout maintimeout; // Timeout that will determine the duration of the calibration.
vera1 1:90041f90ecf2 30 Timeout testtimeout;
vera1 0:f49bda586fc5 31
vera1 0:f49bda586fc5 32 //Define ticker variables
vera1 0:f49bda586fc5 33 bool go_EMG; // Boolean to put on/off the EMG readout
vera1 0:f49bda586fc5 34 bool go_calibration; // Boolean to put on/off the calibration of the EMG
vera1 0:f49bda586fc5 35 bool go_switch;
vera1 0:f49bda586fc5 36
vera1 0:f49bda586fc5 37 //Function that reads out the raw EMG and filters the signal
vera1 0:f49bda586fc5 38
vera1 0:f49bda586fc5 39
vera1 0:f49bda586fc5 40 void calibrationGO() // Function for go or no go of calibration
vera1 0:f49bda586fc5 41 {
vera1 0:f49bda586fc5 42 go_calibration = false;
vera1 0:f49bda586fc5 43 }
vera1 0:f49bda586fc5 44
vera1 0:f49bda586fc5 45 /*
vera1 0:f49bda586fc5 46 Calibration of the robot works as follows:
vera1 0:f49bda586fc5 47 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 48 The value of the MVC is overwritten when the value of the filtered signal is bigger than the current value.
vera1 0:f49bda586fc5 49 During the calibration the user should exert maximum force.
vera1 0:f49bda586fc5 50 */
vera1 0:f49bda586fc5 51
vera1 0:f49bda586fc5 52 void calibrationEMG() // Function for calibrating the EMG signal
vera1 0:f49bda586fc5 53 {
vera1 0:f49bda586fc5 54 if (go_calibration) {
vera1 0:f49bda586fc5 55 emg1.calibration(); // Using the calibration function of the EMG_filter class
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 1:90041f90ecf2 75 /*
vera1 1:90041f90ecf2 76 void r_processStateSwitchGO() // function will be runned after 3 seconds.
vera1 1:90041f90ecf2 77 {
vera1 1:90041f90ecf2 78
vera1 1:90041f90ecf2 79 go_switch = false; // then it will stop again
vera1 1:90041f90ecf2 80 ledstatedef = 1; // now you are definitely in a state and motor controll will begin again.
vera1 1:90041f90ecf2 81 ledstateswitch = 0;
vera1 1:90041f90ecf2 82 }
vera1 1:90041f90ecf2 83 */
vera1 0:f49bda586fc5 84 void r_processStateSwitch()
vera1 0:f49bda586fc5 85 {
vera1 1:90041f90ecf2 86
vera1 1:90041f90ecf2 87 if(go_switch == true) {
vera1 0:f49bda586fc5 88 switch(state) {
vera1 0:f49bda586fc5 89 case R_HORIZONTAL:
vera1 0:f49bda586fc5 90 state = R_VERTICAL;
vera1 0:f49bda586fc5 91 ledblue = 1;
vera1 0:f49bda586fc5 92 ledred = 1;
vera1 0:f49bda586fc5 93 ledgreen = 0;
vera1 1:90041f90ecf2 94 //pc.printf("state is vertical\r\n");
vera1 0:f49bda586fc5 95 break;
vera1 0:f49bda586fc5 96 case R_VERTICAL:
vera1 0:f49bda586fc5 97 state = R_BASE;
vera1 0:f49bda586fc5 98 ledgreen = 1;
vera1 0:f49bda586fc5 99 ledblue = 1;
vera1 0:f49bda586fc5 100 ledred = 0;
vera1 1:90041f90ecf2 101 //pc.printf("state is base\r\n");
vera1 0:f49bda586fc5 102 break;
vera1 0:f49bda586fc5 103 case R_BASE:
vera1 0:f49bda586fc5 104 state = R_HORIZONTAL;
vera1 0:f49bda586fc5 105 ledgreen = 1;
vera1 0:f49bda586fc5 106 ledred = 1;
vera1 0:f49bda586fc5 107 ledblue = 0;
vera1 1:90041f90ecf2 108 //pc.printf("state is horizontal\r\n");
vera1 0:f49bda586fc5 109 break;
vera1 1:90041f90ecf2 110 }
vera1 1:90041f90ecf2 111 go_switch = false; // otherwise it will see a lot of values over 0.7 so it will switch states 100 times per peak.
vera1 1:90041f90ecf2 112 ledstatedef = 1; // now you are definitely in a state and motor controll will begin again.
vera1 1:90041f90ecf2 113 ledstateswitch = 0;
vera1 1:90041f90ecf2 114 }
vera1 0:f49bda586fc5 115 }
vera1 0:f49bda586fc5 116
vera1 0:f49bda586fc5 117 void processEMG ()
vera1 0:f49bda586fc5 118 {
vera1 1:90041f90ecf2 119
vera1 0:f49bda586fc5 120 if (go_EMG) {
vera1 1:90041f90ecf2 121
vera1 0:f49bda586fc5 122 //go_EMG = false; //set the variable to false --> misschien nodig?
vera1 0:f49bda586fc5 123 emg1.filter(); //filter the incoming emg signal
vera1 0:f49bda586fc5 124 emg2.filter();
vera1 0:f49bda586fc5 125 //emg3.filter();
vera1 0:f49bda586fc5 126 /*
vera1 0:f49bda586fc5 127 scope.set(0, emg1.normalized); //views emg1.normalized on port 0 of HIDScope
vera1 0:f49bda586fc5 128 scope.set(1, emg1.emg0);
vera1 0:f49bda586fc5 129 scope.send();*/
vera1 0:f49bda586fc5 130 }
vera1 0:f49bda586fc5 131
vera1 0:f49bda586fc5 132 }
vera1 0:f49bda586fc5 133
vera1 0:f49bda586fc5 134
vera1 0:f49bda586fc5 135
vera1 0:f49bda586fc5 136 int main()
vera1 0:f49bda586fc5 137 {
vera1 1:90041f90ecf2 138 pc.baud(9600); // Set baudrate for proper communication
vera1 0:f49bda586fc5 139 go_EMG = true; // Setting ticker variables
vera1 0:f49bda586fc5 140 go_calibration = true; // Setting the timeout variable
vera1 1:90041f90ecf2 141 maintimeout.attach(&calibrationGO, 5.0); // Attach calibration timeout to calibration function
vera1 1:90041f90ecf2 142 mainticker.attach(&calibrationEMG, 0.001f); // Attach ticker to the calibration function
vera1 1:90041f90ecf2 143 wait(5.0); // Wait for 5.0 seconds to make time for calibration
vera1 1:90041f90ecf2 144 mainticker.attach(&processEMG, 0.001f); // Attaching mainticker to function processEMG, calling the function 1000 times a second
vera1 0:f49bda586fc5 145
vera1 0:f49bda586fc5 146 while (true) {
vera1 0:f49bda586fc5 147 ledstatedef = 1;
vera1 0:f49bda586fc5 148 if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
vera1 0:f49bda586fc5 149 ledstateswitch = 1;
vera1 0:f49bda586fc5 150 ledstatedef = 0;
vera1 0:f49bda586fc5 151 go_switch = true;
vera1 1:90041f90ecf2 152 r_processStateSwitch();
vera1 1:90041f90ecf2 153 wait(2.0f);
vera1 1:90041f90ecf2 154
vera1 1:90041f90ecf2 155 //r_processStateSwitchGO();
vera1 0:f49bda586fc5 156 }
vera1 1:90041f90ecf2 157 pc.printf("go_Switch = %d, emg1 = %f, emg2 = %f \r\n", go_switch, emg1.normalized, emg2.normalized);
vera1 0:f49bda586fc5 158
vera1 1:90041f90ecf2 159 state = R_VERTICAL;
vera1 0:f49bda586fc5 160 switch(state) {
vera1 0:f49bda586fc5 161 case R_HORIZONTAL:
vera1 0:f49bda586fc5 162 r_movehorizontal();
vera1 0:f49bda586fc5 163 break;
vera1 0:f49bda586fc5 164 case R_VERTICAL:
vera1 0:f49bda586fc5 165 r_movevertical();
vera1 0:f49bda586fc5 166 break;
vera1 0:f49bda586fc5 167 case R_BASE:
vera1 0:f49bda586fc5 168 r_movebase();
vera1 0:f49bda586fc5 169 break;
vera1 1:90041f90ecf2 170 }
vera1 0:f49bda586fc5 171
vera1 0:f49bda586fc5 172
vera1 0:f49bda586fc5 173 }
vera1 0:f49bda586fc5 174 }
vera1 0:f49bda586fc5 175
vera1 0:f49bda586fc5 176
vera1 0:f49bda586fc5 177
vera1 0:f49bda586fc5 178
vera1 0:f49bda586fc5 179
vera1 0:f49bda586fc5 180