Diff: main.cpp

Revision:

5:67afe491a1e3

Parent:

4:072b99947fc6

Child:

6:9c8e91bb1316

--- a/main.cpp	Sat Oct 10 08:26:48 2015 +0000
+++ b/main.cpp	Sat Oct 10 20:02:16 2015 +0000
@@ -90,6 +90,35 @@
 // Filter coefficients
         // differential action filter
         const double m_f_a1 = 1.0, m_f_a2 = 2.0, m_f_b0 = 1.0, m_f_b1 = 3.0, m_f_b2 = 4.0;      // coefficients from sheets are used as first test.
+    
+// 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;
 
 
 
@@ -98,7 +127,14 @@
 //////////////////////////////////////////////////////////////
 // these functions are tailored to perform 1 specific function
 
-
+// this funtion flips leds on and off accordin to input with 0 being on
+void LED(int red,int green,int blue)
+{
+    ledred.write(red);
+    ledgreen.write(green);
+    ledblue.write(blue);
+}    
+    
 // counts 2 radians
 // this function takes counts from the encoder and converts it to the amount of radians from the zero position. 
 // It has been set up for standard 2X DECODING!!!!!!
@@ -115,7 +151,7 @@
 double reference_f(double input, double &c_reference)
 {
     double  potset = (input-offset)*gain;
-    if(POT_in == true && potset < 0.1 && potset > -0.1) // larger area for potmeter to get a zero value
+    if(POT_in == true && potset < 0.15 && potset > -0.15) // larger area for potmeter to get a zero value
     { 
         potset = 0;
     }
@@ -161,6 +197,16 @@
     return y;
     }
 
+double EMG_Filter(double u1)
+{
+    // 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;
+}
 
 // PID Controller given in sheets
 // aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!)
@@ -204,6 +250,7 @@
 // MOTOR 1
 void motor1_control()
 {
+    
     double input1 = potright.read()*signalamp1;
     double reference1 = reference_f(input1, c_reference1);      // determine the reference that has been set by the inputsignal
     double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
@@ -213,11 +260,6 @@
     scope.send();   
     double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
     pc.printf("output 1 %f \n",output1);
-  
-    // ws best locatie om output te blokkeren als grenzen bereikt zijn, simpel if-loopje met rad1??
-    
-    
-    // onderstaand kan vervangen worden door een functie voor beide motoren??
     if(output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
         motor1_rich.write(0);
         motor1_aan.write(output1);
@@ -230,7 +272,7 @@
 // MOTOR 2
 void motor2_control()
 {
-    double input2 = potleft.read()*signalamp2;
+    double input2 = potleft.read()*signalamp2;      // replace potleft with filter
     double reference2 = reference_f(input2, c_reference2);      // determine the reference that has been set by the inputsignal
     double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
     double error2 = (reference2 - rads2);                       // determine the error (reference - position)
@@ -288,9 +330,7 @@
         // Main Control stuff and options
         if(loop_start == true && calib_start == false)        // check if start button = true then start the main control loops
         {
-            ledred.write(1);
-            ledgreen.write(1);
-            ledblue.write(0);
+            LED(1,1,0); // turn blue led on
                      
             if(motor1_go)
             {
@@ -305,21 +345,15 @@
         }
         if(loop_start == false && calib_start == true)          // start calibration procedures
         {
-            ledred.write(1);
-            ledgreen.write(0);
-            ledblue.write(1);
+           LED(1,0,1); // turn green led on
         }
         if(loop_start == true && calib_start == true)           // check if both buttons are enabled and stop everything
         {
-            ledred.write(0);
-            ledgreen.write(1);
-            ledblue.write(1);
+            LED(0,1,1); // turn red led on
         }
         else
         {
-            ledred.write(1);
-            ledgreen.write(1);
-            ledblue.write(1);
+           LED(1,1,1); // turn leds off (both buttons false)
         }
     }
 }
\ No newline at end of file