Diff: main.cpp

Revision:

8:5896424eb539

Parent:

7:1906d404cd1e

--- a/main.cpp	Tue Nov 06 10:32:00 2018 +0000
+++ b/main.cpp	Tue Nov 06 14:45:19 2018 +0000
@@ -5,7 +5,7 @@
 #include <iostream>
 #include <string>
 
-enum States {WaitState, MotorCalState, EMGCalState, HomingState, DemoState ,MovingState, GrippingState, ScrewingState, FailureState};
+enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState};
 States currentState = WaitState;
 
 DigitalIn startButton(D0);
@@ -22,7 +22,6 @@
 DigitalOut led3(LED3); // Blue led
 QEI encoder1(D10, D11, NC, 8400, QEI::X4_ENCODING);
 QEI encoder2(D12, D13, NC, 8400, QEI::X4_ENCODING);
-//QEI encoder3(A4, A5), NC, 4200);
 
 AnalogIn emg0(A0);
 AnalogIn emg1(A1);
@@ -33,9 +32,10 @@
 
 Ticker mainTicker;
 Timer stateTimer;
+Ticker calTimer; //Ticker that waits (to prepare the person)
+Ticker EMGsampler; //Ticker that samples the EMG
 
 const double sampleTime = 0.001;
-const float maxVelocity=8.4; // in rad/s
 const double PI = 3.141592653589793238463;
 const double  L1 = 0.328;
 const double L2 = 0.218;
@@ -60,16 +60,16 @@
     {1}
 };
 
-double u1;
-double u2; // u1 is motor output of the long link, u2 is motor of the short link, u3 is motor of gripper, u4 is motor of screwer
-double u3;
-double u4;
-FastPWM pwmpin1(D5); //motor pwm
-DigitalOut directionpin1(D4); // motor direction
+double u1; // Motor output of the long link
+double u2; // Motor of the short link
+double u3; // Motor of the gripper
+double u4; // Motor of the screwer
+FastPWM pwmpin1(D5); // Motor pwm
+DigitalOut directionpin1(D4); // Motor direction
 FastPWM pwmpin2 (D6);
 DigitalOut directionpin2 (D7);
-FastPWM pwmpin3(A4); //motor pwm
-DigitalOut directionpin3(D8); // motor direction
+FastPWM pwmpin3(A4);
+DigitalOut directionpin3(D8);
 FastPWM pwmpin4(A5);
 DigitalOut directionpin4(D9);
 
@@ -80,13 +80,6 @@
 double qMeas1;
 double qMeas2;
 
-double v; // Global variable for printf
-double Dpulses; // Global variable for printf
-double error1; // Global variable for printf
-double error2; // Global variable for printf
-double pulses1; // Global varaible for printf
-double pulses2; // Global varaible for printf
-
 double C[5][5];
 
 double Kp1 = 10;
@@ -96,15 +89,28 @@
 double Ki2 = 0;
 double Kd2 = 2;
 
-const int samplepack = 250; //Amount of data points before one cycle completes
-volatile int counter = 0; //Counts the amount of readings this cycle
-volatile double total[4] = {0,0,0,0}; //Total difference between data points and the calibration value this cycle (x+, x-, y+, y-)
-double refvaluebic = 10; //Minimum total value for the motor to move (biceps)
-double refvaluecalf = 50; //Minimum total value for the motor to move (calfs)
-double incr = 0.00002; //increment
+const int samplepack = 250; // Amount of data points before one cycle completes
+volatile int counter = 0; // Counts the amount of readings this cycle
+volatile double total[4] = {0,0,0,0}; // Total difference between data points and the calibration value this cycle (x+, x-, y+, y-)
+double refvaluebic = 10; // Minimum total value for the motor to move (biceps)
+double refvaluecalf = 50; // Minimum total value for the motor to move (calfs)
+double incr = 0.00002; // Increment
 bool moving[4] = {0,0,0,0}; //x+, x-, y+, y-
 
-BiQuad hpf(0.8635,-1.7269,0.8635,-1.9359,0.9394); //5Hz High pass filter
+volatile int waitingcounter = 0; //How many iterations of waiting have been done
+int countermax = 20; //counter until when should be waited
+volatile int processnow = 0; //Point in the calibration process
+volatile double armresttot = 0;
+volatile double armflextot = 0;
+volatile double legresttot = 0;
+volatile double legflextot = 0;
+const int samplecal = 2500; //Amount of data points to consider in the calibration
+double ratio = samplecal/samplepack;
+volatile double tot = 0; //Total measured EMG of the current type.
+volatile double armcal; //Calibated value for arm measurements
+volatile double legcal; //Calibrated value for leg measurements
+
+BiQuad hpf(0.8635,-1.7269,0.8635,-1.9359,0.9394); //5 Hz High pass filter
 BiQuad notch(0.9922,-1.6054,0.9922,-1.6054,0.9844); //50 Hz Notch filter
 BiQuad lpf(0.9780,1.9561,0.9780,1.9556,0.9565); //250Hz Low pass filter
 BiQuadChain bqc;
@@ -120,11 +126,6 @@
     stateChanged = true;
 }
 
-
-
-
-
-
 void measureEMG ()
 {
     if (currentState == MovingState) {
@@ -152,8 +153,8 @@
             }
             pc.printf("Totals of {x+,x-,y+,y-} are %f, %f, %f, %f \r\n",total[0],total[1],total[2],total[3]);
             pc.printf("Coordinates: (%f,%f)\r\n", setPointX, setPointY);
-            counter = 0; //Reset for next cycle
-            for (int i=0; i<4; i++) { //clear 'total' array
+            counter = 0; // Reset for next cycle
+            for (int i=0; i<4; i++) { // Clear 'total' array
                 total[i] = 0;
             }
         }
@@ -186,40 +187,28 @@
                 currentPulses = encoder1.getPulses();
                 break;
             case 2:
-                //currentPulses = encoder2.getPulses();
-                break;
-            case 3:
-                //currentPulses = encoder3.getPulses();
+                currentPulses = encoder2.getPulses();
                 break;
         }
 
         double deltaPulses = currentPulses - lastPulses;
-        Dpulses = deltaPulses;
         velocity = deltaPulses / (sampleTime * 10); // Velocity in pulses/s
         lastPulses = currentPulses;
         i = 0;
     } else {
         i += 1;
     }
-    v = velocity;
     return velocity;
 }
 
 void measurePosition() // Measure actual angle position with the encoder
 {
-    pulses1 = encoder2.getPulses();   // motor 1 is on M2 and ENC2 of biorobotics shield
-    pulses2 = encoder1.getPulses();   // motor 2 is on M1 and ENC1 of biorobotics shield
-    qMeas1 = (pulses1) * 2 * PI / 8400  +140.7*PI/180 ; // Calculate the angle relative to the starting point (8400 pulses per revolution) + offset
-    qMeas2 = (pulses2) * 2 * PI / 8400   -87*PI/180;
-
-}
+    double pulses1 = encoder2.getPulses();   // motor 1 is on M2 and ENC2 of biorobotics shield
+    double pulses2 = encoder1.getPulses();   // motor 2 is on M1 and ENC1 of biorobotics shield
+    // Calculate the angle relative to the '0' point + offset (we have 8400 pulses per revolution)
+    qMeas1 = (pulses1) * 2 * PI / 8400  +140.7 * PI / 180;
+    qMeas2 = (pulses2) * 2 * PI / 8400   -87 * PI / 180;
 
-void getMotorControlSignal ()   // Milestone 1 code, not relevant anymore
-{
-    //double potSignal = pot.read() * 2 - 1; // read pot and scale to motor control signal
-    //pc.printf("motor control signal = %f\n", posampleTimeignal);
-    u1 = 0;
-    u2 = 0;
 }
 
 template<std::size_t N, std::size_t M, std::size_t P>
@@ -248,15 +237,15 @@
 
 void inverseKinematics ()
 {
-    if (currentState == MovingState || currentState == DemoState) {  // Only in the HomingState should the qRef1, qRef2 consistently change
+    if (currentState == MovingState) {  // Only in the MovingState should the qRef1, qRef2 change every sampleTime
 
-        double  Pe_set[3][1] {            // defining setpoint location of end effector
-            {setPointX},             //setting xcoord to pot 1
-            {setPointY},             // setting ycoord to pot 2
+        double  Pe_set[3][1] { // Defining setpoint location of end effector
+            {setPointX},  // Setting xcoord to setPointX
+            {setPointY},  // Setting ycoord to setPointY
             {1}
         };
 
-//Calculating new H matrix
+        // Calculating new H matrix
         double T1e[3][3] {
             {cos(qRef1), -sin(qRef1), 0},
             {sin(qRef1), cos(qRef1), 0},
@@ -269,28 +258,28 @@
         };
 
 
-        mult<3,3,3>(T1e,T20e);  // matrix multiplication
+        mult<3,3,3>(T1e,T20e); // Matrix multiplication
         double H201[3][3] {
             {C[0][0],C[0][1],C[0][2]},
             {C[1][0],C[1][1],C[1][2]},
             {C[2][0],C[2][1],C[2][2]}
         };
 
-        mult<3,3,3>(H201,H200);   // matrix multiplication
+        mult<3,3,3>(H201,H200); // Matrix multiplication
         double H20 [3][3] {
             {C[0][0],C[0][1],C[0][2]},
             {C[1][0],C[1][1],C[1][2]},
             {C[2][0],C[2][1],C[2][2]}
         };
 
-        mult<3,3,1>(H20,Pe2);   // matrix multiplication
+        mult<3,3,1>(H20,Pe2); // Matrix multiplication
         double Pe0[3][1] {
             {C[0][0]},
             {C[1][0]},
             {C[2][0]}
         };
 
-        double pe0x = Pe0[0][0];                 // seperating coordinates of end effector location
+        double pe0x = Pe0[0][0]; // Seperating coordinates of end effector location
         double pe0y = Pe0[1][0];
 
         // Determing the jacobian
@@ -313,10 +302,10 @@
             {T_1[2][0], T_2[2][0]}
         };
 
-//Determing 'Pulling" force to setpoint
+        // Determing 'Pulling" force to setpoint
 
-        double kspring= 0.1;     //virtual stifness of the force
-        double Fs[3][1] {                                    //force vector from end effector to setpoint
+        double kspring= 0.1; // Virtual stifness of the force
+        double Fs[3][1] { // Force vector from end effector to setpoint
             {kspring*Pe_set[0][0] - kspring*Pe0[0][0]},
             {kspring*Pe_set[1][0] - kspring*Pe0[1][0]},
             {kspring*Pe_set[2][0] - kspring*Pe0[2][0]}
@@ -329,7 +318,7 @@
             {Fy}
         };
 
-        double Jt[2][3] {                                    // transposing jacobian matrix
+        double Jt[2][3] { // Transposing jacobian matrix
             {J[0][0], J[1][0], J[2][0]},
             {T_2[0][0], T_2[1][0], T_2[2][0]}
         };
@@ -338,23 +327,22 @@
         double tau_st1 = C[0][0];
         double tau_st2 = C[1][0];
 
-//Calculating joint behaviour
+        // Calculating joint behaviour
 
         double b1 =1;
         double b2 =1;
-        //joint friction coefficent
-        //double sampleTime = 1/1000;               //Time step to reach the new angle
-        double w_s1 = tau_st1/b1;          // angular velocity
-        double w_s2 = tau_st2/b2;          // angular velocity
-        //checking angle boundaries
-        qRef1 = qRef1 +w_s1*1;         // calculating new angle of qRef1 in time step sampleTime
+        // Joint friction coefficent
+        double w_s1 = tau_st1/b1; // Angular velocity
+        double w_s2 = tau_st2/b2; // Angular velocity
+        // Checking angle boundaries
+        qRef1 = qRef1 +w_s1*1; // Calculating new angle of qRef1 in time step sampleTime
         if (qRef1 > 2*PI/3) {
             qRef1 = 2*PI/3;
         } else if (qRef1 < PI/6) {
             qRef1= PI/6;
         }
 
-        qRef2 = qRef2 + w_s2*1;        // calculating new angle of qRef2 in time step sampleTime
+        qRef2 = qRef2 + w_s2*1; // Calculating new angle of qRef2 in time step sampleTime
         if (qRef2 > -PI/4) {
             qRef2 = -PI/4;
         } else if (qRef2 < -PI/2) {
@@ -363,19 +351,17 @@
     }
 }
 void PID_controller() // Put the error trough PID control to make output 'u'
-
 {
     if (currentState >= HomingState && currentState < FailureState) {
         // Should only work when we move the robot to a defined position
-        error1 = qRef1 - qMeas1;
-        error2 = qRef2 - qMeas2;
+        double error1 = qRef1 - qMeas1;
+        double error2 = qRef2 - qMeas2;
 
         static double errorIntegral1 = 0;
         static double errorIntegral2 = 0;
         static double errorPrev1 = error1;
         static double errorPrev2 = error2;
         static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-        //Ki = pot2.read() * 0.5; //Only Kd is controlled by a pot, Kp and Ki are constant
 
         // Proportional part:
         double u_k1 = Kp1 * error1;
@@ -397,11 +383,9 @@
         double u_d2 = Kd2 * filteredErrorDerivative2;
         errorPrev2 = error2;
 
-        // Sum all parsampleTime
+        // Sum all parts
         u1 = u_k1 + u_i1 + u_d1;
         u2 = u_k2 + u_i2 + u_d2;
-
-
     }
 }
 
@@ -417,15 +401,62 @@
     pwmpin4 = fabs(u4);
 }
 
+void EMGwaiting(){ //Flashes LED
+    led3 = !led3;
+    waitingcounter++;   
+}
+ 
+void EMGsampling(){ //Ticker function of EMG
+    switch(processnow){
+        case 0:
+        case 1: 
+            tot += abs(bqc.step(emg0.read()));
+            break;
+        case 2:
+        case 3:
+            tot += abs(bqc.step(emg2.read()));
+            break;
+    }
+    counter++;
+    if (counter >= samplecal){
+        tot = tot/ratio;
+        switch(processnow){
+            case 0:
+                armresttot = tot;
+                pc.printf("<Result> Average area of arm signal in rest: %f.\r\n",armresttot);
+                break;
+            case 1:
+                armflextot = tot;
+                pc.printf("<Result>Average area of arm signal while flexing: %f.\r\n",armflextot);
+                armcal = (armflextot + armresttot)/2.0;
+                pc.printf("<Result> Calibration value for arms determined at %f.\r\n",armcal);
+                break;
+            case 2:
+                legresttot = tot;
+                pc.printf("<Result> Average area of leg signal in rest: %f.\r\n",legresttot);
+                break;
+            case 3:
+                legflextot = tot;
+                pc.printf("<Result> Average area of leg signal while flexing: %f.\r\n",legflextot);
+                legcal = (legflextot + legresttot)/2;
+                pc.printf("<Result> Calibration value for legs determined at %f.\r\n",legcal);
+                break;
+        }
+        processnow++;
+        tot = 0;
+        counter = 0;
+    }
+}
+
 void stateMachine ()
 {
     switch (currentState) {
         case WaitState:
             if (stateChanged == true) {
-                led1 = 0;
+                // Everything that needs to be done when this state is entered the first time
+                led1 = 0; // Green
                 led2 = 1;
                 led3 = 1;
-                // Entry action: all the things you do once in this state
                 u1 = 0; // Turn off all motors
                 u2 = 0;
                 u3 = 0;
@@ -434,7 +465,7 @@
             }
 
             if (startButton.read() == false) { // When button is pressed, value is false
-                //pc.printf("Switching to motor calibration");
+                // Everything that needs to be done when leaving this state
                 led1 = 1;
                 currentState = MotorCalState;
                 stateChanged = true;
@@ -443,23 +474,19 @@
             break;
         case MotorCalState:
             if (stateChanged == true) {
-                // Entry action: all the things you do once in this state
-                led2 = 0;
-                // Set motorpwm to 'low' value
-                //u1 = 0.6; //TODO: Check if direction is right
-                //u2 = 0.6;
+                led2 = 0; // Red
+                u1 = 0.3; // Motor is set to 'low' value
+                u2 = -0.3;
                 stateTimer.reset();
                 stateTimer.start();
                 stateChanged = false;
             }
 
-            // Add stuff you do every loop
-            getMotorControlSignal();
-
-            if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && startButton.read() == false && fabs(measureVelocity(2)) < 0.1f) { //TODO: add
-                //TODO: Add reset of encoder2
+            // The robot can only go to the next state after 3s, when the motor velocities are close to 0 and the start button is pressed
+            if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && fabs(measureVelocity(2)) < 100 && startButton.read() == false) {
                 led2 = 1;
                 encoder1.reset(); // Reset encoder for the 0 position
+                encoder2.reset();
                 currentState = EMGCalState;
                 stateChanged = true;
                 u1 = 0; // Turn off motors
@@ -468,17 +495,58 @@
             break;
         case EMGCalState:
             if (stateChanged == true) {
-                // Entry action: all the things you do once in this state;
-                led3 = 0;
-                stateTimer.reset();
-                stateTimer.start();
+                led3 = 0; // Blue
                 stateChanged = false;
+
+                pc.printf("<----------Now entering EMG calibration state ---------->\r\n");
+                pc.printf("<Waiting> Get ready for calibration of resting arms.\r\n");
+                calTimer.attach(&EMGwaiting,0.5); //Wait
+                while (waitingcounter < countermax) {}
+                waitingcounter = 0;
+                calTimer.detach();
+
+                pc.printf("<Measuring> Measuring arm signal at rest. Hold still.\r\n");
+                EMGsampler.attach(&EMGsampling,sampleTime); //Measure arms rest
+                while (processnow == 0) {}
+                EMGsampler.detach();
+
+                pc.printf("<Waiting> Get ready for calibration of flexing arms.\r\n");
+                calTimer.attach(&EMGwaiting,0.5); // Wait
+                while (waitingcounter < countermax) {}
+                waitingcounter = 0;
+                calTimer.detach();
+
+                pc.printf("<Measuring> Measuring arm signal while flexing.\r\n");
+                EMGsampler.attach(&EMGsampling,sampleTime); //Measure arms flexing
+                while (processnow == 1) {}
+                EMGsampler.detach();
+
+                pc.printf("<Waiting> Get ready for calibration of resting legs.\r\n");
+                calTimer.attach(&EMGwaiting,0.5); //Wait
+                while (waitingcounter < countermax) {}
+                waitingcounter = 0;
+                calTimer.detach();
+
+                pc.printf("<Measuring> Measuring leg signal at rest. Hold still.\r\n");
+                EMGsampler.attach(&EMGsampling,sampleTime); //Measure legs rest
+                while (processnow == 2) {}
+                EMGsampler.detach();
+
+                pc.printf("<Waiting> Get ready for calibration of flexing legs.\r\n");
+                calTimer.attach(&EMGwaiting,0.5); // Wait
+                while (waitingcounter < countermax) {}
+                waitingcounter = 0;
+                calTimer.detach();
+
+                pc.printf("<Measuring> Measuring leg signal while flexing.\r\n");
+                EMGsampler.attach(&EMGsampling,sampleTime); //Measure legs flexing
+                while (processnow == 3) {}
+                EMGsampler.detach();
+
+                pc.printf("<Result> EMG calibrations complete.\r\n");
             }
 
-            // Add stuff you do every loop
-
             if (stateTimer >= 3.0f) {
-                //pc.printf("Starting homing...\n");
                 led3 = 1;
                 currentState = HomingState;
                 stateChanged = true;
@@ -486,22 +554,14 @@
             break;
         case HomingState:
             if (stateChanged == true) {
-                // Entry action: all the things you do once in this state;
-                led1 = 0;
-                led2 = 0; // EmisampleTime yellow together
-                //TODO: Set qRef1 and qRef2
+                led1 = 0; // Yellow
+                led2 = 0;
+                // qRef1 and qRef2 are set, so the motors will move to that position
                 qRef1 = 60 * PI / 180;
-                qRef2 = -90 *PI/180;
+                qRef2 = -90 * PI / 180;
                 stateChanged = false;
             }
 
-            // Nothing extra happens till robot reaches starting position and button is pressed
-            if (gripperButton.read() == false) { //TODO: Also add position condition
-                led1 = 1;
-                led2 = 1;
-                currentState = DemoState;
-                stateChanged = true;
-            }
             if (startButton.read() == false) { //TODO: Also add position condition
                 led1 = 1;
                 led2 = 1;
@@ -509,37 +569,11 @@
                 stateChanged = true;
             }
             break;
-        case DemoState:
-            if (stateChanged == true) {
-                stateTimer.reset();
-                setPointX = 0.15;
-                setPointY = 0.3;
-                stateTimer.start();
-                stateChanged = false;
-            }
-            static double blinkTimer = 0;
-            if (blinkTimer >= 0.5) {
-                led2 = !led2;
-                blinkTimer = 0;
-            }
-            blinkTimer += sampleTime;
-            if (stateTimer >= 5.0) {
-                setPointX = -0.1;
-                setPointY = 0.3;
-            }
-            
-            if (stateTimer >= 20.0) {
-                stateTimer.reset();
-                currentState = HomingState;
-                stateChanged = true;
-                }
-            break;
         case MovingState:
             if (stateChanged == true) {
-                // Entry action: all the things you do once in this state;
-                led1 = 0;
+                led1 = 0; // White
                 led2 = 0;
-                led3 = 0; // EmisampleTime white together
+                led3 = 0;
                 stateChanged = false;
             }
 
@@ -554,16 +588,15 @@
             break;
         case GrippingState:
             if (stateChanged == true) {
-                // Entry action: all the things you do once in this state;
-                led2 = 0;
-                led3 = 0; // EmisampleTime light blue together
+                led2 = 0; // Light blue
+                led3 = 0;
                 stateTimer.reset();
                 stateTimer.start();
                 stateChanged = false;
             }
 
             if (gripperMotorButton.read() == false) {
-                pc.printf("Button pressed \r\n");
+                // Gripper motor is activated
                 u3 = 0.4;
                 if (directionSwitch == true) {
                     // Close gripper, so positive direction
@@ -575,6 +608,7 @@
                 u3 = 0;
             }
 
+            // Due to the lack of buttons, we use a delay, so that when we don't immediately go to the next state when we enter this state
             if (gripperButton.read() == false && stateTimer >= 2.0f) {
                 led2 = 1;
                 led3 = 1;
@@ -593,14 +627,14 @@
         case ScrewingState:
             if (stateChanged == true) {
                 // Entry action: all the things you do once in this state;
-                led1 = 0;
-                led3 = 0; // EmisampleTime pink together
+                led1 = 0; // Pink
+                led3 = 0;
                 stateChanged = false;
             }
 
             if (gripperMotorButton.read() == false) {
-                u4 = 0.3;
-                u3 = -0.35;
+                u4 = 0.3; // Screwing motor is activated
+                u3 = -0.35; // Gripper motor is activated with a slightly higher output to make sure the gripper doesn't open
                 if (directionSwitch == true) {
                     // Screw
                 } else {
@@ -608,7 +642,7 @@
                     u4 = u4 * -1;
                     u3 = u3 * -1;
                 }
-            } else {
+            } else { // If the button isn't pressed, turn off motors
                 u4 = 0;
                 u3 = 0;
             }
@@ -616,7 +650,7 @@
             if (startButton.read() == false) {
                 led1 = 1;
                 led3 = 1;
-                u3 = 0;
+                u3 = 0; // Turn off motors
                 u4 = 0;
                 currentState = MovingState;
                 stateChanged = true;
@@ -633,12 +667,11 @@
             }
 
             static double blinkTimer2 = 0;
-            if (blinkTimer2 >= 0.5) {
+            if (blinkTimer2 >= 0.5) { // The red LED is flashing at 1 Hz
                 led1 = !led1;
                 blinkTimer2 = 0;
             }
             blinkTimer2 += sampleTime;
-
             break;
     }
 }
@@ -650,9 +683,8 @@
     inverseKinematics();
 }
 
-void mainLoop ()
+void mainLoop () // All the major functions are looped every 0.001s
 {
-    // Add measure, motor controller and output function
     measureAll();
     stateMachine();
     PID_controller();
@@ -661,25 +693,16 @@
 
 int main()
 {
-    startButton.mode(PullUp);
+    startButton.mode(PullUp); // Makes the button active
     failureButton.mode(PullUp);
     gripperButton.mode(PullUp);
     directionSwitch.mode(PullUp);
     gripperMotorButton.mode(PullUp);
     pc.baud(115200);
-    pc.printf("checkpoint 1\n");
-    //pwmpin1.period(sampleTime);
-    //pwmpin2.period(sampleTime);
-    bqc.add(&hpf).add(&notch).add(&lpf); //Add bqfilters to bqc
+    bqc.add(&hpf).add(&notch).add(&lpf); // Add bqfilters to bqc
     mainTicker.attach(mainLoop, sampleTime);
     failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated
 
     while (true) {
-        //pc.printf("State = %i\n", currentState);
-        //int pulses = encoder1.getPulses();
-        //pc.printf("pulses = %i\n", pulses);
-        // pc.printf("v = %f\n", v);
-        pc.printf("Error1 = %f Error2 = %f \n qRef1 = %f rQref2 = %f \r\n qMeas1 = %f qMeas2 = %f \n, Pulses1 = %f Pulses2 = %f \n", error1,error2,qRef1,qRef2,qMeas1,qMeas2, pulses1, pulses2);
-        wait(2);
     }
 }
\ No newline at end of file