File content as of revision 2:bc6043623fb7:

#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
#include "BiQuad.h"

MODSERIAL pc(USBTX, USBRX);
DigitalOut DirectionPin1(D4);
DigitalOut DirectionPin2(D7);
PwmOut PwmPin1(D5);
PwmOut PwmPin2(D6);
DigitalIn Knop1(D2);
DigitalIn Knop2(D3);
DigitalIn Knop3(PTA4);
DigitalIn Knop4(PTC6);
AnalogIn pot1 (A5);
AnalogIn pot2 (A4);
AnalogIn emg0( A0 );
AnalogIn emg1( A1 );
AnalogIn emg2( A2 );
AnalogIn emg3( A3 );
DigitalOut led_G(LED_GREEN);
DigitalOut led_R(LED_RED);
DigitalOut led_B(LED_BLUE);

QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);

BiQuadChain bqc1;
BiQuadChain bqc2;
BiQuadChain bqc3;
BiQuadChain bqc4;
BiQuadChain bqc5;
BiQuadChain bqc6;
BiQuadChain bqc7;
BiQuadChain bqc8;

BiQuad      BqNotch1_1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01  );
BiQuad      BqNotch2_1(  1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
BiQuad      BqNotch1_2( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01  );
BiQuad      BqNotch2_2(  1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
BiQuad      BqNotch1_3( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01  );
BiQuad      BqNotch2_3(  1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
BiQuad      BqNotch1_4( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01  );
BiQuad      BqNotch2_4(  1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
BiQuad      BqHP1( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
BiQuad      BqHP2( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
BiQuad      BqHP3( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
BiQuad      BqHP4( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );

BiQuad      BqLP1( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
BiQuad      BqLP2( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
BiQuad      BqLP3( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
BiQuad      BqLP4( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );

BiQuad      LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

Ticker      StateTicker;
Ticker      printTicker;

HIDScope    scope( 4 );

volatile float      Bicep_Right          = 0.0;
volatile float      Bicep_Left           = 0.0;
volatile float      Tricep_Right         = 0.0;
volatile float      Tricep_Left          = 0.0;
volatile float      FilterHP_Bi_R;
volatile float      Filterabs_Bi_R;
volatile float      Filtered_Bi_R;
volatile float      FilterHP_Bi_L;
volatile float      Filterabs_Bi_L;
volatile float      Filtered_Bi_L;
volatile float      FilterHP_Tri_R;
volatile float      Filterabs_Tri_R;
volatile float      Filtered_Tri_R;
volatile float      FilterHP_Tri_L;
volatile float      Filterabs_Tri_L;
volatile float      Filtered_Tri_L;

volatile float error_1_integral = 0;
volatile float error_2_integral = 0;
volatile float error_1_prev; // initialization with this value only done once!
volatile float error_2_prev;

volatile const float pi             = 3.1415926; 
volatile const float rad_count      = 0.0007479;                                // 2pi/8400;
volatile const float maxVelocity    = 2.0f * pi;                                // in rad/s
volatile const float r_3            = 0.035;                                

volatile float referenceVelocity1   = 0.5;                                      // dit is de gecentreerde waarde en dus de nulstand
volatile float referenceVelocity2   = 0.5;

volatile int i   = 0;                                                           // Led blink status
volatile int ii  = 0;                                                           // Calibratie timer
volatile int iii = 0;                                                           // Start up timer

volatile float Kp = 17.5;
volatile float Ki = 1.02;
volatile float Kd = 23.2;
volatile float Ts = 0.01; // Sample time in seconds
volatile float error_1;
volatile float error_2;
volatile float U_1;
volatile float U_2; 

volatile float q_1;
volatile float q_2;
volatile float r_1;
volatile float r_2;
volatile float w_1;
volatile float w_2;

volatile float Flex_Bi_R;
volatile float Flex_Bi_L;
volatile float Flex_Tri_R;
volatile float Flex_Tri_L;
volatile float Threshold_Value;
volatile float Threshold_Bi_R;
volatile float Threshold_Bi_L;
volatile float Threshold_Tri_R;
volatile float Threshold_Tri_L;
volatile bool  Checking_Bi_R        = false;
volatile bool  Checking_Bi_L        = false;
volatile bool  Checking_Tri_R       = false;
volatile bool  Checking_Tri_L       = false;

enum states{Starting, Calibration, Homing_M1, Homing_M2, Post_Homing, Function, Safe};
    
volatile states Active_State = Starting;

volatile float vx;
volatile float vy;
volatile int counts1;
volatile int counts2;
volatile float rad_m1;
volatile float rad_m2;
    
void Encoding()
{
 
    counts1 = Encoder1.getPulses();
    counts2 = Encoder2.getPulses();
    
    rad_m1 = rad_count * (float)counts1;
    rad_m2 = rad_count * (float)counts2;
}

void Filter() 
{
    FilterHP_Bi_R = bqc1.step( emg0.read() );
    Filterabs_Bi_R = fabs(FilterHP_Bi_R); 
    Filtered_Bi_R = bqc2.step( Filterabs_Bi_R );
    
    FilterHP_Bi_L = bqc3.step( emg1.read() );
    Filterabs_Bi_L = fabs(FilterHP_Bi_L);
    Filtered_Bi_L = bqc4.step( Filterabs_Bi_L );
    
    FilterHP_Tri_R = bqc5.step( emg2.read() );
    Filterabs_Tri_R = fabs(FilterHP_Tri_R); 
    Filtered_Tri_R = bqc6.step( Filterabs_Tri_R );
    
    FilterHP_Tri_L = bqc7.step( emg3.read() );
    Filterabs_Tri_L = fabs(FilterHP_Tri_L);
    Filtered_Tri_L = bqc8.step( Filterabs_Tri_L );
}

void BlinkLed()
{
    if(i==1)
    {
            led_G=led_B=1;
            static int rr = 0;
            rr++;
            if (rr == 1)
            {
                led_R = !led_R;
            }
            else if (rr == 50)
            {
                rr = 0;
            }
    }     
    else if(i==2)
    {        
            led_R=led_B=1;
            
            static int gg = 0;
            gg++;
            if (gg == 1)
            {
                led_G = !led_G;
            }
            else if (gg == 250)
            { 
                gg = 0;
            }
    }
    else if (i==3)
    {            
            led_R=1;
            static int bb = 0;
            bb++;
            if (bb == 1)
            {
                led_B = !led_B;
            }
            else if (bb == 500)
            {
                bb = 0;
            }
    }
    else if (i==4)
    {
            led_G=1;
            static int rr = 0;
            rr++;
            if (rr == 1)
            {
                led_R = !led_R;
                led_B = !led_B;
            }
            else if (rr == 250)
            {
                rr = 0;
            }
    }
    else 
    {
        led_R=led_G=led_B=1;
    }  
}

void EMG_Read()
{
    Bicep_Right     =   emg0.read();
    Bicep_Left      =   emg1.read();
    Tricep_Right    =   emg2.read();
    Tricep_Left     =   emg3.read();   
}

void sample()
{
    
    scope.set(0, Filtered_Bi_R*10.0f );
    scope.set(1, Filtered_Bi_L*10.0f );
    scope.set(2, Filtered_Tri_R*10.0f );
    scope.set(3, Filtered_Tri_L*10.0f );
  
    scope.send();
}

void Inverse()
{
    q_1= rad_m1+(pi/6.0f);                                                       // uit Encoder
    q_2= rad_m2+(pi/6.0f);                                                      // uit Encoder
    r_1= -0.2f;
    r_2= -0.2f;
    
        float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2);
        float z = 2.0f*(r_2*cos(q_1)*cos(q_2))-r_3;
        float y = r_2*cos(q_1)*cos(q_2)-r_2*sin(q_1)*sin(q_2)+2.0f*(r_1*cos(q_1))-r_3;
        float x = (-2.0f)*r_2*sin(q_1)*cos(q_2);
        float D = 1.0f/(u*z-x*y);                                               // Determinant
        //printf("Determinant is %f\r\n", D);
        
        float a = D*z;                                                          // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
        float b = -D*x;                                                         // Inverse jacobian
        float c = -D*y;                                                         // Inverse jacobian
        float d = D*u;                                                          // Inverse jacobian
        
        vx = pot1.read()/5.0f;                                                 // uit emg data
        vy = pot2.read()/5.0f;                                                 // uit emg data
        
        w_1 = vx*a+vy*b;
        w_2 = vx*c+vy*d;
}
  
void Calibrating()
{
    static float n = 0.0;
    static float m = 0.0;
    static float l = 0.0;
    static float k = 0.0;
    
    ii++;
    
            if (ii<=2500)
            {
                if (ii == 1)
                {
                    pc.printf("Relax your muscles please. \r\n");
                    i = 2;
                }
                else if (ii == 1750)
                {
                    pc.printf("Flex your right bicep now please.\r\n");
                    i = 3; 
                }
                                                                                //chillen
            }
            else if (ii>2500 && ii<5000) //
            {
                n = n + Filtered_Bi_R;                                            // dit wordt de variable naam na het filter
                i = 1;
            }
            else if (ii == 5000)
            {
                Flex_Bi_R = n / 2500.0f;
                pc.printf("You can relax your right bicep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Bi_R);
                i = 2;
            }
            else if (ii>5000 && ii<=6000)
            {
                if (ii == 5500)
                {
                    pc.printf("Flex your left bicep now please. \r\n");
                    i = 3;
                }
                //chillen
            }
            else if(ii>6000 && ii<8500)
            {
                m = m + Filtered_Bi_L;
                i = 1;
            }
            else if (ii == 8500)
            {
                Flex_Bi_L = m / 2500.0f;
                pc.printf("You can relax your left bicep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Bi_L);
                i = 2;
            }
            else if (ii>8500 && ii<=9500)
            {
                if (ii == 9000)
                {
                    pc.printf("Flex your right tricep now please. \r\n");
                    i = 3;
                }
                //chillen
            }
            else if (ii>9500 && ii<12000)
            {
                l = l + Filtered_Tri_R;
                i = 1;
            }
            else if (ii == 12000)
            {
                Flex_Tri_R = l / 2500.0f;
                pc.printf("You can relax your right tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Tri_R);
                i = 2;
            }
            else if (ii>12000 && ii <=13000)
            {
                if (ii == 12500)
                {
                    pc.printf("Flex your left tricep now please. \r\n");
                    i = 3; 
                }
                //chillen
            }
            else if (ii>13000 && ii<15500)
            {
                k = k + Filtered_Tri_L;
                i = 1;
            }
            else if (ii == 15500)
            {
                Flex_Tri_L = k / 2500.0f;
                pc.printf("You can relax your left tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\nThe calibration has been completed, the system is now operatable. \r\n",Flex_Tri_L);
                i = 2;
            }

Threshold_Value = 0.8f;
            
Threshold_Bi_R  = Threshold_Value * Flex_Bi_R;
Threshold_Bi_L  = Threshold_Value * Flex_Bi_L;
Threshold_Tri_R = Threshold_Value * Flex_Tri_R;
Threshold_Tri_L = Threshold_Value * Flex_Tri_L;                  

            if (ii == 16500)
            {
                pc.printf("\r\nThreshold value right bicep  = %f\r\nThreshold value left bicep   = %f\r\nThreshold value right tricep = %f\r\nThreshold value left tricep  = %f\r\n\r\n",Threshold_Bi_R,Threshold_Bi_L,Threshold_Tri_R,Threshold_Tri_L);
            }
            else if (ii == 20000)
            {
                pc.printf("\r\nAutomatic switch to Homing State\r\n");
                Active_State = Homing_M1;
                i = 0;
            }
}



void Start_Up()
{
    i++;
    iii++;
    if (iii == 1)
    {
        pc.printf("\r\n\r\nSystem is starting...\r\nWaiting for further input...\r\n");
    }
    
    else if (iii == 30000)
    {
        pc.printf("1 minute without input..\r\nReseting start-up...\r\n");
        iii = 0;
    }    
    else if (iii == 40001)                                                      // sleeping state is only added for designing purposes and will most likely never be used
    {                                                                           // when working with patients. Furthermore it cannot be reached automaticly
        pc.printf("Sleeping... Press button 4 to wake me up!\r\n\r\n");
        iii++;
    }
    else if (iii == 45000)
    {
        iii = 40000;
    }
}

void OFF_m1()
{
    PwmPin1 = 0;
}
void OFF_m2()
{
    PwmPin2 = 0;
}

void Going_Home_Motor1()
{
    if (counts1 == 0)
    {
        pc.printf("Switching to Homing state for motor 2\r\n");
        Active_State = Homing_M2;                                               // Statement here instead of statemachine because of checking speed
    }
    else if (counts1 > 0)
    { 
        PwmPin1 = 0.8f;
        DirectionPin1 = false;
    }
    else
    {
        PwmPin1 = 0.8f;
        DirectionPin1 = true;
    }    
}

void Going_Home_Motor2()
{
    pc.printf("Entered homing state 2"); 
    if (counts2 == 0)
    { 
        // als het goed is hoeft hier niets te staan                            // Statement in statemachine because of the double check
    }
    else if (counts2 > 0)
    {
        PwmPin2 = 0.8f;
        DirectionPin2 = true;
    }
    else
    {
        PwmPin2 = 0.8f;
        DirectionPin2 = false;
    }
}

void Checking_EMG()
{
    // This function will make the the setting of signal to movement easier
    
    if (Filtered_Bi_R >= Threshold_Bi_R)
    {
                Checking_Bi_R               = true;
    }
    if (Filtered_Bi_L >= Threshold_Bi_L)
    {
                Checking_Bi_L               = true;
    }   
    if (Filtered_Tri_R >= Threshold_Tri_R)
    {
                Checking_Tri_R              = true;
    }   
    if (Filtered_Tri_L >= Threshold_Tri_L)
    {
                Checking_Tri_L             = true;
    }   
}

void Setting_Movement()
{
    // Here we will set the emg values to the movement of the arm
    
    if (Checking_Bi_R && Checking_Bi_L)                                         // sloping y =  x,  y > 0
    {
        vx = 0.5f;
        vy = 0.5f;
    }
    else if (Checking_Bi_R && Checking_Tri_L)                                   // sloping y = -x,  y > 0
    {
        vx = -0.5f;
        vy =  0.5f;
    }
    else if (Checking_Bi_L && Checking_Tri_R)                                   // sloping y = -x,  y < 0
    {
        vx =  0.5f;
        vy = -0.5f;
    }
    else if (Checking_Tri_R && Checking_Tri_L)                                  // sloping y =  x,  y < 0
    {
        vx = -0.5f;
        vy = -0.5f;
    }
    else if (Checking_Bi_R)                                                     // y > 0
    {
        vx =  0.0f;
        vy =  0.5f;
    }
    else if (Checking_Bi_L)                                                     // x > 0
    {
        vx =  0.5f;
        vy =  0.0f;
    }
    else if (Checking_Tri_R)                                                    // y < 0
    {
        vx =  0.0f;
        vy = -0.5f;
    }
    else if (Checking_Tri_L)                                                    // y < 0
    {
        vx = -0.5f;
        vy =  0.0f;
    }        
}

void PID_controller()
{
      error_1 = (w_1*0.002f) - rad_m1;
      error_2 = (w_2*0.002f) - rad_m2;
      
      error_1_prev = error_1;
      error_2_prev = error_2;
    
      // Proportional part:
      float u_k_1 = Kp * error_1;
      float u_k_2 = Kp * error_2;
    
      // Integral part
      error_1_integral = error_1_integral + error_1 * Ts;
      error_2_integral = error_2_integral + error_2 * Ts;
      float u_i_1 = Ki * error_1_integral;
      float u_i_2 = Ki * error_2_integral;
    
      // Derivative part
      float error_1_derivative = (error_1 - error_1_prev)/Ts;
      float error_2_derivative = (error_2 - error_2_prev)/Ts;
      float filtered_error_1_derivative = LowPassFilter.step(error_1_derivative);
      float filtered_error_2_derivative = LowPassFilter.step(error_2_derivative);
      float u_d_1 = Kd * filtered_error_1_derivative;
      float u_d_2 = Kd * filtered_error_2_derivative;
      error_1_prev = error_1;
      error_2_prev = error_2;
    
      // Sum all parts and return it
      U_1 = u_k_1 + u_i_1 + u_d_1;   
      U_2 = u_k_2 + u_i_2 + u_d_2;
}

void motor1()
{          
        float u = U_1;
        DirectionPin1 = u < 0.0f;
        PwmPin1 = fabs(u);
}

void motor2()
{  
        float u = U_2;
        DirectionPin2 = u > 0.0f;
        PwmPin2 = fabs(u);
}

void Printing()
{
    float v1 = PwmPin1 * maxVelocity;
    float v2 = PwmPin2 * maxVelocity;
    
        if (Active_State == Function || Active_State == Homing_M1)
        {
            pc.printf("q1    = %f [rad] \r\nq2    = %f [rad] \r\ncount1= %i\r\ncount2= %i\r\nq1dot = %f [rad/s] \r\nq2dot = %f [rad/s] \r\n\r\n\r\n\r\n\r\n", rad_m1, rad_m2,counts1, counts2, v1, v2);
        }
        
        pc.printf("%i \r\n%i \r\n\r\n",counts1,counts2);
}

void EMG_test()                                                                 // even nalopen of dit ook kan met i++!
{
    led_G=led_R=led_B=1;
     
    if (Filtered_Bi_R >= Threshold_Bi_R)
    {
        i = 1;
        //led_R = 0;  // rood
    }
    if (Filtered_Bi_L >= Threshold_Bi_L)
    {
        i = 3;
        //led_B = 0;  // blauw
    }
    if (Filtered_Tri_R >= Threshold_Tri_R)
    {
        i = 2;
        //led_G = 0;  // groen
    }
    if (Filtered_Tri_L >= Threshold_Tri_L)
    {
        i = 4;
        //led_B = 0;
        //led_R = 0;  // paars
    }
            
}

void StateMachine() 
{
    switch (Active_State)
    {
        case Starting:   
        Start_Up();
        OFF_m1();
        OFF_m2();
        
        if (!Knop4 == true)
        {
            Active_State = Calibration;
            pc.printf("Entering Calibration State \r\n");
        }
        else if (!Knop3 == true)
        {
            Active_State = Homing_M1;
            pc.printf("Entering Homing State \r\n");
        }
                     
                                   
        break;
        
        case Calibration:
                
                Filter();
                Calibrating();
                OFF_m1();
                OFF_m2();
                BlinkLed();
                
                    if (!Knop1 && !Knop2)
                    {
                        pc.printf("Switched to Sleeping State\r\n");
                        Active_State = Starting;
                        iii = 40001;
                    }
                    else if (Knop1==false)
                    {
                        pc.printf("Manual switch to Homing state \r\n");
                        Active_State = Homing_M1;
                    }

                    
                sample();
                EMG_Read();  
        break;
        
        case Homing_M1:

                Going_Home_Motor1();
                OFF_m2();
                Encoding();
                
                    if (fabs(rad_m1)>(pi/6.0f) || fabs(rad_m2)>(pi/6.0f))          // pi/4 is a safe value, can/will be editted
                    {
                        pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
                        Active_State = Safe;
                    }
                    else if (!Knop1 && !Knop2)
                    {
                        pc.printf("Switched to Sleeping State\r\n");
                        Active_State = Starting;
                        iii = 40000;
                    }
                    else if (Knop2==false)
                    {
                        pc.printf("Manual switch to Funtioning State \r\n");
                        Active_State = Function;
                    }
                    else if (Knop4==false)
                    {
                        Active_State = Calibration;
                        pc.printf("Re-entering Calibration State \r\n");
                    }
                    
        break;
        
        case Homing_M2:
        
                    Going_Home_Motor2();
                    OFF_m1();
                    Encoding();
                
                    if (fabs(rad_m1)>(pi/6.0f) || fabs(rad_m2)>(pi/6.0f))          // pi/4 is a safe value, can/will be editted
                    {
                        pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
                        Active_State = Safe;
                    }
                    else if (counts2 == 0 && counts1 == 0)
                    {
                        Active_State = Post_Homing;
                    }
            
        break;    
        
        case Post_Homing:
        
                static int mm = 0;
                mm++;
                if (mm == 1000);
                {
                    Active_State = Function;
                    pc.printf("Homing was succesfull\r\n\r\nAutomatic switch to Funtioning state\r\n\r\n");
                    mm=0;                                                           // reseting the state
                }
                
                OFF_m1();
                OFF_m2();
        break;
        
        case Function:
                
                EMG_test();                
                Filter();
                Inverse();
                sample();
                EMG_Read();
                Encoding();
                Checking_EMG();
                Setting_Movement();
                PID_controller();
                motor1();
                motor2();
                        
                if (fabs(rad_m1)>(pi/6.0f) || fabs(rad_m2)>(pi/6.0f))          // pi/4 is a safe value, can/will be editted
                {
                    pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
                    Active_State = Safe;
                }
                else if (Knop4==false)
                { 
                    pc.printf("Re-entering Calibration State \r\n");
                    Active_State = Calibration;
                    ii=0;
                }
                else if (Knop3==false)
                {
                    pc.printf("Re-entering Homing State \r\n");
                    Active_State = Homing_M1;
                }
                else if (!Knop1 && !Knop2)
                {
                    pc.printf("Switched to Sleeping State\r\n");
                    Active_State = Starting;
                    iii = 40000;
                }
                                       
        break;    
        
        case Safe:
        
            OFF_m1();
            OFF_m2();
            
        break;
            
        default:
            pc.printf("UNKNOWN COMMAND");
    }
}    

int main()
{
    pc.baud(115200);    
    PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz 
    
    bqc1.add( &BqNotch1_1 ).add( &BqNotch2_1 ).add( &BqHP1 );                   //Oh wat lelijk...
    bqc2.add(&BqLP1);
    bqc3.add( &BqNotch1_2 ).add( &BqNotch2_2 ).add( &BqHP2 );
    bqc4.add(&BqLP2);
    bqc5.add( &BqNotch1_3 ).add( &BqNotch2_3 ).add( &BqHP3 );
    bqc6.add(&BqLP3);
    bqc7.add( &BqNotch1_4 ).add( &BqNotch2_4 ).add( &BqHP4 );
    bqc8.add(&BqLP4);
    
    StateTicker.attach(&StateMachine, 0.002);
   
    printTicker.attach(&Printing, 2);
     
    while(true)
    {
    }
}