main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "BiQuad.h"
00004 #include "math.h"
00005 
00006 //Define objects
00007 AnalogIn    emg( A0 ); //EMG
00008 AnalogIn    emg1( A1 ); //EMG
00009 HIDScope    scope( 2 ); // aantal scopes dat gemaakt kan worden
00010 DigitalOut  ledB(LED_BLUE); 
00011 DigitalOut  ledG(LED_GREEN);
00012 DigitalIn TestButton(PTA4); // button naast het ledje
00013 DigitalIn onoff(PTC6); // button aan de andere kant
00014 Ticker emgSampleTicker; // Ticker voor de sample frequency
00015 PwmOut motor2MagnitudePin(D5); // magnitude motor 2
00016 DigitalOut motor2DirectionPin(D4); // direction of the motor 2
00017 InterruptIn MotorOn(D8);
00018 
00019 
00020 
00021  int P= 200; // aantal test punten voor de moving average
00022  double A[200]; // de vector waar punten in worden opgeslagen voor de moving average moet even groot zijn als P
00023  double B[200];
00024  int k = 0; // counter voor de configuratie
00025  double Vvector[200]; // vector voor de Vwaarde configuratie
00026  double Vwaarde[2]; // vector voor waardes van V
00027  int x = 0; // x waarde voor de Vwaarde
00028  double movmeanR;
00029  double movmeanL; //moving average for the right arm (first emg-shield)
00030  int MotorLock = 0;
00031  double EMGInputR;
00032  double EMGInputL;
00033  float ErrorZero = 3; 
00034  double MotorSpeed = 0.5;
00035  
00036  
00037  // Filters 
00038 BiQuadChain bqcR;
00039 BiQuad bq1R( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
00040 BiQuad bq2R( 0.6844323315947306, -1.3688646631894612,  0.6844323315947306,   -1.2243497755555959, 0.5133795508233266); //hp?
00041 BiQuad bq3R(  0.7566897754116633, -1.2243497755555959,  0.7566897754116633,   -1.2243497755555959, 0.5133795508233266); // notch?
00042 BiQuadChain bqcL;
00043 BiQuad bq1L( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
00044 BiQuad bq2L( 0.6844323315947306, -1.3688646631894612,  0.6844323315947306,   -1.2243497755555959, 0.5133795508233266); //hp?
00045 BiQuad bq3L(  0.7566897754116633, -1.2243497755555959,  0.7566897754116633,   -1.2243497755555959, 0.5133795508233266); // notch?
00046 
00047 
00048 
00049 // sample function voor plotten van de emg signalen en moving average 
00050 void emgSample() {
00051     
00052     double emgFilteredR = bqcR.step( emg.read() ); // gefilterde waarde van het emg signaal
00053     double emgFilteredL = bqcL.step( emg1.read());
00054     double emgabsR = abs(emgFilteredR); // absolute waarde van het gefilterde signaal
00055     double emgabsL = abs(emgFilteredL);
00056     //scope.set(0, emgFilteredR ); // stuurt de waarden naar de grafiek
00057     //scope.set(1, emgabsR ); // stuurt de waarden naar de grafiek
00058     
00059     
00060     // deze for-loop maakt de vector voor de moving average
00061     for(int i = P-1; i >= 0; i--){
00062         if (i == 0) {
00063             A[i] = emgabsR;
00064             }
00065          else {
00066              A[i] = A[i-1];
00067              }   
00068     }
00069     double sumR = 0;
00070     // deze for-loop sommeert de array
00071     for (int n = 0; n < P-1; n++) {
00072     sumR = sumR + A[n];
00073     }
00074     
00075     movmeanR = sumR/P; //dit is de moving average waarde
00076     
00077     // deze for-loop maakt de vector voor de moving average
00078     for(int i = P-1; i >= 0; i--){
00079         if (i == 0) {
00080             B[i] = emgabsL;
00081             }
00082          else {
00083              B[i] = B[i-1];
00084              }   
00085     }
00086     double sumL = 0;
00087     // deze for-loop sommeert de array
00088     for (int n = 0; n < P-1; n++) {
00089     sumL = sumL + B[n];
00090     }
00091     
00092     movmeanL = sumL/P; //dit is de moving average waarde
00093     
00094     // hier wordt het test programma opgestart, zodat zero waarde kan worden gekregen
00095     if (TestButton==0 & k<200) {
00096         Vvector[k] = movmeanR;
00097         ledB = !ledB;
00098         k++;
00099     }    
00100     else if (k==200) { // hier moet de test klaar zijn
00101         double sumZ = 0;
00102             for (int n = 0; n < 199; n++) {
00103             sumZ = sumZ + Vvector[n];
00104             } // neemt de som van de zerovector array
00105         Vwaarde[x] = sumZ/200; // dit is het gemiddelde voor een betrouwbare value
00106         //scope.set(3,Vwaarde[0]); //stuurt de zeroV waarde naar het plotje
00107         if (x == 1) {
00108           //  scope.set(4,Vwaarde[1]); //stuurt de maxV waarde naar het plotje
00109         }
00110         k++;
00111         ledB = 1;
00112         ledG = !ledG;
00113     }
00114     else if (k == 201 && onoff ==0) {// dit is om het ledje uit te doen en om het mogelijk te maken de test opnieuw te doen
00115         ledG = !ledG;
00116         k = 0;
00117         if (x==0) {
00118             x++;
00119         }
00120         else if (x==1) {
00121             x=0;
00122         }
00123     }
00124     scope.set(0, movmeanR); // stuurt de moving average naar de plot
00125     scope.set(1, movmeanL);
00126     
00127     if (movmeanR > Vwaarde[1]) {
00128         movmeanR = Vwaarde[1]; // zorgt ervoor dat emg nooit > 1 kan zijn
00129     }
00130     
00131     double EMGInputRawR =  (movmeanR - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero); 
00132     if (EMGInputRawR < 0) {
00133         EMGInputR = 0;
00134     }
00135     else {
00136         EMGInputR = EMGInputRawR;
00137     } 
00138     //scope.set(5,EMGInputR);  
00139     
00140     scope.send();
00141     
00142     //motor2MagnitudePin = 0.2;
00143 }
00144 
00145 float GetReferenceVelocity()
00146 {
00147     // Returns reference velocity in rad/s. 
00148     // Positive value means clockwise rotation.
00149     const float maxVelocity=8.4; // in rad/s of course!
00150     float referenceVelocity;  // in rad/s
00151     referenceVelocity = (EMGInputR * maxVelocity) * MotorLock; 
00152     return referenceVelocity;
00153 }
00154 
00155 void SetMotor2(float motorValue)
00156 {
00157     // Given -1<=motorValue<=1, this sets the PWM and direction
00158     // bits for motor 1. Positive value makes motor rotating
00159     // clockwise. motorValues outside range are truncated to
00160     // within range
00161     if (fabs(motorValue)>1) motor2MagnitudePin = 1;
00162         else motor2MagnitudePin = fabs(motorValue);
00163 }
00164 float FeedForwardControl(float referenceVelocity) {
00165     // very simple linear feed-forward control
00166     const float MotorGain=8.4; // unit: (rad/s) / PWM
00167     float motorValue = referenceVelocity / MotorGain;
00168     return motorValue;
00169 }
00170 
00171 void MotorLocker() {
00172     if (MotorLock == 0) {
00173         MotorLock = 1;
00174     }
00175     else if (MotorLock == 1) {
00176         MotorLock = 0;
00177     }
00178 }
00179 
00180 int main()
00181 {  
00182 ledB = 1;
00183 ledG = 1;
00184 bqcR.add( &bq1R ).add( &bq2R ).add( &bq3R ); // hier wordt het filter gemaakt
00185 bqcL.add( &bq1L ).add( &bq2L ).add( &bq3L ); // hier wordt het filter gemaakt
00186 emgSampleTicker.attach( &emgSample, 0.002 ); //dit bepaald de sample frequency en is nu 500 Hz
00187 Vwaarde[0] = 0.01;
00188 motor2DirectionPin = 1;
00189 
00190     while(1) {
00191         if (Vwaarde[1] == 0 || x==1) {
00192             MotorLock = 0;
00193         }
00194         else MotorLock = 1;
00195        // MotorOn.rise(&MotorLocker);
00196         motor2MagnitudePin = EMGInputR*MotorLock;
00197         wait(0.01);
00198     }
00199 }