Diff: main.cpp

Revision:

0:9c203fdb48e0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 26 09:01:33 2017 +0000
@@ -0,0 +1,258 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "BiQuad.h"
+#include "HIDScope.h"
+#include <complex>
+#include <iostream>
+
+
+AnalogIn potmeter1(A3);
+AnalogIn    emg0( A0 );
+AnalogIn    emg1( A1 );
+
+HIDScope    scope( 3 );
+DigitalOut  led(LED1);
+
+Ticker sample_timer;
+Ticker MyTickerMean;
+
+//Constants------------------------------------
+const int n = 10;
+//const int n2 = 10;
+float emg0_filtered[n] = {}; 
+//float mean_filtered[n2] = {};
+int count = 0;
+int count2 = 0;
+const float controller_TS = 0.1;
+float mean = 0.0;
+float sum = 0.0;
+
+
+//Constants EMG switch
+const float LeftFastmin=0.075;
+const float LeftFastmax=0.15;
+const float LeftSlowmin=0.2;
+const float LeftSlowmax=0.35;
+const float RightSlowmin=0.4;
+const float RightSlowmax=0.65;
+const float RightFastmin=0.7;
+const float RightFastmax=1.5;
+int SignalNumber = 0;
+int numberOfSet = 0;
+const int action =50;
+
+
+//Functions---------------------------------------
+float read_pot(double potmeter){
+    float pot_value = potmeter;
+    return pot_value;
+}
+/*
+float get_sum(float array[], const int n){
+    float sum_math = 0.0;
+    for (int m=0 ; m<n ; m++ ){
+        sum_math = sum_math + array[m];
+    }
+    return sum_math;   
+}*/
+
+float get_sum(float array[], const int n){
+    float sum_math = 0.0;
+    for (int m=0 ; m<n ; m++ ){
+        sum_math = sum_math + array[m];
+    }
+    return sum_math;   
+}
+ 
+
+
+float get_mean_value(){
+    emg0_filtered[count] = read_pot(potmeter1);
+    count++;
+    if (count == n){
+        count = 0;
+    }
+    float mean_math = get_sum(emg0_filtered,n)/n;
+    return mean_math;
+}
+
+//----------------------------------------------------
+
+
+void get_Signal_number(){
+    mean = get_mean_value();
+    //Check first case
+    if( mean < LeftFastmin ) {
+        if (count2 <action){
+            mean = get_mean_value();
+            if(mean < LeftFastmin){
+                count2++;
+            }
+            else{
+                count2=0;
+                SignalNumber=0;
+            } 
+        }
+        else{
+            SignalNumber = 0;
+            count2=0;
+        }
+    }
+     //Check second case  
+     else if(mean <= LeftFastmax and mean > LeftFastmin){
+        if (count2 <action){
+            mean = get_mean_value();
+            if(mean <=LeftFastmax and mean>LeftFastmin){
+                count2++;
+            }
+            else{
+                count2=0;
+                SignalNumber=0;
+            } 
+        }
+        else{
+            SignalNumber = 1;
+            count2=0;
+        }
+    }
+     else if( mean <=LeftSlowmax and mean>LeftSlowmin) {
+        if (count2 <action){
+            mean = get_mean_value();
+            if(mean <=LeftSlowmax and mean>LeftSlowmin){
+                count2++;
+            }
+            else{
+                count2=0;
+                SignalNumber=0;
+            } 
+        }
+        else{
+            SignalNumber = 2;
+            count2=0;
+        }
+     }
+     else if( mean <=RightSlowmax and mean>RightSlowmin) {
+        if (count2 <action){
+            mean = get_mean_value();
+            if(mean <=RightSlowmax and mean>RightSlowmin){
+                count2++;
+            }
+            else{
+                count2=0;
+                SignalNumber=0;
+            } 
+        }
+        else{
+            SignalNumber = 3;
+            count2=0;
+        }
+     }
+     else if( mean <=RightFastmax and mean>RightFastmin ) {
+        if (count2 <action){
+            mean = get_mean_value();
+            if(mean <=RightFastmax and mean>RightFastmin){
+                count2++;
+            }
+            else{
+                count2=0;
+                SignalNumber=0;
+            } 
+        }
+        else{
+            SignalNumber = 4;
+            count2=0;
+        }
+    }
+    else{
+        count2=0;
+        SignalNumber =0;
+    }
+}       
+        
+/*switch(SignalNumber)
+    {   
+        case 0: //Standstill
+         motor1Direction=1;
+         motor1Speed=0;
+         printf("Motor 1 is standing still\n");
+         break;
+        case 1: //Move Left fast
+         motor1Direction=1;
+         motor1Speed=1;
+         printf("Motor 1 is moving left fast\n");
+         break;
+        case 2: //Move Left slow (movement speed is half of that of fast movement)
+         motor1Direction=1;
+         motor1Speed=0.5;
+         printf("Motor 1 is moving left slow\n");
+         break;
+        case 3: //Move right slow
+         motor1Direction=0;
+         motor1Speed=0.5;
+         printf("Motor 1 is moving right slow\n");
+         break;
+        case 4: //Move right fast
+         motor1Direction=0;
+         motor1Speed=1;
+         printf("Motor 1 is moving right fast\n");
+         break;
+        default : //if something is wrong, standstill
+         motor1Direction=1;
+         motor1Speed=0;
+         printf("Motor 1 is standing still, something went wrong\n")
+        
+}*/
+
+
+
+
+///-----------------------------------------------------------------------
+/** Sample function
+ * this function samples the emg and sends it to HIDScope
+ **/
+ 
+// Example: 4th order Butterworth LP (w_c = 0.1*f_nyquist)
+BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
+BiQuad HP2( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
+BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
+BiQuadChain BiQuad_filter;
+float Signal;
+float Signal_filtered;
+
+// Add the biquads to the chain
+
+ 
+void sample()
+{
+    Signal=(emg0+emg1)/2;
+    Signal_filtered= BiQuad_filter.step(Signal);
+    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
+    scope.set(0, emg0.read() );
+    scope.set(1, emg1.read() );
+    scope.set(2, Signal_filtered);
+    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
+    *  Ensure that enough channels are available (HIDScope scope( 2 ))
+    *  Finally, send all channels to the PC at once */
+    scope.send();
+    /* To indicate that the function is working, the LED is toggled */
+    led = !led;
+}
+
+
+  
+
+
+
+
+
+int main()
+{
+    BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3);
+
+    MyTickerMean.attach(&get_Signal_number, controller_TS);
+    sample_timer.attach(&sample, 0.02);
+    
+    while (true) {}
+        
+}
+