Diff: main.cpp

Revision:

18:e3b41351ee71

Parent:

17:d643e5954165

Child:

19:da210f89db18

--- a/main.cpp	Wed Oct 07 12:33:19 2015 +0000
+++ b/main.cpp	Wed Oct 07 13:25:49 2015 +0000
@@ -1,10 +1,10 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "encoder.h"
-// #include "HIDScope.h"
+#include "HIDScope.h"
 
 Serial pc(USBTX,USBRX);
-// HIDScope scope(4);          // definieerd het aantal kanalen van de scope
+HIDScope scope(1);          // definieerd het aantal kanalen van de scope
 
 Ticker          controller1, controller2;        // definieer de ticker die controler1 doet
 
@@ -25,7 +25,7 @@
 Encoder motor2_enc(D10,D9);
 
 //POTMETERS
-AnalogIn potright(A0);
+AnalogIn potright(A0);      
 AnalogIn potleft(A1);
 
 // RESETBUTTON
@@ -34,7 +34,7 @@
 
 
 // controller stuff
-double controlfreq = 10 ;    // controlloops frequentie (Hz)
+double controlfreq = 50 ;    // controlloops frequentie (Hz)
 double controlstep = 1/controlfreq; // timestep derived from controlfreq
 const double K1 = 1 ;       // P constant motorcontrolle 1
 const double K2 = 1;        // p constant motorcontroller 2
@@ -46,7 +46,63 @@
 // define the maximum rate of change for the setpoint (velocity)
 double Vmax = 5;            // rad/sec
 
+// // // // // // // FILTEREN TOEVOEGEN
 
+// tweede orde notch filter 50 Hz
+// biquad 1 coefficienten
+const double numnotch50biq1_1 = 1;
+const double numnotch50biq1_2 = -1.61816178466632;
+const double numnotch50biq1_3 = 1.00000006127058;
+const double dennotch50biq1_2 = -1.59325742941798;
+const double dennotch50biq1_3 = 0.982171881701431;
+// biquad 2 coefficienten
+const double numnotch50biq2_1 = 1;
+const double numnotch50biq2_2 = -1.61816171933244;
+const double numnotch50biq2_3 = 0.999999938729428;
+const double dennotch50biq2_2 = -1.61431180968071;
+const double dennotch50biq2_3 = 0.982599066293075;
+
+// highpass filter 20 Hz coefficienten
+const double numhigh20_1 = 0.837089190566345;
+const double numhigh20_2 = -1.67417838113269;
+const double numhigh20_3 = 0.837089190566345;
+const double denhigh20_2 = -1.64745998107698;
+const double denhigh20_3 = 0.700896781188403;
+
+// lowpass 5 Hz coefficienten
+const double numlow5_1 =0.000944691843840162;
+const double numlow5_2 =0.00188938368768032;
+const double numlow5_3 =0.000944691843840162;
+const double denlow5_2 =-1.91119706742607;
+const double denlow5_3 =0.914975834801434;
+
+// Define the storage variables and filter coeicients for two filters
+double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0,f4_v1 = 0, f4_v2 = 0;
+
+double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
+{
+    double v = u- a1*v1-a2*v2;
+    double y = b0*v+b1*v1+b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+    }
+
+// FILTERAAR
+double EMG_Filter()
+{
+// double u1 = ..., u2 = ... ;
+double u1 = potright.read();
+double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
+double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
+double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
+double y4 = abs(y3);
+double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
+return y5;
+}
+
+
+// // // // // // EINDE FILTERZOOI
 
 // counts 2 radians
 // this function takes the counts from the encoder and converts it to 
@@ -65,8 +121,8 @@
 {
     double offset = 0.5;            // offset the inputsignal to -0.5->0.5
     double gain = 2;                // increase the signal
-    double potset = (input-offset)*gain;
-    double setpoint = c_setpoint + potset * controlstep * Vmax ;
+    // double potset = (input-offset)*gain;
+    double setpoint = c_setpoint + input * controlstep * Vmax ;
         c_setpoint = setpoint;
     return setpoint;
 }
@@ -103,12 +159,21 @@
 // // // // // // // // // //
 void motor1_control()
 {
-    double setpoint1 = setpoint_f(potright.read(), c_setpoint1);      // determine the setpoint that has been set by the inputsignal
+    double input = (EMG_Filter())*15 ;
+    if(input > 1 )
+    {
+        input = 1;
+    }
+    else if(input < 0.2)
+    {
+        input = 0;
+    }
+    double setpoint1 = setpoint_f(input, c_setpoint1);      // determine the setpoint that has been set by the inputsignal
     double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
     double error1 = (setpoint1 - rads1);                       // determine the error (reference - position)
-    // scope.set(0,setpoint1);
+     scope.set(0,input);
     // scope.set(1,rads1);
-    // scope.send();
+     scope.send();
     double output1 = K_control(error1, K1);        // bereken de controller output voor motor 1(zie hierboven)
     if(output1 > 0) {                    // 
         motor1_rich.write(0);