main.cpp

00001 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
00002 // Libraries
00003 #include "mbed.h"
00004 #include "BiQuad.h"
00005 #include "FastPWM.h"
00006 #include "HIDScope.h"
00007 #include "MODSERIAL.h"
00008 #include <algorithm>
00009 
00010 // LEDs
00011 DigitalOut ledRed(LED_RED,1);       // red LED K64F
00012 DigitalOut ledGreen(LED_GREEN,1);   // green LED K64F
00013 DigitalOut ledBlue(LED_BLUE,1);     // blue LED K64F
00014 // DigitalOut ledBio1(,1);          // led 1 Biorobotics shield                 // LED pins
00015 // DigitalOut ledBio2(,1);          // led 2 Biorobotics shield                 
00016 
00017 Ticker blinkTimer;                  // LED ticker
00018 
00019 // Buttons/inputs
00020 InterruptIn buttonBio1(D0);         // button 1 BioRobotics shield
00021 InterruptIn buttonBio2(D1);         // button 2 BioRobotics shield
00022 InterruptIn buttonK64F(SW3);        // button on K64F
00023 InterruptIn buttonEmergency(SW2);   // emergency button on K64F
00024 
00025 // Motor pins
00026 
00027 
00028 // PC communication
00029 MODSERIAL pc(USBTX, USBRX);         // communication with pc
00030 
00031 // Define & initialise state machine
00032 enum states {   calibratingMotors, calibratingEMG,
00033                 homing, operating, reading, failing, demoing
00034             };
00035 states currentState = calibratingMotors;// start in waiting mode
00036 bool changeState = true;                // initialise the first state
00037 
00038 // Parameters for the EMG
00039 
00040 //EMG inputs
00041 AnalogIn EMG0In(A0);                                            //EMG input 0
00042 AnalogIn EMG1In(A1);                                            //EMG input 1
00043 
00044 //Timers
00045 Ticker ReadUseEMG0_timer;                                       //Timer to read, filter and use the EMG
00046 Ticker EMGCalibration0_timer;                                   //Timer for the calibration of the EMG   
00047 Ticker FindMax0_timer;                                          //Timer for finding the max muscle
00048 Ticker ReadUseEMG1_timer;                                       //Timer to read, filter and use the EMG
00049 Ticker EMGCalibration1_timer;                                   //Timer for the calibration of the EMG   
00050 Ticker FindMax1_timer;                                          //Timer for finding the max muscle
00051 Ticker SwitchState_timer;                                       //Timer to switch from the Calibration to the working mode
00052 
00053 //Constants
00054 const int Length = 10000;                                       //Length of the array for the calibration
00055 const int Parts = 50;                                           //Mean average filter over 50 values
00056 
00057 //Parameters for the first EMG signal
00058 float EMG0;                                                     //float for EMG input
00059 float EMG0filt;                                                 //float for filtered EMG
00060 float EMG0filtArray[Parts];                                     //Array for the filtered array
00061 float EMG0Average;                                              //float for the value after Moving Average Filter
00062 float Sum0 = 0;                                                 //Sum0 for the moving average filter
00063 float EMG0Calibrate[Length];                                    //Array for the calibration 
00064 int ReadCal0 = 0;                                               //Integer to read over the calibration array
00065 float MaxValue0 = 0;                                            //float to save the max muscle
00066 float Threshold0 = 0;                                           //Threshold for the first EMG signal
00067 
00068 //Parameters for the second EMG signal
00069 float EMG1;                                                     //float for EMG input
00070 float EMG1filt;                                                 //float for filtered EMG
00071 float EMG1filtArray[Parts];                                     //Array for the filtered array
00072 float EMG1Average;                                              //float for the value after Moving Average Filter
00073 float Sum1 = 0;                                                 //Sum0 for the moving average filter
00074 float EMG1Calibrate[Length];                                    //Array for the calibration 
00075 int ReadCal1 = 0;                                               //Integer to read over the calibration array
00076 float MaxValue1 = 0;                                            //float to save the max muscle
00077 float Threshold1 = 0;                                           //Threshold for the second EMG signal
00078 
00079 //Filter variables
00080 BiQuad Notch50_0(0.7887,0,0.7887,0,0.5774);                     //Make Notch filter around 50 Hz
00081 BiQuad Notch50_1(0.7887,0,0.7887,0,0.5774);                     //Make Notch filter around 50 Hz
00082 BiQuad High0(0.8006,-1.6012,0.8006,-1.561,0.6414);              //Make high-pass filter
00083 BiQuad High1(0.8006,-1.6012,0.8006,-1.561,0.6414);              //Make high-pass filter
00084 BiQuadChain filter0;                                            //Make chain of filters for the first EMG signal
00085 BiQuadChain filter1;                                            //Make chain of filters for the second EMG signal
00086 
00087 //Bool for movement
00088 bool xMove = false;                                             //Bool for the x-movement
00089 bool yMove = false;                                             //Bool for the y-movement
00090 
00091 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ FUNCTIONS ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
00092 // ============================ GENERAL FUNCTIONS =============================
00093 void stopProgram(void)
00094 {
00095     // Error message
00096     pc.printf("[ERROR] emergency button pressed\r\n");
00097     currentState = failing;             // change to state
00098     changeState = true;                 // next loop, switch states
00099 }
00100 
00101 void blinkLED(DigitalOut led)                                                   // blinkTimer.attach(&blinkLED,0.5) aanroepen bij initialisation, bij verlaten state:
00102 {                                                                               // blinkTimer.detach
00103     led =! led;                         // toggle LED
00104 }
00105 
00106 //  ============================= EMG FUNCTIONS ===============================
00107 
00108 //Function to read and filter the EMG
00109 void ReadUseEMG0(){
00110     for(int i = Parts ; i > 0 ; i--){                           //Make a first in, first out array
00111         EMG0filtArray[i] = EMG0filtArray[i-1];                  //Every value moves one up
00112     }
00113     
00114     Sum0 = 0;
00115     EMG0 = EMG0In;                                              //Save EMG input in float
00116     EMG0filt = filter0.step(EMG0);                              //Filter the signal
00117     EMG0filt = abs(EMG0filt);                                   //Take the absolute value
00118     EMG0filtArray[0] = EMG0filt;                                //Save the filtered signal on the first place in the array
00119                                              
00120     for(int i = 0 ; i < Parts ; i++){                           //Moving Average filter
00121         Sum0 += EMG0filtArray[i];                               //Sum the new value and the previous 49
00122     }
00123     EMG0Average = (float)Sum0/Parts;                            //Divide the sum by 50
00124     
00125     if (EMG0Average > Threshold0){                              //If the value is higher than the threshold value
00126         xMove = true;                                           //Set movement to true
00127     }
00128     else{
00129         xMove = false;                                          //Otherwise set movement to false
00130     }                                 
00131 }
00132 
00133 //Function to read and filter the EMG
00134 void ReadUseEMG1(){
00135     for(int i = Parts ; i > 0 ; i--){                           //Make a first in, first out array
00136         EMG1filtArray[i] = EMG1filtArray[i-1];                  //Every value moves one up
00137     }
00138     
00139     Sum1 = 0;
00140     EMG1 = EMG1In;                                              //Save EMG input in float
00141     EMG1filt = filter1.step(EMG1);                              //Filter the signal
00142     EMG1filt = abs(EMG1filt);                                   //Take the absolute value
00143     EMG1filtArray[0] = EMG1filt;                                //Save the filtered signal on the first place in the array
00144                                              
00145     for(int i = 0 ; i < Parts ; i++){                           //Moving Average filter
00146         Sum1 += EMG1filtArray[i];                               //Sum the new value and the previous 49
00147     }
00148     EMG1Average = (float)Sum1/Parts;                            //Divide the sum by 50
00149     
00150     if (EMG1Average > Threshold1){                              //If the value is higher than the threshold value
00151         yMove = true;                                           //Set y movement to true
00152     }
00153     else{
00154         yMove = false;                                           //Otherwise set y movement to false
00155     }
00156 }
00157 
00158 //Function to make an array during the calibration
00159 void CalibrateEMG0(){
00160     for(int i = Parts ; i > 0 ; i--){                           //Make a first in, first out array
00161         EMG0filtArray[i] = EMG0filtArray[i-1];                  //Every value moves one up
00162     }
00163     
00164     Sum0 = 0;
00165     EMG0 = EMG0In;                                              //Save EMG input in float
00166     EMG0filt = filter0.step(EMG0);                              //Filter the signal
00167     EMG0filt = abs(EMG0filt);                                   //Take the absolute value
00168     EMG0filtArray[0] = EMG0filt;                                //Save the filtered signal on the first place in the array
00169                                              
00170     for(int i = 0 ; i < Parts ; i++){                           //Moving Average filter
00171         Sum0 += EMG0filtArray[i];                               //Sum the new value and the previous 49
00172     }
00173     EMG0Calibrate[ReadCal0] = (float)Sum0/Parts;                //Divide the sum by 50
00174     
00175     ReadCal0++;
00176 }
00177 
00178 //Function to make an array during the calibration
00179 void CalibrateEMG1(){
00180     for(int i = Parts ; i > 0 ; i--){                           //Make a first in, first out array
00181         EMG1filtArray[i] = EMG1filtArray[i-1];                  //Every value moves one up
00182     }
00183     
00184     Sum1 = 0;
00185     EMG1 = EMG1In;                                              //Save EMG input in float
00186     EMG1filt = filter1.step(EMG1);                              //Filter the signal
00187     EMG1filt = abs(EMG1filt);                                   //Take the absolute value
00188     EMG1filtArray[0] = EMG1filt;                                //Save the filtered signal on the first place in the array
00189                                              
00190     for(int i = 0 ; i < Parts ; i++){                           //Moving Average filter
00191         Sum1 += EMG1filtArray[i];                               //Sum the new value and the previous 49
00192     }
00193     EMG1Calibrate[ReadCal1] = (float)Sum1/Parts;                //Divide the sum by 50
00194     
00195     ReadCal1++;
00196 }
00197 
00198 //Function to find the max value during the calibration
00199 void FindMax0(){
00200     MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length);    //Find max value, but discard the first 100 values
00201     Threshold0 = 0.30f*MaxValue0;                               //The threshold is a percentage of the max value
00202     pc.printf("The calibration value of the first EMG is %f.\n\r The threshold is %f. \n\r",MaxValue0,Threshold0);              //Print the max value and the threshold
00203     FindMax0_timer.detach();                                    //Detach the timer, so you only use this once
00204 }
00205 
00206 //Function to find the max value during the calibration
00207 void FindMax1(){
00208     MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length);    //Find max value, but discard the first 100 values
00209     Threshold1 = 0.30f*MaxValue1;                               //The threshold is a percentage of the max value
00210     pc.printf("The calibration value of the second EMG is %f.\n\r The threshold is %f. \n\r",MaxValue1,Threshold1); //Print the Max value and the threshold
00211     FindMax1_timer.detach();                                    //Detach the timer, so you only use this once
00212 }
00213 
00214 //  ========================= KINEMATICS FUNCTIONS ============================
00215 
00216 
00217 //  ============================ MOTOR FUNCTIONS ==============================
00218 
00219 
00220 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
00221 void stateMachine(void)                                                         
00222 {
00223     switch (currentState) {         // determine which state Odin is in         // Van een aantal if statements moeten de conditions nog bepaald worden, niet vergeten
00224 
00225 //  ========================= MOTOR CALIBRATION MODE ==========================
00226         case calibratingMotors:
00227 //      ------------------------- initialisation --------------------------
00228             if (changeState) {                  // when entering the state
00229                 pc.printf("[MODE] calibrating motors...r\n");
00230                 // print current state
00231                 changeState = false;            // stay in this state
00232 
00233                 // Actions when entering state
00234                 /* */
00235 
00236             }
00237 //      ----------------------------- action ------------------------------
00238             // Actions for each loop iteration
00239             /* */
00240 
00241 //      --------------------------- transition ----------------------------
00242             // Transition condition #1: after 3 sec in state && all motor       // Als vóór het einde van de 3 seconden alle motoren al snelheid 0 hebben, zitten ze zo dus door te draaien terwijl dat niet kan
00243             //      velocities zero, start calibrating EMG-x                    // dat lijkt me niet de bedoeling
00244             if (1) {                                                            // CONDITION
00245                 // Actions when leaving state
00246                 /* */
00247 
00248                 currentState = calibratingEMG;     // change to state
00249                 changeState = true;                 // next loop, switch states
00250             }
00251             break; // end case
00252 
00253 //  =========================== EMG CALIBRATION MODE ===========================
00254         case calibratingEMG:
00255 //      ------------------------- initialisation --------------------------
00256             if (changeState) {                  // when entering the state
00257                 pc.printf("[MODE] calibrating EMG 1 (x-direction)...\r\n");
00258                 // print current state
00259                 changeState = false;            // stay in this state
00260 
00261                 // Actions when entering state
00262                 /* */
00263 
00264             }
00265 //      ----------------------------- action ------------------------------
00266             // Actions for each loop iteration
00267             /* */
00268 
00269 //      --------------------------- transition ----------------------------
00270             // Transition condition #1: after 20 sec in state
00271             if (1) {                                                            // CONDITION
00272                 // Actions when leaving state
00273                 /* */
00274 
00275                 currentState = homing;              // change to state
00276                 changeState = true;                 // next loop, switch states
00277             }
00278             break; // end case
00279 
00280 //  ============================== HOMING MODE ================================
00281         case homing:
00282 //      ------------------------- initialisation --------------------------     // Evt. ook checken op snelheid als mensen zo dom zijn om tijdens homing op random knopjes te drukken
00283             if (changeState) {                  // when entering the state
00284                 pc.printf("[MODE] homing...\r\n");
00285                 // print current state
00286                 changeState = false;            // stay in this state
00287 
00288 
00289                 // Actions when entering state
00290                 /* */
00291 
00292             }
00293 //      ----------------------------- action ------------------------------
00294             // Actions for each loop iteration
00295             /* */
00296 
00297 //      --------------------------- transition ----------------------------     // Volgorde veranderen voor logica?
00298             // Transition condition #1: with button press, enter demo mode
00299             if (buttonBio1 == true) {                                           // Het is niet nodig om hier ook nog "&& currentState == homing" toe te voegen, want hij bereikt dit stuk code alleen in homing mode
00300                 // Actions when leaving state
00301                 /* */
00302 
00303                 currentState = demoing;             // change to state
00304                 changeState = true;                 // next loop, switch states
00305             }
00306             // Transition condition #2: with button press, enter operation mode
00307             if (buttonBio2 == true) {
00308                 // Actions when leaving state
00309                 /* */
00310 
00311                 currentState = operating;           // change to state
00312                 changeState = true;                 // next loop, switch states
00313             }
00314             break; // end case
00315 
00316 //  ============================= OPERATING MODE ==============================
00317         case operating:
00318 //      ------------------------- initialisation --------------------------
00319             if (changeState) {                  // when entering the state
00320                 pc.printf("[MODE] operating...\r\n");
00321                 // print current state
00322                 changeState = false;            // stay in this state
00323 
00324                 // Actions when entering state
00325                 /* */
00326 
00327             }
00328 //      ----------------------------- action ------------------------------
00329             // Actions for each loop iteration
00330             /* */
00331 
00332 //      --------------------------- transition ----------------------------
00333             // Transition condition #1: with button press, back to homing mode  // Is het nodig om vanuit operating mode naar homing mode terug te kunnen gaan? -> ja, voor als je een demo wilt starten
00334             if (buttonBio2 == true) {
00335                 // Actions when leaving state
00336                 /* */
00337 
00338                 currentState = homing;              // change to state
00339                 changeState = true;                 // next loop, switch states
00340             }
00341             // Transition condition #2: motor angle error < certain value,
00342             //      start reading                                                    
00343             if (1) {                                                            // CONDITION
00344                 // Actions when leaving state
00345                 /* */
00346 
00347                 currentState = homing;              // change to state
00348                 changeState = true;                 // next loop, switch states
00349             }
00350             break; // end case
00351 
00352 //  ============================== READING MODE =============================== // Beweegt deze modus zowel heen als weer en zo ja, hoe bepaalt hij wat moet gebeuren?
00353         case reading:
00354 //      ------------------------- initialisation --------------------------
00355             if (changeState) {                  // when entering the state
00356                 pc.printf("[MODE] reading...\r\n");
00357                 // print current state
00358                 changeState = false;            // stay in this state
00359 
00360                 // Actions when entering state
00361                 /* */
00362 
00363             }
00364 //      ----------------------------- action ------------------------------
00365             // Actions for each loop iteration
00366             /* */
00367 
00368 //      --------------------------- transition ----------------------------
00369             // Transition condition #1: with button press, back to homing mode  // Hier automatisch terug naar operating mode!
00370             if (1) {                                                            // En hij gaat nu terug naar homing mode, maar dan moet je dus elke keer weer kiezen voor demo of operation mode?
00371                 // Actions when leaving state                                   // CONDITION
00372                 /* */
00373 
00374                 currentState = homing;              // change to state
00375                 changeState = true;                 // next loop, switch states
00376             }
00377             break; // end case
00378 
00379 //  ============================== DEMOING MODE ===============================
00380         case demoing:
00381 //      ------------------------- initialisation --------------------------
00382             if (changeState) {                  // when entering the state
00383                 pc.printf("[MODE] demoing...\r\n");
00384                 // print current state
00385                 changeState = false;            // stay in this state
00386 
00387                 // Actions when entering state
00388                 /* */
00389 
00390             }
00391 //      ----------------------------- action ------------------------------
00392             // Actions for each loop iteration
00393             /* */
00394 
00395 //      --------------------------- transition ----------------------------
00396             // Transition condition #1: with button press, back to homing mode
00397             if (1) {
00398                 // Actions when leaving state
00399                 /* */
00400 
00401                 currentState = homing;              // change to state
00402                 changeState = true;                 // next loop, switch states
00403             }
00404             // Transition condition #2: after 3 sec relaxation, start reading   // In 3 seconden zijn de bladzijden uit zichzelf alweer helemaal teruggegaan. Misschien dit gewoon doen na het voorgedefinieerde pad
00405             if (1) {
00406                 // Actions when leaving state
00407                 /* */
00408 
00409                 currentState = reading;             // change to state
00410                 changeState = true;                 // next loop, switch states
00411             }
00412             break; // end case
00413 
00414 // =============================== FAILING MODE ================================
00415         case failing:
00416 //      ------------------------- initialisation --------------------------
00417             if (changeState) {                  // when entering the state
00418                 pc.printf("[ERROR] entering failure mode\r\n");
00419                                                 // print current state
00420                 changeState = false;            // stay in this state
00421 
00422                 // Actions when entering state
00423                 ledGreen = 1;       // red LED on                               // Blijft dit aan?
00424                 ledBlue = 1;
00425                 ledRed = 0;
00426 
00427 //                pin3 = 0;           // all motor forces to zero               // Pins nog niet gedefiniëerd
00428 //                pin5 = 0;
00429 //                pin6 = 0;
00430                 exit (0);           // abort mission
00431             }
00432             break; // end case
00433 
00434 // ============================== DEFAULT MODE =================================
00435         default:
00436 // ---------------------------- enter failing mode -----------------------------
00437             currentState = failing;                 // change to state
00438             changeState = true;                     // next loop, switch states
00439             // print current state
00440             pc.printf("[ERROR] unknown or unimplemented state reached\r\n");
00441 
00442     }   // end switch
00443 }       // end stateMachine
00444 
00445 
00446 
00447 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
00448 
00449 int main()
00450 {
00451 //  ================================ EMERGENCY ================================
00452     //If the emergency button is pressed, stop program via failing state        
00453     buttonEmergency.rise(stopProgram);                                          // Automatische triggers voor failure mode?
00454     
00455 //  ================================ EMERGENCY ================================ 
00456     pc.baud(115200); // communication with terminal                             // Baud rate
00457 
00458 // ==================================== LOOP ===================================
00459     while (true) {                  // loop forever
00460 
00461     }
00462 }