main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "HIDScope.h"
00004 #include "MODSERIAL.h"
00005 #include "BiQuad.h"
00006 #include "EMG_filter.h"
00007 
00008 MODSERIAL pc(USBTX,USBRX);
00009 //HIDScope scope (2);
00010 
00011 enum r_states {R_HORIZONTAL, R_VERTICAL, R_BASE};
00012 r_states state;
00013 
00014 //Define EMG inputs
00015 EMG_filter emg1(A0);
00016 EMG_filter emg2(A1);
00017 DigitalOut ledred(LED_RED);
00018 DigitalOut ledgreen(LED_GREEN);
00019 DigitalOut ledblue(LED_BLUE);
00020 DigitalOut ledstateswitch(D8);
00021 DigitalOut ledstatedef(D11);
00022 
00023 
00024 //EMG_filter emg3(A2);
00025 
00026 //Define ticker1
00027 Ticker mainticker;      //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_EMG
00028 Timeout calibrationgo;      // Timeout that will determine the duration of the calibration.
00029 
00030 //Define ticker variables
00031 bool go_EMG;                    // Boolean to put on/off the EMG readout
00032 bool go_calibration;            // Boolean to put on/off the calibration of the EMG
00033 bool go_switch;
00034 
00035 //Function that reads out the raw EMG and filters the signal
00036 
00037 
00038 void calibrationGO()                   // Function for go or no go of calibration
00039 {
00040     go_calibration = false;
00041 }
00042 
00043 /*
00044 Calibration of the robot works as follows:
00045 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.
00046 The value of the MVC is overwritten when the value of the filtered signal is bigger than the current value.
00047 During the calibration the user should exert maximum force.
00048 */
00049 
00050 void calibrationEMG()                   // Function for calibrating the EMG signal
00051 {
00052     if (go_calibration) {
00053         emg1.calibration();                 // Using the calibration function of the EMG_filter class
00054         emg2.calibration();
00055         //emg3.calibration();
00056     }
00057 }
00058 
00059 
00060 void r_movehorizontal()
00061 {
00062 
00063 
00064 }
00065 
00066 void r_movevertical()
00067 {
00068 
00069 }
00070 
00071 void r_movebase()
00072 {
00073 
00074 }
00075 
00076 void r_processStateSwitch()
00077 {
00078    
00079     while(go_switch) {
00080         go_switch = false;
00081         switch(state) {
00082             case R_HORIZONTAL:
00083                 state = R_VERTICAL;
00084                 ledblue = 1;
00085                 ledred = 1;
00086                 ledgreen = 0;
00087                 pc.printf("state is vertical");
00088                 break;
00089             case R_VERTICAL:
00090                 state = R_BASE;
00091                 ledgreen = 1;
00092                 ledblue = 1;
00093                 ledred = 0;
00094                 pc.printf("state is base");
00095                 break;
00096             case R_BASE:
00097                 state = R_HORIZONTAL;
00098                 ledgreen = 1;
00099                 ledred = 1;
00100                 ledblue = 0;
00101                 pc.printf("state is horizontal");
00102                 break;
00103                 }
00104                 wait(1.0f);              
00105                 ledstatedef = 1;
00106                 ledstateswitch = 0;
00107         }   
00108 }
00109 
00110 void processEMG ()
00111 {
00112     r_processStateSwitch();
00113     if (go_EMG) {
00114         //go_EMG = false;                   //set the variable to false --> misschien nodig?
00115         emg1.filter();                      //filter the incoming emg signal
00116         emg2.filter();
00117         //emg3.filter();
00118         /*
00119         scope.set(0, emg1.normalized);      //views emg1.normalized on port 0 of HIDScope
00120         scope.set(1, emg1.emg0);
00121         scope.send();*/
00122     }
00123 
00124 }
00125 
00126 
00127 
00128 int main()
00129 {
00130     pc.baud(9600);                    // Set baudrate for proper communication
00131     go_EMG = true;                      // Setting ticker variables
00132     go_calibration = true;              // Setting the timeout variable
00133     calibrationgo.attach(&calibrationGO, 5.0);     // Attach calibration timeout to calibration function
00134     mainticker.attach(&calibrationEMG, 0.001f);      // Attach ticker to the calibration function
00135     wait(5.0);                                         // Wait for 5.0 seconds to make time for calibration
00136     mainticker.attach(&processEMG, 0.001f);         // Attaching mainticker to function processEMG, calling  the function 1000 times a second
00137 
00138     while (true) {
00139         ledstatedef = 1;
00140         if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
00141             ledstateswitch = 1;
00142             ledstatedef = 0;
00143             go_switch = true;
00144 
00145         }
00146         pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
00147 
00148         switch(state) {
00149             case R_HORIZONTAL:
00150                 r_movehorizontal();
00151                 break;
00152             case R_VERTICAL:
00153                 r_movevertical();
00154                 break;
00155             case R_BASE:
00156                 r_movebase();
00157                 break;
00158                        }
00159 
00160 
00161     }
00162 }
00163 
00164 
00165 
00166 
00167 
00168