Diff: NR_method_1.cpp

Revision:

6:e492bc8fc3fb

Parent:

5:d3031d082c22

Child:

7:fcb20c3ccee9

--- a/NR_method_1.cpp	Thu Nov 01 11:36:28 2018 +0000
+++ b/NR_method_1.cpp	Thu Nov 01 12:59:27 2018 +0000
@@ -7,6 +7,9 @@
 #include <ctime>
 #include <QEI.h>
 #include "PID_controler.h"
+#include <HIDScope.h>
+
+
 //#include  <MODSERIAL.h>
 
 // hidscope
@@ -16,6 +19,9 @@
 int waiting;
 InterruptIn button(SW3);
 
+InterruptIn xxx(D0);
+InterruptIn yyy(D1);
+
 Serial pc(USBTX, USBRX);
 // emg signals input
 AnalogIn   emg1(A0);
@@ -32,24 +38,23 @@
 // filtering
 //filter coeffiecents
 // highpass
-double b01h = 0.978030479206560;
-double b02h = -1.95606095841312;
-double b03h =  0.978030479206560;
-double a01h = -1.95557824031504;
-double a02h =   0.956543676511203;
+double b01h = 0.956543225556877;
+double b02h = -1.91308645111375;
+double b03h =  0.956543225556877;
+double a01h = -1.91119706742607;
+double a02h =   0.914975834801434;
 // notchfilter
-double b01n = 0.995532032687234;
-double b02n =   -1.89361445373551;
-double b03n = 0.995532032687234;
-double a01n =   -1.89361445373551;
-double a02n =  0.991064065374468 ;
+double b01n = 0.991103635646810;
+double b02n =   -1.60363936885013;
+double b03n = 0.991103635646810;
+double a01n =   -1.60363936885013;
+double a02n =  0.982207271293620;
 //lowpass 1
-double b01l =  8.76555487540065e-05;
-double b02l =  0.000175311097508013;
-double b03l =  8.76555487540065e-05;
-double a01l =  -1.97334424978130;
-double a02l =  0.973694871976315;
-
+double b01l =  0.000346041337639103;
+double b02l =  0.000692082675278205;
+double b03l =  0.000346041337639103;
+double a01l =  -1.94669754075618;
+double a02l =  0.948081706106740;
 BiQuadChain bqc;
 BiQuad bq1( b01h, b02h, b03h, a01h, a02h ); //highpass
 BiQuad bq2( b01n, b02n, b03n, a01n, a02n ); //notch
@@ -96,6 +101,7 @@
 int bb;
 int bc;
 float z;
+int i;
 
 double angle_a = 0; //in rad
 double angle_b = 0.5 * pi;  //in rad
@@ -139,6 +145,7 @@
 void Position1x(double b)
 {
     if (b > 0.20) {
+        i = 0;
         Cxx =x1;
         if (dir == true) {
             if(x1 > -46) {
@@ -162,14 +169,13 @@
             } else {
             }
         }
-
-        wait(0.5);
     }
 }
 
 void Position1y(double b)
 {
     if (b > 0.20) {
+        i = 0;
         Cyy=y1;
         if(dir == true) {
             if(y1 < 43) {
@@ -190,8 +196,6 @@
             } else {
             }
         }
-
-        wait(0.5);
     }
 }
 void change()
@@ -335,7 +339,6 @@
         position_define();
         angle_define();
         motor_controler();
-        button.fall(&change_wait);
     }
     }
 }
@@ -361,6 +364,7 @@
     bqc4.add( &bq6 );
     Cxx = -35;
     Cyy = 27;
+    i = 251;
     while(true) {
 
         if(bas == true) {
@@ -370,14 +374,22 @@
             angle_define();
             motor_controler();
             
-            
-            scope.set(1, emgFiltered3); // filtered 1
-            scope.set(2, emgFiltered23); //filtered signal 2
+            scope.set(0, Cxx); // filtered 1
+            scope.set(1, Cyy); //filtered signal 2
             scope.send();
-
+            
+            if (i <= 250)
+            {
+                emgFiltered3 = 0;
+                emgFiltered23 = 0;
+                i++;   
+            }
+            
+            
             //sw2.fall(change);
-            //Position1x(emgFiltered3);
-            //Position1y(emgFiltered23);
+            Position1x(emgFiltered3);
+            Position1y(emgFiltered23);
+            
             Led = 0;
             bas= false;
         }