Diff: main.cpp

Revision:

39:d065ad7a978d

Parent:

38:f1d2d42a4bdc

Child:

40:7418f46a1ac0

--- a/main.cpp	Tue Oct 24 13:17:42 2017 +0000
+++ b/main.cpp	Thu Oct 26 13:29:56 2017 +0000
@@ -1,6 +1,7 @@
 #include <vector>
 #include <numeric>
 #include "mbed.h"
+#include "Matrix.h"
 #include "MODSERIAL.h"
 #include "HIDScope.h"
 #include "QEI.h"
@@ -9,8 +10,7 @@
 #include "controller.h"
 #include "motorConfig.h"
 #include "errorFetch.h"
-#include "biquadFilter.h"
-#include "biquadChain.h"
+#include "BiQuad.h"
 
 // ADJUSTABLE PARAMETERS
 // controller ticker time interval
@@ -24,13 +24,15 @@
 
 
 // high pass
-biquadFilter HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770);
-biquadFilter HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904);
-biquadChain HPbqc(HPbq1, HPbq2);
+BiQuadChain HPbqc;
+BiQuad HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770);
+BiQuad HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904);
+
+
 // low pass
-biquadFilter LPbq1(1.0e-5*1.3294, 1.0e-5*2.6587, 1.0e-5*1.3294, 1.0000, -1.7783, 0.7924);
-biquadFilter LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.8934, 0.9085);
-biquadChain LPbqc(LPbq1, LPbq2);
+BiQuadChain LPbqc;
+BiQuad LPbq1(1.0e-5*1.3294, 1.0e-5*2.6587, 1.0e-5*1.3294, 1.0000, -1.7783, 0.7924);
+BiQuad LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.8934, 0.9085);
 
 // Controller parameters
 const float k_p = 1;
@@ -83,19 +85,23 @@
 volatile float z; 
 volatile float q;
 volatile float k;
+volatile float w;
 
-void sendDataToPc(float data1, float data2, float data3, float data4, float data5){
+
+void sendDataToPc(float data1, float data2, float data3, float data4, float data5, float data6){
         // Capture data
         x = data1;
         y = data2;
         z = data3;
         q = data4;
         k = data5;
+        w = data6;
         scope.set(0, x);   
         scope.set(1, y); 
         scope.set(2, z);
         scope.set(3, q);
         scope.set(4, z);
+        scope.set(5, w);
         scope.send(); // send what's in scope memory to PC
 }
 
@@ -129,8 +135,14 @@
     float r2_uf = ref2.getReference();
     pc.printf("In controller ticker \r\n");
     
-    float r1 = r1_uf;
-    float r2 = r2_uf;
+    // Filter reference
+    float r1 = HPbqc.step(r1_uf);
+    r1 = fabs(r1);
+    r1 = LPbqc.step(r1);
+
+    float r2 = HPbqc.step(r2_uf);
+    r2 = fabs(r2);
+    r2 = LPbqc.step(r2);
     
     // Finite state machine
     switch(currentState){
@@ -155,7 +167,7 @@
                 e1.fetchError(m1counts, r1);
                 e2.fetchError(m2counts, r2);
 
-                sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, 0.0); // just send the EMG signal value to HIDscope
+                sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, 0.0,0.0); // just send the EMG signal value to HIDscope
                 
                 break;
                 }
@@ -164,16 +176,6 @@
                 if(stateChanged){
                     pc.printf("Active state \r\n");
                     }
-                // Filter reference
-                float r1 = HPbqc.applyFilter(r1_uf);
-                r1 = fabs(r1);
-                r1 = LPbqc.applyFilter(r1);
-
-                float r2 = HPbqc.applyFilter(r2_uf);
-                r2 = fabs(r2);
-                r2 = LPbqc.applyFilter(r2);
-
-                
                 
                 // Compute error
                 e1.fetchError(m1counts, r1);
@@ -186,7 +188,7 @@
                 motor2.setMotor(motorValue2);
                 
                 // Send data to HIDscope
-                sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, 0.0);
+                sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, motorValue1, motorValue2);
                 
                 // Set LED to blue
                 ledR = 1;
@@ -241,7 +243,7 @@
                 ledR = 1;
                 ledG = 0;
                 ledB = 1;
-                sendDataToPc(r1_uf, r2_uf, 0.0, 0.0, 0.0);
+                sendDataToPc(r1_uf, r2_uf, 0.0, 0.0, 0.0, 0.0);
                 break;
                 }
         }
@@ -279,6 +281,9 @@
     {
     pc.baud(115200);
     pc.printf("Main function");
+    HPbqc.add(&HPbq1).add(&HPbq2);
+    LPbqc.add(&LPbq1).add(&LPbq2);
+    
     // Attaching state change functions to buttons;
     sw2.fall(&killSwitch);
     sw3.fall(&activateRobot);