Biorobotics 2018: Project group 16
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp
- Committer:
- ThomBMT
- Date:
- 2018-11-01
- Revision:
- 4:a682fb1f37d2
- Parent:
- 3:766e9f13d84e
File content as of revision 4:a682fb1f37d2:
#include "mbed.h" #include "MODSERIAL.h" #include "HIDScope.h" #include "QEI.h" #include "BiQuad.h" // Pin defintions: 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); // Encoder functions. QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING); QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); // Filter definitions: 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); // Tickers for the main loop: Ticker StateTicker; Ticker printTicker; // Setting the HIDscope: HIDScope scope( 4 ); // All variables for EMG-processing: 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; // Variables for the PID-Controller: 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 Kp = 3.2; volatile const float Ki = 0.000; volatile const float Kd = 0.0; volatile const float Ts = 0.002f; // Sample time in seconds volatile float error_1; volatile float error_2; volatile float u_k_1; volatile float u_k_2; volatile float u_i_1; volatile float u_i_2; volatile float u_d_1; volatile float u_d_2; volatile float U_1; volatile float U_2; // Constant values used in computations and such: 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; // Some counting variables: volatile int i = 0; // Led blink status volatile int ii = 0; // Calibratie timer volatile int iii = 0; // Start up timer // Variables for computations of joint angle and velocities: volatile float q_ref1_o = 7.0f*pi/4.0f; volatile float q_ref1_n; volatile float q_ref2_o = 1.0f*pi/3.0f; volatile float q_ref2_n; volatile float r_1; volatile float r_2; volatile float w_1; volatile float w_2; volatile float vx; volatile float vy; volatile float rad_m1; volatile float rad_m2; volatile int counts1; volatile int counts2; // Threshold values and variables: 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; // States for the switch-statemachine: enum states{Starting, Calibration, Homing_M1, Homing_M2, Post_Homing, Function, Safe}; // The very first state: volatile states Active_State = Starting; void Encoding() { // Encoding is necessary for the computations of the joint angles and // velocities of the arm given certain linear velocities. counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); rad_m1 = (rad_count * (float)counts1) + (1.0f*pi/3.0f); rad_m2 = (rad_count * (float)counts2) + (7.0f*pi/4.0f); } void Filter() { // Our Filter function will take the raw (incomming) EMG-signal and process // it in such a way that we can use it to opperate our system. 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() { // This is a function used purely for feedback that will give insight in // what the system is doing without having to fully opperate it. 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() { // This is only to define our first (raw) EMG-signals and give each a // suitable name. Bicep_Right = emg0.read(); Bicep_Left = emg1.read(); Tricep_Right = emg2.read(); Tricep_Left = emg3.read(); } void sample() { // HIDscope allows us to check our filtered EMG-signal and gives insight // into thresholdvalues and how well the calibration was done. 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 Calibrating() { // Calibration is done to ensure that threshold values not predetermined and // can vary each user. This makes the system more universal. 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; } // Setting the Threshold: Threshold_Value = 0.85f; 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 for Motor 1\r\n"); Active_State = Function; i = 0; } } void Start_Up() { // This function is active only when the system is started up or when the // system has entered Sleep_Mode (iii > 40 000). // It is not possible to get out of this function without some opperation on // the Mbed. 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() { // This function ensures that Motor 1 is turned off. PwmPin1 = 0; } void OFF_m2() { // This function ensures that Motor 2 is turned off. PwmPin2 = 0; } void Going_Home_Motor1() { // This is the Homing function for Motor 1. if (counts1 == 0) { pc.printf("Switching to Homing State for Motor 2\r\n"); Active_State = Function; // Statement here instead of statemachine because of checking speed } else if (counts1 > 0) { PwmPin1 = 0.5f; DirectionPin1 = false; } else { PwmPin1 = 0.5f; DirectionPin1 = true; } } void Going_Home_Motor2() { // This is the Homing function for Motor 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.5f; DirectionPin2 = true; } else { PwmPin2 = 0.5f; 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.01f; vy = 0.01f; } else if (Checking_Bi_R && Checking_Tri_L) // sloping y = -x, y > 0 { vx = -0.01f; vy = 0.01f; } else if (Checking_Bi_L && Checking_Tri_R) // sloping y = -x, y < 0 { vx = 0.01f; vy = -0.01f; } else if (Checking_Tri_R && Checking_Tri_L) // sloping y = x, y < 0 { vx = -0.01f; vy = -0.01f; } if (Checking_Bi_R) // y > 0 { vx = 0.0f; vy = 0.01f; } else if (Checking_Bi_L) // x > 0 { vx = 0.01f; vy = 0.0f; } else if (Checking_Tri_R) // x < 0 { vx = 0.0f; vy = -0.01f; } else if (Checking_Tri_L) // y < 0 { vx = -0.01f; vy = 0.0f; } } void Inverse() { // Here we calculate the movement of each joint (Motor 1 and Motor 2) given // a certain linear velocity-input. q_ref1_n = q_ref1_o+w_1*Ts; q_ref2_n = q_ref2_o+w_2*Ts; r_1= -0.2f; r_2= -0.2f; float u = -r_2*sin(q_ref1_n)*cos(q_ref2_n)-(r_2)*cos(q_ref1_n)*sin(q_ref2_n); float z = 2.0f*(r_2*cos(q_ref1_n)*cos(q_ref2_n))-r_3; float y = r_2*cos(q_ref1_n)*cos(q_ref2_n)-r_2*sin(q_ref1_n)*sin(q_ref2_n)+2.0f*(r_1*cos(q_ref1_n))-r_3; float x = (-2.0f)*r_2*sin(q_ref1_n)*cos(q_ref2_n); 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 float referenceVelocity1; if (pot1.read()>0.8f) { // Clockwise rotation referenceVelocity1 = 0.005f; } else if (pot1.read() < 0.2f) { // Counterclockwise rotation referenceVelocity1 = -0.005f ; } else { referenceVelocity1 = pot1.read() * 0.0f; } float referenceVelocity2; // This is where Motor 2 is opperated. if (pot2.read()>0.8f) { // Clockwise rotation referenceVelocity2 = 0.005f; } else if (pot2.read() < 0.2f) { // Counterclockwise rotation referenceVelocity2 = -0.005f ; } else { referenceVelocity2 = pot2.read() * 0.0f; } //vx = 0.0;//referenceVelocity1; // uit emg data //vy = 0.01;referenceVelocity2; // uit emg data w_1 = vx*a+vy*b; w_2 = vx*c+vy*d; q_ref1_o = q_ref1_n; q_ref2_o = q_ref2_n; } void PID_controller() { // The PID-controller reduces the error between the reference signal // and the actual value. error_1 = q_ref1_o - rad_m2 + (w_1*Ts); error_2 = q_ref2_o - rad_m1 + (w_2*Ts); error_1_prev = error_1; error_2_prev = error_2; // Proportional part: u_k_1 = Kp * error_1; 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; u_i_1 = Ki * error_1_integral; 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); u_d_1 = Kd * error_1_derivative; u_d_2 = Kd * 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() { // This is where Motor 1 is operated. float u = 0.4f + 0.6f* fabs(U_1); PwmPin1 = fabs(u); if (U_1 >= 0) { DirectionPin1 = false; } else if (U_1 < 0) { DirectionPin1 = true; } } void motor2() { float u = 0.4f+ 0.6f* fabs(U_2); PwmPin2 = fabs(u); if (U_2 <= 0) { DirectionPin2 = false; } else if (U_2 > 0) { DirectionPin2 = true; } //DirectionPin2 = u > 0.0f; } void Printing() { // This function is merely for printing feedback from the system to our // screen when we wish to see or check something. if (Active_State == Function )//|| Active_State == Homing_M1) { pc.printf("U_K1 is %f \r\nU_K2 is %f \r\n\r\n rad_m1 is %f \r\n rad_m2 is %f \r\n\r\n Bi_R is %f \r\n Bi_L is %f \r\n\r\n counts1 is %i \r\n counts2 is %i\r\n\r\n", u_k_1, u_k_2, rad_m1, rad_m2, Filtered_Bi_R, Filtered_Bi_L, counts1, counts2); //u_k_1 + u_i_1 + u_d_1; //u_k_2 + u_i_2 + u_d_2; //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", rad_m1, rad_m2,counts1, counts2, w_1, w_2); // pc.printf("U_1 = %f [error] \r\nU_2 = %f [error]\n\r\n",U_1,U_2); //pc.printf("u_k_1 = %f\r\nu_k_2 = %f\r\nu_i_1 = %f \r\nu_i_2 = %f \r\nu_d_1 = %f \r\nu_d_2 = %f\r\n\r\n",u_k_1,u_k_2,u_i_1,u_i_2,u_d_1,u_d_2); } } void EMG_test() // even nalopen of dit ook kan met i++! { // Here we can check the opperation of the system for checking purposes. 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() { // In the Statemachine every function is integrated into the system // depending on the state that the system is in. This is makes the // integration and the opperation of the system a lot simpeler. // We have 5 main states: // // - Starting // - Start_Up // - Sleep_Mode // - Calibration // - Homing // - Homing_M1 // - Homing_M2 // - Post_Homing // - Function // - Safe // As seen above some states have substates that make the system run better // or give the state some unique features. These are explained within the // functions that are called in that state. 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 = Function; } sample(); EMG_Read(); break; case Homing_M1: Going_Home_Motor1(); OFF_m2(); Encoding(); if (fabs(rad_m1)>(2.0f*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)>(2.0f*pi/3.0f) || fabs(rad_m2)>(25.0f*pi/12.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() { // Our main loop is as empty as possible and contains only statements that // cannot be put elsewhere due to functioning reasons. // Also the "while-loop" is completely empty, which improves the running // speed of the system. 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, 0.5); while(true) { } }