Revision 67:5ebb08e337ae, committed 2019-10-17

Comitter:

JornD

Date:

Thu Oct 17 14:39:36 2019 +0000

Branch:

Branch2

Parent:

66:fa7171cf3f67

Commit message:

WORKING - Implemented moving average

Changed in this revision

--- a/controller.cpp.orig	Wed Oct 16 13:39:19 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,65 +0,0 @@
-#include "structures.h"
-#include <math.h>
-#include "global.cpp"
-
-    extern const float Ts;
-    float K = 2/Ts;
-
-/*
-To call this function, the coefficients variable in the main scope needs to have a 
-"*" in front of it. The coefficients can be printed using:
-
-       for ( int i = 0; i < 5; i++ ) {
-        std::cout << *(coefficient_variable_name+i) << std::endl;
-        }
-
-*/
-
-void BuildController(ControllerSettings& StrucName, float P[], float Q[])
-{
-    
-    StrucName.A = (Q[0] * pow(K,2) + Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
-    StrucName.B = (2 * Q[2] - 2 * Q[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
-    StrucName.C = (Q[0] * pow(K,2) - Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
-    StrucName.D = (2 * P[2] - 2 * P[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
-    StrucName.E = (P[0] * pow(K,2) - P[1] * K + P[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);;
-}
-
-void InitializeControllers(void)
-{
-    //Define Controller structures, shorthand: Set_
-    ControllerSettings Set_ProcessEMG;
-    ControllerSettings Set_Base;            //Controller base
-    ControllerSettings Set_EndAffector;     //Controller end affector 
-    
-    //Define Memory cells of the Input/Output, shorthand: Mem_
-    MemoryIO Mem_ProcessEMG;
-    MemoryIO Mem_Base;
-    MemoryIO Mem_EndAffector;
-    
-    
-    //Build Controllers and define P and Q, the transfer function of the controller
-    //--Build controller, EMG
-        float PE[] = {1,2,3};
-        float QE[] = {4,5,6};
-        BuildController(Set_ProcessEMG, PE, QE);
-    //--Build controller, Base
-        float PB[] = {1,2,3};
-        float QB[] = {4,5,6};
-        BuildController(Set_Base, PB, QB);
-    //--Build controller, End Affector
-        float PA[] = {1,2,3};
-        float QA[] = {4,5,6};
-        BuildController(Set_EndAffector, PA, QA);
-        
-}
-
-float Biquad(ControllerSettings& StrucName, MemoryIO& MemName, float X)
-{
-    //Difference equation which implements the digital biquad filter
-    float Y = StrucName.A * X + StrucName.B * MemName.Xm + StrucName.C * MemName.Xmm + StrucName.D * MemName.Ym + StrucName.E * MemName.Ymm;        
-    
-    
-    return Y; 
-}
-

--- a/emgprocessing.cpp	Wed Oct 16 13:39:19 2019 +0000
+++ b/emgprocessing.cpp	Thu Oct 17 14:39:36 2019 +0000
@@ -1,9 +1,12 @@
 #include "functions.h"
 #include "structures.h"
 #include "global.h"
+#include "math.h"
 
-float V[5];
+float V[5] = {0,0,0,0,0};
+float W[2] = {0,0};
 float SUM;
+float a = 0.2;
 
 float MovingAverage(float X)
 {
@@ -20,17 +23,30 @@
     
     return Avg;
 }
+float CurveSmoothing(float X)
+{
+    W[1] = W[0];
+    W[0] = X;
+    
+    float Y = a*W[0] + (1-a)*W[1];
+    
+    return Y;
+}
 
 float ProcessEMG(float X)
 {
     //Apply LPF and Notch Filter
-    float Temp = Biquad(Set_LPFEMG, Mem_LPFEMG, X);
-    float Y = Biquad(Set_NOTEMG, Mem_NOTEMG, Temp);
+    float temp = Biquad(Set_LPFEMG, Mem_LPFEMG, X);
+    temp = Biquad(Set_NOTEMG, Mem_NOTEMG, temp);
 
     //Calculate moving average
-    float Avg = MovingAverage(Y);
+    float Avg = MovingAverage(temp);
     //Subtract moving average
-    Y = Y - Avg;
-
+    temp = temp - Avg;
+    //Take absolute value
+    temp = fabs(temp);
+    //Smooth ze Curve
+    float Y = CurveSmoothing(temp);
+    
     return Y;
 }