main.cpp

00001 // EMG + KINEMATICS + PID + MOTOR CONTROL
00002 
00003 //----------------~INITIATING-------------------------
00004 #include "mbed.h"
00005 
00006 // EMG                 --      DEPENDENCIES
00007 #include <iostream>
00008 #include "BiQuad.h"
00009 #include "BiQuadchains_zelfbeun.h"
00010 #include "MODSERIAL.h"
00011 
00012 // KINEMATICS          --       DEPENDENCIES
00013 #include "stdio.h"
00014 #define _USE_MATH_DEFINES
00015 #include <math.h>
00016 #define M_PI    3.14159265358979323846 /* pi */
00017 
00018 // PID CONTROLLER      --      DEPENDENCIES
00019 #include "BiQuad.h"
00020 #include "QEI.h"
00021 //#include "HIDScope.h"
00022 
00023 
00024 // GENERAL PIN DEFENITIONS
00025 MODSERIAL pc(USBTX, USBRX);
00026 
00027 // EMG     --      PIN DEFENITIONS 
00028 DigitalOut gpo(D0);
00029 
00030 DigitalIn button2(SW3);  
00031 DigitalIn button1(SW2); //or SW2
00032 
00033 DigitalOut led1(LED_GREEN);
00034 DigitalOut led2(LED_RED);
00035 DigitalOut led3(LED_BLUE);
00036 
00037 //EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample
00038 Ticker sample_ticker; //ticker for filtering pref. with 1000Hz, define in tick.attach
00039 Ticker threshold_check_ticker;
00040 Timer t; //timer try out for Astrid
00041 Timer timer_calibration; //timer for EMG calibration
00042 
00043 //Input system
00044 AnalogIn emg1(A0); //right biceps
00045 AnalogIn emg2(A1); //right triceps
00046 AnalogIn emg3(A2); //left biceps
00047 AnalogIn emg4(A3); //left triceps
00048 
00049 
00050 // PID  CONTROLLER     --        PIN DEFENITIONS 
00051 AnalogIn button3(A4);
00052 AnalogIn button4(A5);
00053 
00054 DigitalOut directionpin1(D7);   // motor 1
00055 DigitalOut directionpin2(D4);   // motor 2
00056 DigitalOut directionpin3(D13);  // motor 3
00057 PwmOut pwmpin1(D6);             // motor 1
00058 PwmOut pwmpin2(D5);             // motor 2
00059 PwmOut pwmpin3(D12);            // motor 3
00060 
00061 QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING);
00062 QEI encoder2 (D11, D10, NC, 8400, QEI::X4_ENCODING); // motor 2
00063 QEI encoder3 (D3, D2, NC, 8400, QEI::X4_ENCODING);  // motor 3
00064 // HIDScope scope(2);
00065 
00066 //  PID - TICKERS
00067 Ticker ref_rot;
00068 Ticker show_counts;
00069 Ticker Scope_Data;
00070 
00071 //------------------------GLOBALS-----------------------------
00072 // GLOBALS EMG
00073 //Filtered EMG signals from the end of the chains
00074 volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
00075 int i = 0;
00076 
00077 //Define doubles for calibration and ticker
00078 double ts = 0.001; //tijdsstap
00079 double calibration_time = 55; //time EMG calibration should take
00080 
00081 volatile double temp_highest_emg1 = 0; //highest detected value right biceps
00082 volatile double temp_highest_emg2 = 0;
00083 volatile double temp_highest_emg3 = 0;
00084 volatile double temp_highest_emg4 = 0;
00085 
00086 //Doubles for calculation threshold
00087 double biceps_p_t = 0.4; //set threshold at percentage of highest value
00088 double triceps_p_t = 0.5; //set threshold at percentage of highest value
00089 volatile double threshold1;
00090 volatile double threshold2;
00091 volatile double threshold3;
00092 volatile double threshold4;
00093 
00094 // thresholdreads bools
00095 int bicepsR;
00096 int tricepsR;
00097 int bicepsL;
00098 int tricepsL;
00099 
00100 // VARIABLES ROBOT KINEMATICS
00101 // constants
00102 const float la = 0.256;         // lengte actieve arm
00103 const float lp = 0.21;          // lengte passieve arm
00104 const float rp = 0.052;         // straal van midden end effector tot hoekpunt
00105 const float rm = 0.23;          // straal van global midden tot motor
00106 const float a = 0.09;           // zijde van de driehoek
00107 const float xas = 0.40;         // afstand van motor 1 tot motor 3
00108 const float yas = 0.346;        // afstand van xas tot motor 2
00109 const float thetap = 0;         // rotatiehoek van de end effector
00110 
00111 
00112 // motor locatie
00113 const int a1x = 0;              //x locatie motor 1
00114 const int a1y = 0;              //y locatie motor 1
00115 const float a2x = (0.5)*xas;    // x locatie motor 2
00116 const float a2y = yas;          // y locatie motor 2
00117 const float a3x = xas;          // x locatie motor 3
00118 const int a3y = 0;              // y locatie motor 3
00119 
00120 // script voor het bepalen van de desired position aan de hand van emg (1/0)
00121 
00122 //  EMG OUTPUT
00123 int EMGxplus;
00124 int EMGxmin ;
00125 int EMGyplus;
00126 int EMGymin ;
00127 
00128 // Dit moet experimenteel geperfectioneerd worden
00129 float tijdstap = 0.005;      //nu wss heel langzaam, kan miss omhoog KEER V GEEFT VERANDERING IN POSITIE
00130 float v = 0.1;                // snelheid kan wss ook hoger
00131 
00132 float px = 0.2;     //starting x    // BOUNDARIES
00133 float py = 0.155;   // starting y   // BOUNDARIES
00134 
00135 // verschil horizontale as met de actieve arm
00136 float da1 = 1.619685; // verschil a1 hoek en motor
00137 float da2 = -0.609780;
00138 float da3 = 3.372859;
00139 
00140 // limits (since no forward kinematics)
00141 float upperxlim = 0.275; //36, 0.04, 0.315, -0.085niet helemaal naar requierments ff kijken of ie groter kan
00142 float lowerxlim = 0.10;
00143 float upperylim = 0.225;
00144 float lowerylim = 0.03; //0.03 is goed
00145 
00146 // VARIABLES PID CONTROLLER
00147 double PI = M_PI;// CHANGE THIS INTO M_PI
00148 double Kp = 14; //200 , 50
00149 double Ki = 0;   //1, 0.5
00150 double Kd = 3; //200, 10
00151 double Ts = 0.1; // Sample time in seconds
00152 double reference_rotation; //define as radians
00153 double motor_position;
00154 bool AlwaysTrue;
00155 
00156 //----------------FUNCTIONS--------------------------
00157 
00158 // ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~
00159 void emgsample()
00160 {
00161     //All EMG signal through Highpass
00162     double emgread1 = emg1.read();
00163     double emgread2 = emg2.read();
00164     double emgread3 = emg3.read();
00165     double emgread4 = emg4.read();
00166 
00167     double emg1_highpassed = highp1.step(emgread1);
00168     double emg2_highpassed = highp2.step(emgread2);
00169     double emg3_highpassed = highp3.step(emgread3);
00170     double emg4_highpassed = highp4.step(emgread4);
00171 
00172     //All EMG highpassed through Notch
00173     double emg1_notched = notch1.step(emg1_highpassed);
00174     double emg2_notched = notch2.step(emg2_highpassed);
00175     double emg3_notched = notch3.step(emg3_highpassed);
00176     double emg4_notched = notch4.step(emg4_highpassed);
00177 
00178     //All EMG notched rectify
00179     double emg1_abs = abs(emg1_notched);
00180     double emg2_abs = abs(emg2_notched);
00181     double emg3_abs = abs(emg3_notched);
00182     double emg4_abs = abs(emg4_notched);
00183 
00184     //All EMG abs into lowpass
00185     emg1_filtered = lowp1.step(emg1_abs);
00186     emg2_filtered = lowp2.step(emg2_abs);
00187     emg3_filtered = lowp3.step(emg3_abs);
00188     emg4_filtered = lowp4.step(emg4_abs);
00189 }
00190 
00191 void CalibrationEMG()
00192 {
00193     //static float samples = calibration_time/ts;
00194     while(timer_calibration<55) {
00195         if(timer_calibration>0 && timer_calibration<10) {
00196             led1=!led1;
00197             if(emg1_filtered>temp_highest_emg1) {
00198                 temp_highest_emg1= emg1_filtered;
00199                 pc.printf("Temp1 = %f \r\n",temp_highest_emg1);
00200             }
00201         }
00202         if(timer_calibration>10 && timer_calibration<15) {
00203             led1=0;
00204             led2=0;
00205             led3=0;
00206         }
00207         if(timer_calibration>15 && timer_calibration<25) {
00208             led2=!led2;
00209             if(emg2_filtered>temp_highest_emg2) {
00210                 temp_highest_emg2= emg2_filtered;
00211                 pc.printf("Temp2 = %f \r\n",temp_highest_emg2);
00212             }
00213         }
00214         if(timer_calibration>25 && timer_calibration<30) {
00215             led1=0;
00216             led2=0;
00217             led3=0;
00218         }
00219         if(timer_calibration>30 && timer_calibration<40) {
00220             led3=!led3;
00221             if(emg3_filtered>temp_highest_emg3) {
00222                 temp_highest_emg3= emg3_filtered;
00223                 pc.printf("Temp3 = %f \r\n",temp_highest_emg3);
00224             }
00225         }
00226         if(timer_calibration>40 && timer_calibration<45) {
00227             led1=0;
00228             led2=0;
00229             led3=0;
00230         }
00231         if(timer_calibration>45 && timer_calibration<55) {
00232             led2=!led2;
00233             led3=!led3;
00234             if(emg4_filtered>temp_highest_emg4) {
00235                 temp_highest_emg4= emg4_filtered;
00236                 pc.printf("Temp4 = %f \r\n",temp_highest_emg4);
00237             }
00238         }
00239         led1=1;
00240         led2=1;
00241         led3=1;
00242     }
00243 /*
00244     pc.printf("Highest value right biceps= %f \r\n", temp_highest_emg1);
00245     pc.printf("Highest value right triceps= %f \r\n", temp_highest_emg2);
00246     pc.printf("Highest value left biceps= %f \r\n", temp_highest_emg3);
00247     pc.printf("Highest value left triceps= %f \r\n", temp_highest_emg4);
00248 */
00249 
00250     threshold1 = temp_highest_emg1*biceps_p_t;  //Right Biceps
00251     threshold2 = temp_highest_emg2*triceps_p_t; //Right Triceps
00252     threshold3 = temp_highest_emg3*biceps_p_t;  //Left Biceps
00253     threshold4 = temp_highest_emg4*triceps_p_t; //Left Triceps
00254 }
00255 
00256 //Check if emg_filtered has reached their threshold
00257 void threshold_check()
00258 {
00259 
00260     //EMG1 threshold check
00261     if(emg1_filtered>threshold1) {
00262         bicepsR = 1;
00263     } else {
00264         bicepsR= 0;
00265     }
00266     //EMG2 threshold check
00267     if(emg2_filtered>threshold2) {
00268         tricepsR = 1;
00269     } else {
00270         tricepsR= 0;
00271     }
00272     //EMG3 threshold check
00273     if(emg3_filtered>threshold3) {
00274         bicepsL = 1;
00275     } else {
00276         bicepsL= 0;
00277     }
00278     //EMG4 threshold check
00279     if(emg4_filtered>threshold4) {
00280         tricepsL = 1;
00281     } else {
00282         tricepsL= 0;
00283     }
00284 
00285     /*
00286     pc.printf("Biceps Right = %i", bicepsR);
00287     pc.printf("Triceps Right = %i",tricepsR);
00288     pc.printf("Biceps Left = %i", bicepsL);
00289     pc.printf("Triceps Left = %i", tricepsL);
00290     */
00291 }
00292 
00293 //Activate ticker for Movement state, filtering and Threshold checking
00294 void movement_ticker_activator()
00295 {
00296     sample_ticker.attach(&emgsample, ts);
00297     threshold_check_ticker.attach(&threshold_check, ts);
00298 }
00299 void movement_ticker_deactivator()
00300 {
00301     sample_ticker.detach();
00302     threshold_check_ticker.detach();
00303 }
00304 
00305 // ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~
00306 
00307 // functie x positie
00308 float positionx(int EMGxplus,int EMGxmin)
00309 {
00310     float EMGx = EMGxplus - EMGxmin;
00311 
00312     float verplaatsingx = EMGx * tijdstap * v;
00313     float pxnieuw = px + verplaatsingx;
00314     // x limit
00315     if (pxnieuw <= upperxlim && pxnieuw >= lowerxlim) {
00316         px = pxnieuw;
00317     } else {
00318         if (pxnieuw >= lowerxlim) {
00319             px = upperxlim;
00320         } else {
00321             px = lowerxlim;
00322         }
00323     }
00324 //printf("X eindpunt (%f) en verplaatsing: (%f)\n\r",px,verplaatsingx);
00325     return px;
00326 }
00327 
00328 
00329 // functie y positie
00330 float positiony(int EMGyplus,int EMGymin)
00331 {
00332     float EMGy = EMGyplus - EMGymin;
00333 
00334     float verplaatsingy = EMGy * tijdstap * v;
00335     float pynieuw = py + verplaatsingy;
00336 
00337     // y limit
00338     if (pynieuw <= upperylim && pynieuw >= lowerylim) {
00339         py = pynieuw;
00340     } else {
00341         if (pynieuw >= lowerylim) {
00342             py = upperylim;
00343         } else {
00344             py = lowerylim;
00345         }
00346     }
00347 //printf("Y eindpunt (%f) en verplaatsing: (%f) \n\r",py,verplaatsingy);
00348     return (py);
00349 }
00350 
00351 
00352 //~~~~~~~~~~~~CALCULATIING MOTOR ANGLES ~~~~~~~~
00353 // arm 1 --> reference angle motor 1
00354 float hoek1(float px, float py) // input: ref x, ref y
00355 {
00356     float c1x =  px - rp * cos(thetap +(M_PI/6));       // x locatie hoekpunt end-effector
00357     float c1y = py - rp*sin(thetap+(M_PI/6));           // y locatie hoekpunt end-effector
00358     float alpha1 = atan2((c1y-a1y),(c1x-a1x));          // hoek tussen horizontaal en lijn van motor naar bijbehorende end-effector punt
00359     float psi1 = acos(( pow(la,2)-pow(lp,2)+pow((c1x-a1x),2)+pow((c1y-a1y),2))/(2*la*sqrt(pow ((c1x-a1x),2)+pow((c1y-a1y),2) ))); //Hoek tussen lijn van motor naar bijbehorende end=effector punt en actieve arm
00360     float a1 = alpha1 + psi1 - da1;                          //hoek tussen horizontaal en actieve arm
00361     //printf("arm 1 = %f \n\r",a1);
00362     return a1;
00363 }
00364 
00365 // arm 2 --> reference angle motor 2
00366 float hoek2(float px, float py)
00367 {
00368     float c2x =  px - rp * cos(thetap -(M_PI/2));
00369     float c2y = py - rp*sin(thetap-(M_PI/2));
00370     float alpha2 = atan2((c2y-a2y),(c2x-a2x));
00371     float psi2 = acos(( pow(la,2)-pow(lp,2)+pow((c2x-a2x),2)+pow((c2y-a2y),2))/(2*la*sqrt(pow ((c2x-a2x),2)+pow((c2y-a2y),2) )));
00372     float a2 = alpha2 + psi2 - da2;
00373     //printf("arm 2 = %f \n\r",a2);
00374     return a2;
00375 }
00376 
00377 //arm 3 --> reference angle motor 3
00378 float hoek3(float px, float py)
00379 {
00380     float c3x =  px - rp * cos(thetap +(5*M_PI/6));
00381     float c3y = py - rp*sin(thetap+(5*M_PI/6));
00382     float alpha3 = atan2((c3y-a3y),(c3x-a3x));
00383     float psi3 = acos(( pow(la,2)-pow(lp,2)+pow((c3x-a3x),2)+pow((c3y-a3y),2))/(2*la*sqrt(pow ((c3x-a3x),2)+pow((c3y-a3y),2) )));
00384     float a3 = alpha3 + psi3 - da3;
00385     //printf("arm 3 = %f \n\r",a3);
00386     return a3;
00387 }
00388 
00389 // ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~
00390 
00391 double PID_controller(double error)
00392 {
00393     static double error_integral = 0;
00394     static double error_prev = error; // initialization with this value only done once!
00395     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
00396 
00397     // Proportional part:
00398     double u_k = Kp * error;
00399 
00400     // Integral part
00401     error_integral = error_integral + error * Ts;
00402     double u_i = Ki * error_integral;
00403 
00404     // Derivative part
00405     double error_derivative = (error - error_prev)/Ts;
00406     double filtered_error_derivative = LowPassFilter.step(error_derivative);
00407     double u_d = Kd * filtered_error_derivative;
00408     error_prev = error;
00409 
00410     // Sum all parts and return it
00411     return u_k + u_i + u_d;
00412 }
00413 
00414 
00415 // DIRECTON AND SPEED CONTROL
00416 void moter_control(double u)
00417 {
00418 
00419     directionpin1= u > 0.0f; //eithertrueor false
00420     if (fabs(u)> 0.7f) {
00421         u = 0.7f;
00422     } else {
00423         u= u;
00424     }
00425     pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
00426 }
00427 
00428 void moter2_control(double u)
00429 {
00430     directionpin2= u > 0.0f; //eithertrueor false
00431     if (fabs(u)> 0.7f) {
00432         u = 0.7f;
00433     } else {
00434         u= u;
00435     }
00436     pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
00437 }
00438 
00439 void moter3_control(double u)
00440 {
00441     directionpin3= u > 0.0f; //eithertrueor false
00442     if (fabs(u)> 0.7f) {
00443         u = 0.7f;
00444     } else {
00445         u= u;
00446     }
00447     pwmpin3 = fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
00448 }
00449 
00450 // CONTROLLING THE MOTOR
00451 void Motor_mover()
00452 {
00453     double motor_position = encoder1.getPulses(); //output in counts
00454     double reference_rotation = hoek1(px, py);
00455     double error = reference_rotation - motor_position*(2*PI)/8400;
00456     double u = PID_controller(error);
00457     moter_control(u);
00458 
00459     double motor_position2 = encoder2.getPulses(); //output in counts
00460     double reference_rotation2 = hoek2(px, py);
00461     double error_2 = reference_rotation2 - motor_position2*(2*PI)/8400;
00462     double u_2 = PID_controller(error_2);
00463     moter2_control(u_2);
00464 
00465     double motor_position3 = encoder3.getPulses(); //output in counts
00466     double reference_rotation3 = hoek3(px, py);
00467     double error_3 = reference_rotation3 - motor_position3*(2*PI)/8400;
00468     double u_3 = PID_controller(error_3);
00469     moter3_control(u_3);
00470 
00471 
00472 }
00473 
00474 
00475 
00476 //-------------------- STATE MACHINE --------------------------
00477 enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK};
00478 states currentState = MOTORS_OFF; //Chosen startingposition for states
00479 bool stateChanged = true; // Make sure the initialization of first state is executed
00480 
00481 void ProcessStateMachine(void)
00482 {
00483     switch (currentState) {
00484         case MOTORS_OFF:
00485             // Actions
00486             if (stateChanged) {
00487                 // state initialization: rood
00488                 led1 = 1;
00489                 led2 = 0;
00490                 led3 = 1;
00491                 wait (1);
00492                 stateChanged = false;
00493             }
00494 
00495             // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
00496             if (!button1) {
00497                 currentState = CALIBRATION;
00498                 stateChanged = true;
00499             } else if (!button2) {
00500                 currentState = HOMING  ;
00501                 stateChanged = true;
00502             } else {
00503                 currentState = MOTORS_OFF;
00504                 stateChanged = true;
00505             }
00506 
00507             break;
00508 
00509         case CALIBRATION:
00510             // Actions
00511             if (stateChanged) {
00512                 // state initialization: oranje
00513                 temp_highest_emg1 = 0; //highest detected value right biceps
00514                 temp_highest_emg2 = 0;
00515                 temp_highest_emg3 = 0;
00516                 temp_highest_emg4 = 0;
00517 
00518                 timer_calibration.reset();
00519                 timer_calibration.start();
00520 
00521                 sample_ticker.attach(&emgsample, ts);
00522                 CalibrationEMG();
00523                 sample_ticker.detach();
00524                 timer_calibration.stop();
00525 
00526 
00527                 stateChanged = false;
00528             }
00529 
00530             // State transition logic: automatisch terug naar motors off.
00531 
00532             currentState = MOTORS_OFF;
00533             stateChanged = true;
00534             break;
00535 
00536         case HOMING:
00537             // Actions
00538             if (stateChanged) {
00539                 // state initialization: green
00540                 t.reset();
00541                 t.start();
00542                 led1 = 0;
00543                 led2 = 1;
00544                 led3 = 1;
00545                 wait (1);
00546 
00547                 stateChanged = false;
00548             }
00549 
00550             // State transition logic: naar DEMO (button1), naar MOVEMENT(button2)
00551             if (!button1) {
00552                 currentState = DEMO;
00553                 stateChanged = true;
00554             } else if (!button2) {
00555                 currentState = MOVEMENT  ;
00556                 stateChanged = true;
00557             } else if (t>300) {
00558                 t.stop();
00559                 t.reset();
00560                 currentState = MOTORS_OFF  ;
00561                 stateChanged = true;
00562             } else {
00563                 currentState = HOMING  ;
00564                 stateChanged = true;
00565             }
00566             break;
00567 
00568         case DEMO:
00569             // Actions
00570             if (stateChanged) {
00571                 // state initialization: light blue
00572                 led1 = 0;
00573                 led2 = 1;
00574                 led3 = 0;
00575                 wait (1);
00576 
00577                 stateChanged = false;
00578             }
00579 
00580             // State transition logic: automatisch terug naar HOMING
00581             currentState = HOMING;
00582             stateChanged = true;
00583             break;
00584 
00585         case MOVEMENT:
00586             // Actions
00587             if (stateChanged) {
00588                 // state initialization: purple
00589                 //t.reset();
00590                 //t.start();
00591 
00592                 led1 = 1;
00593                 led2 = 0;
00594                 led3 = 0;
00595                 wait(2);
00596 
00597                 movement_ticker_activator();
00598 
00599                 led1 = 0;
00600                 led2 = 0;
00601                 led3 = 0;
00602                 wait(2);
00603 
00604 
00605                 stateChanged = false;
00606             }
00607 
00608             // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT
00609             if (!button1) {
00610                 movement_ticker_deactivator();
00611                 currentState = CLICK;
00612                 stateChanged = true;
00613             } else if (!button2) {
00614                 movement_ticker_deactivator();
00615                 currentState = MOTORS_OFF  ;
00616                 stateChanged = true;
00617             } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0) { //this check if person is idle for more than 300seconds
00618                 t.start();
00619             } else if  (bicepsR==1 || tricepsR==1 || bicepsL==1 || tricepsL==1) {
00620                 t.stop();
00621                 t.reset();
00622             }
00623 
00624             if(t>20) {
00625                 movement_ticker_deactivator();
00626                 t.stop();
00627                 t.reset();
00628                 currentState = HOMING  ;
00629                 stateChanged = true;
00630             }
00631             // here ends the idle checking mode
00632             else {
00633                 //For every muscle a different colour if threshold is passed
00634                 if(bicepsR==1) {
00635                     led1 = 0;
00636                     led2 = 1;
00637                     led3 = 1;
00638                 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
00639                     led1 = 1;
00640                     led2 = 1;
00641                     led3 = 1;
00642                 }
00643                 if(tricepsR==1) {
00644                     led1 = 1;
00645                     led2 = 0;
00646                     led3 = 1;
00647                 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
00648                     led1 = 1;
00649                     led2 = 1;
00650                     led3 = 1;
00651                 }
00652                 if(bicepsL==1) {
00653                     led1 = 1;
00654                     led2 = 1;
00655                     led3 = 0;
00656                 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
00657                     led1 = 1;
00658                     led2 = 1;
00659                     led3 = 1;
00660                 }
00661                 if(tricepsL==1) {
00662                     led1 = 1;
00663                     led2 = 0;
00664                     led3 = 0;
00665                 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
00666                     led1 = 1;
00667                     led2 = 1;
00668                     led3 = 1;
00669                 }
00670             }
00671 
00672             break;
00673 
00674         case CLICK:
00675             // Actions
00676             if (stateChanged) {
00677                 // state initialization: blue
00678                 led1 = 1;
00679                 led2 = 1;
00680                 led3 = 0;
00681                 wait (1);
00682 
00683                 stateChanged = false;
00684             }
00685 
00686             // State transition logic: automatisch terug naar MOVEMENT.
00687 
00688             currentState = MOVEMENT;
00689             stateChanged = true;
00690             break;
00691 
00692     }
00693 }
00694 
00695 // --------------------------MAIN--------------------
00696 
00697 
00698 int main()
00699 {
00700     //BiQuad Chain add
00701     highp1.add( &highp1_1 ).add( &highp1_2 );
00702     notch1.add( &notch1_1 ).add( &notch1_2 );
00703     lowp1.add( &lowp1_1 ).add(&lowp1_2);
00704 
00705     highp2.add( &highp2_1 ).add( &highp2_2 );
00706     notch2.add( &notch2_1 ).add( &notch2_2 );
00707     lowp2.add( &lowp2_1 ).add(&lowp2_2);
00708 
00709     highp3.add( &highp3_1 ).add( &highp3_2 );
00710     notch3.add( &notch3_1 ).add( &notch3_2 );
00711     lowp3.add( &lowp3_1 ).add(&lowp3_2);
00712 
00713     highp4.add( &highp4_1 ).add( &highp4_2 );
00714     notch4.add( &notch4_1 ).add( &notch4_2 );
00715     lowp4.add( &lowp4_1 ).add(&lowp4_2);
00716 
00717     pc.baud(115200);
00718     led1 = 1;
00719     led2 = 1;
00720     led3 = 1;
00721     
00722     pwmpin1.period_us(60); // setup motor
00723     ref_rot.attach(Motor_mover, 0.1f);// HAS TO GO TO STATE MACHINE
00724     movement_ticker_activator()
00725     while (true) {
00726         //ProcessStateMachine();
00727         
00728         if (button2 == false) {
00729             wait(0.01f);
00730 
00731             // berekenen positie
00732             float px = positionx(1,0);  // EMG: +x, -x
00733             float py = positiony(0,0);  // EMG: +y, -y
00734             //printf("positie (%f,%f)\n\r",px,py);
00735         }
00736 
00737         if (button1 == false) {
00738             wait(0.01f);
00739             // berekenen positie
00740             float px = positionx(0,1);  // EMG: +x, -x
00741             float py = positiony(0,0);  // EMG: +y, -y
00742             //printf("positie (%f,%f)\n\r",px,py);
00743         }
00744 
00745         if (button3 == false) {
00746             wait(0.01f);
00747             // berekenen positie
00748             float px = positionx(0,0);  // EMG: +x, -x
00749             float py = positiony(1,0);  // EMG: +y, -y
00750             //printf("positie (%f,%f)\n\r",px,py);
00751         }
00752 
00753         if (button4 == false) {
00754             wait(0.01f);
00755             // berekenen positie
00756             float px = positionx(0,0);  // EMG: +x, -x
00757             float py = positiony(0,1);  // EMG: +y, -y
00758             //printf("positie (%f,%f)\n\r",px,py);
00759         }
00760 
00761     }
00762 
00763 }
00764 
00765 
00766 
00767 
00768 
00769