Diff: Project_main.cpp

Revision:

40:7e93c2f1c1e9

Parent:

39:0328b1f16a5a

Child:

41:245f33fb2b8b

--- a/Project_main.cpp	Wed Oct 08 12:26:56 2014 +0000
+++ b/Project_main.cpp	Fri Oct 10 09:34:43 2014 +0000
@@ -1,34 +1,63 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "HIDScope.h"
+#include "arm_math.h"
 
 #define TIMEB4NEXTCHOICE 1  // sec keuzelampje blijft aan
 #define TIMEBETWEENBLINK 20 // sec voor volgende blink
 
 //Define objects
 AnalogIn    emg0(PTB1);         //Analog input biceps
-float       emg_bifloat;        //Float voor EMG-waarde biceps
+float       filemg_bifloat;     //filtered emg-waarde biceps
 
 AnalogIn    emg1(PTB2);         //Analog input triceps
-float       emg_trifloat;       //Float voor EMG-waarde triceps
+float       filemg_trifloat;    //filtered emg-waarde triceps
 
 Ticker log_timer;
 Ticker reset_timer;
 MODSERIAL pc(USBTX,USBRX);
 HIDScope scope(2);
 
+arm_biquad_casd_df1_inst_f32 bilowpass;
+float bilowpass_const[] = {0.02008337 , 0.04016673 , 0.02008337 , 1.56101808 , -0.64135154}; //biceps lowpass 5 Hz, Fsample = 500Hz
+float bilowpass_states[4];
+
+arm_biquad_casd_df1_inst_f32 bihighpass;
+float bihighpass_const[] = {0.97803048, -1.95606096,  0.97803048, 1.95557824 , -0.95654368}; //biceps highpass 0.5 Hz, Fsample = 500Hz
+float bihighpass_states[4];
+
+arm_biquad_casd_df1_inst_f32 trilowpass;
+float trilowpass_const[] = {0.02008337 , 0.04016673 , 0.02008337 , 1.56101808 , -0.64135154}; //biceps lowpass 5 Hz, Fsample = 500Hz
+float trilowpass_states[4];
+
+arm_biquad_casd_df1_inst_f32 trihighpass;
+float trihighpass_const[] = {0.97803048, -1.95606096,  0.97803048, 1.95557824 , -0.95654368}; //biceps highpass 0.5 Hz, Fsample = 500Hz
+float trihighpass_states[4];
+
 PwmOut      red(LED_RED);
 PwmOut      green(LED_GREEN);
 PwmOut      blue(LED_BLUE);
 
-int         direction = 0;
-int         force = 0;
+uint8_t         direction = 0;
+uint8_t         force = 0;
 
 void looper() 
 {
     //put raw emg value of biceps and triceps in emg_bifloat and emg_trifloat, respectively
+    float       emg_bifloat;        //Float voor EMG-waarde biceps
+    float       emg_trifloat;       //Float voor EMG-waarde triceps
     emg_bifloat = emg0.read();         // read float value (0..1 = 0..3.3V) biceps
     emg_trifloat = emg1.read();        // read float value (0..1 = 0..3.3V) triceps
+    
+    //process emg biceps
+    arm_biquad_cascade_df1_f32(&bihighpass, &emg_bifloat, &filemg_bifloat, 1 );
+    filemg_bifloat = fabs(filemg_bifloat);
+    arm_biquad_cascade_df1_f32(&bilowpass, &filemg_bifloat, &filemg_bifloat, 1 );
+    
+    //process emg triceps
+    arm_biquad_cascade_df1_f32(&trihighpass, &emg_trifloat, &filemg_trifloat, 1 );
+    filemg_trifloat = fabs(filemg_trifloat);
+    arm_biquad_cascade_df1_f32(&trilowpass, &filemg_trifloat, &filemg_trifloat, 1 );
 
     /*send value to PC. Line below is used to prevent buffer overrun */
     if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) { //VRAAG: praktisch nut hiervan? print emg value wanneer buffercount groter dan 30 is
@@ -37,8 +66,8 @@
 
     /* EMG-singaal van biceps en triceps worden hier gefilterd*/
 
-    scope.set(0,emg_bifloat);
-    scope.set(1,emg_trifloat);
+    scope.set(0,filemg_bifloat);
+    scope.set(1,filemg_trifloat);
     scope.send();
 }
 
@@ -56,7 +85,14 @@
 int main()
 {
     pc.baud(115200); //baudrate instellen
-    log_timer.attach(looper, 0.001); // The looper() function will be called every 0.001 seconds (with the ticker object)
+    log_timer.attach(looper, 0.002); // The looper() function will be called every 0.002 seconds (with the ticker object)
+    //set up filters triceps
+    arm_biquad_cascade_df1_init_f32(&bilowpass,1 , bilowpass_const, bilowpass_states);
+    arm_biquad_cascade_df1_init_f32(&bihighpass,1 ,bihighpass_const,bihighpass_states);
+    //set up filters triceps
+    arm_biquad_cascade_df1_init_f32(&trilowpass,1 , trilowpass_const, trilowpass_states);
+    arm_biquad_cascade_df1_init_f32(&trihighpass,1 ,trihighpass_const,trihighpass_states);
+    
 //    reset_timer.attach(resetlooper, 0.1); //
 
     goto directionchoice; // goes to first while(1) for the deciding the direction
@@ -69,7 +105,7 @@
                 green=1;
                 blue=1;
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
-                    if(emg_bifloat>0.8) {                   // 0.8 klopt niet als grenswaarde. #nofilter
+                    if(filemg_bifloat>0.8) {                   // 0.8 klopt niet als grenswaarde. #nofilter
                         direction = 1;
                         red=1;
                         green = 0;
@@ -87,7 +123,7 @@
                 green=0;
                 blue=1;
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
-                    if(emg_bifloat>0.8) {                    //0.8 klopt niet als grenswaarde. #nofilter
+                    if(filemg_bifloat>0.8) {                    //0.8 klopt niet als grenswaarde. #nofilter
                         direction = 2;
                         red=0;
                         green = 1;
@@ -105,7 +141,7 @@
                 green=1;
                 blue=0;
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
-                    if(emg_bifloat>0.8) {                    //0.8 klopt niet als grenswaarde. #nofilter
+                    if(filemg_bifloat>0.8) {                    //0.8 klopt niet als grenswaarde. #nofilter
                         direction = 3;
                         red=0;
                         green = 0;
@@ -131,7 +167,7 @@
                     goto directionchoice;
                 }
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
-                    if(emg_bifloat>0.8) {                    // 0.8 klopt niet als grenswaarde. #nofilter
+                    if(filemg_bifloat>0.8) {                    // 0.8 klopt niet als grenswaarde. #nofilter
                         force = 1;
                         red=1;
                         green = 0;
@@ -152,7 +188,7 @@
                     goto directionchoice;
                 }
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
-                    if(emg_bifloat>0.8) {                    //0.8 klopt niet als grenswaarde. #nofilter
+                    if(filemg_bifloat>0.8) {                    //0.8 klopt niet als grenswaarde. #nofilter
                         force = 2;
                         red=0;
                         green = 1;
@@ -173,7 +209,7 @@
                     goto directionchoice;
                 }
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
-                    if(emg_bifloat>0.8) {                    //0.8 klopt niet als grenswaarde. #nofilter
+                    if(filemg_bifloat>0.8) {                    //0.8 klopt niet als grenswaarde. #nofilter
                         force = 3;
                         red=0;
                         green = 0;
@@ -190,11 +226,16 @@
     }
 choicesmade:
 
+    red = 0;
+    green = 0;
+    blue = 0;           // wit lampje
+
     /* Vanaf hier komt de aansturing van de motor (inclusief de controller)*/
 
 
     if(direction == 1 && force == 1) {            // links zwak
         pc.printf("links zwak ");
+        wait(2);
     }
     if(direction == 1 && force == 2) {            // links normaal
         pc.printf("links normaal ");
@@ -224,8 +265,5 @@
         pc.printf("error ");
         // mogelijkheid om een goto te maken naar directionchoice (opzich wel handig)
     }
-
-    red = 0;
-    green = 0;
-    blue = 0;           // wit lampje
+    
 }
\ No newline at end of file