ksf

Dependents:   kinematics_controlv2 kinematics_controlv4 kinematics_control_copyfds Robot_control

Revision:
1:15543c143a63
Parent:
0:5f8dee4d4b09
Child:
2:1a677b57ce81
--- a/signalnumber.cpp	Mon Oct 30 10:41:44 2017 +0000
+++ b/signalnumber.cpp	Tue Oct 31 14:34:30 2017 +0000
@@ -3,11 +3,10 @@
 
 
 //Constants for mean value
-const int n = 10;                   //Window size for the mean value
+const int n = 400;                   //Window size for the mean value
 float emg0_filtered[n] = {}; 
 int count = 0;
 int count2 = 0;
-//const float controller_TS = 0.1;
 float mean = 0.0;
 float sum = 0.0;
 
@@ -69,12 +68,12 @@
 mode. If the mean value changes outside of the region the switch will be set to 
 0, this is the resting state. Also the counter will be reset.
 */
-int SignalNumber::getnumber(const int n, const int action, double input){
-    mean = getmean(n,input);
+int SignalNumber::getnumber(const int n, const int action, double input, float k){
+    mean = getmean(n, input)*k;
     //Check first case
     if( mean < LeftFastmin ) {
         if (count2 <action){
-            mean = getmean(n, input);
+            mean = getmean(n, input)*k;
             if(mean < LeftFastmin){
                 count2++;
             }
@@ -91,7 +90,7 @@
      //Check second case  
      else if(mean <= LeftFastmax and mean > LeftFastmin){
         if (count2 <action){
-            mean = getmean(n,input);
+            mean = getmean(n, input)*k;
             if(mean <=LeftFastmax and mean>LeftFastmin){
                 count2++;
             }
@@ -107,7 +106,7 @@
     }
      else if( mean <=LeftSlowmax and mean>LeftSlowmin) {
         if (count2 <action){
-            mean = getmean(n,input);
+            mean = getmean(n, input)*k;
             if(mean <=LeftSlowmax and mean>LeftSlowmin){
                 count2++;
             }
@@ -123,7 +122,7 @@
      }
      else if( mean <=RightSlowmax and mean>RightSlowmin) {
         if (count2 <action){
-            mean = getmean(n,input);
+            mean = getmean(n, input)*k;
             if(mean <=RightSlowmax and mean>RightSlowmin){
                 count2++;
             }
@@ -139,7 +138,7 @@
      }
      else if( mean <=RightFastmax and mean>RightFastmin ) {
         if (count2 <action){
-            mean = getmean(n,input);
+            mean = getmean(n, input)*k;
             if(mean <=RightFastmax and mean>RightFastmin){
                 count2++;
             }
@@ -161,3 +160,28 @@
 }       
 
 
+// In progress
+int SignalNumber::getmode(const int n, const int action, double input, float k){
+    int mode;
+    mean = getmean(n, input)*k;
+    //Check first case
+    if( mean < LeftFastmin ) {
+        if (count2 <action){
+            mean = getmean(n, input)*k;
+            if(mean < LeftFastmin){
+                count2++;
+            }
+            else{
+                count2=0;
+                Number=0;
+            } 
+        }
+        else{
+            Number = 0;
+            count2=0;
+        }
+    }
+    return mode;
+}
+
+