Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 14:95acac6a07c7
- Parent:
- 13:ec227b229f3d
- Child:
- 15:5d24f832bb7b
--- a/main.cpp Wed Nov 01 11:25:22 2017 +0000 +++ b/main.cpp Thu Nov 02 12:19:54 2017 +0000 @@ -7,13 +7,13 @@ #include "QEI.h" const double pi = 3.1415926535897; // Definition of pi - + // SERIAL COMMUNICATION WITH PC //////////////////////////////////////////////// MODSERIAL pc(USBTX, USBRX); HIDScope scope(4); // STATES ////////////////////////////////////////////////////////////////////// -enum states{MOTORS_OFF, MOVING, HITTING}; +enum states{MOTORS_OFF, CALIBRATING, MOVING, HITTING}; states currentState = MOTORS_OFF; // Start with motors off bool stateChanged = true; // Make sure the initialization of first state is executed @@ -28,62 +28,86 @@ PwmOut motor2MagnitudePin(D6); InterruptIn button1(D2); // CONNECT BUT1 TO D2 InterruptIn button2(D3); // CONNECT BUT2 TO D3 -InterruptIn button3(SW2); +InterruptIn button3(SW2); InterruptIn button4(SW3); -//AnalogIn emg(A0); -AnalogIn potmeter1(A0); -AnalogIn potmeter2(A1); +AnalogIn potmeter1(A0); // CONNECT POT1 TO A0 +AnalogIn potmeter2(A1); // CONNECT POT2 TO A1 +DigitalOut led1(LED_RED); +DigitalOut led2(LED_BLUE); +DigitalOut led3(LED_GREEN); +AnalogIn emg_r(A2); // CONNECT EMG TO A2 +AnalogIn emg_l(A3); // CONNECT EMG TO A3 // MOTOR CONTROL /////////////////////////////////////////////////////////////// -Ticker controllerTicker; -const double controller_Ts = 1/200.1; // Time step for controllerTicker [s]; Should be between 5kHz and 10kHz +Ticker controllerTicker; // Ticker for the controller +const double controller_Ts = 1/200.1; // Time step for controllerTicker [s] const double motorRatio = 131.25; // Ratio of the gearbox in the motors [] const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse] -double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start -double velocity = 0.05; // X and Y velocity of the end effector when desired +volatile double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start +double velocity = 0.01; // X and Y velocity of the end effector when desired // MOTOR 1 -volatile double position1; // Position of motor 1 [rad] -volatile double reference1 = 0; // Desired rotation of motor 1 [rad] -double motor1Max = 0; // Maximum rotation of motor 1 [rad] -double motor1Min = 2*pi*-40/360; // Minimum rotation of motor 1 [rad] +volatile double position1; // Position of motor 1 [rad] +volatile double reference1 = 0; // Desired rotation of motor 1 [rad] +double motor1Max = 0; // Maximum rotation of motor 1 [rad] +double motor1Min = 2*pi*-40/360; // Minimum rotation of motor 1 [rad] // Controller gains -const double motor1_KP = 13; // Position gain [] +const double motor1_KP = 13; // Position gain [] const double motor1_KI = 7; // Integral gain [] const double motor1_KD = 0.3; // Derivative gain [] double motor1_err_int = 0, motor1_prev_err = 0; // Derivative filter coefficients -const double motor1_f_a1 = 0.25, motor1_f_a2 = 0.8; // Derivative filter coefficients [] -const double motor1_f_b0 = 1, motor1_f_b1 = 2, motor1_f_b2 = 0.8; // Derivative filter coefficients [] +const double motor1_f_a1 = 0.25, motor1_f_a2 = 0.8; // Derivative filter coefficients [] +const double motor1_f_b0 = 1, motor1_f_b1 = 2, motor1_f_b2 = 0.8; // Derivative filter coefficients [] // Filter variables double motor1_f_v1 = 0, motor1_f_v2 = 0; // MOTOR 2 -volatile double position2; // Position of motor 2 [rad] -volatile double reference2 = 0; // Desired rotation of motor 2 [rad] +volatile double position2; // Position of motor 2 [rad] +volatile double reference2 = 0; // Desired rotation of motor 2 [rad] double motor2Max = 2*pi*25/360; // Maximum rotation of motor 2 [rad] double motor2Min = 2*pi*-28/360; // Minimum rotation of motor 2 [rad] // Controller gains -//const double motor2_KP = potmeter1*10; // Position gain [] -//const double motor2_KI = potmeter2*20; // Integral gain [] -const double motor2_KP = 13; // Position gain [] +//const double motor2_KP = potmeter1*10; // Position gain [] +//const double motor2_KI = potmeter2*20; // Integral gain [] +const double motor2_KP = 13; // Position gain [] const double motor2_KI = 5; // Integral gain [] const double motor2_KD = 0.1; // Derivative gain [] double motor2_err_int = 0, motor2_prev_err = 0; // Derivative filter coefficients -const double motor2_f_a1 = 0.25, motor2_f_a2 = 0.8; // Derivative filter coefficients [] -const double motor2_f_b0 = 1, motor2_f_b1 = 2, motor2_f_b2 = 0.8; // Derivative filter coefficients [] +const double motor2_f_a1 = 0.25, motor2_f_a2 = 0.8; // Derivative filter coefficients [] +const double motor2_f_b0 = 1, motor2_f_b1 = 2, motor2_f_b2 = 0.8; // Derivative filter coefficients [] // Filter variables double motor2_f_v1 = 0, motor2_f_v2 = 0; -// EMG FILTER ////////////////////////////////////////////////////////////////// -//BiQuadChain bqc; -//BiQuad bq1(3.6, 5.0, 2.0, 0.5, 30.0); // EMG filter coefficients [] -//BiQuad bq2(0, 0, 0, 0, 0); // EMG filter coefficients [] +// EMG ////////////////////////////////////////////////////////////////// +Ticker emgLeft; +Ticker emgRight; +double emgTs = 0.5; +// Filters +BiQuadChain bqc; +BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004); +BiQuad bq3_notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780); +BiQuad bq1_low(3.65747e-2, 7.31495e-2, 3.65747e-2, -1.390892, 0.537191); +// Right arm +volatile double emgFiltered_r; +volatile double filteredAbs_r; +volatile double emg_value_r; +volatile double onoffsignal_r; +volatile bool check_calibration_r=0; +volatile double avg_emg_r; +volatile bool active_r = false; +// Left arm +volatile double emgFiltered_l; +volatile double filteredAbs_l; +volatile double emg_value_l; +volatile double onoffsignal_l; +volatile bool check_calibration_l=0; +volatile double avg_emg_l; +volatile bool active_l = false; -Ticker sampleTicker; -const double tickerTs = 0.1; // Time step for sampling [s] - +Ticker sampleTicker; +const double sampleTs = 0.05; // FUNCTIONS /////////////////////////////////////////////////////////////////// // BIQUAD FILTER FOR DERIVATIVE OF ERROR /////////////////////////////////////// @@ -96,7 +120,6 @@ return y; } - // PID-CONTROLLER WITH FILTER ////////////////////////////////////////////////// double PID_Controller(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, @@ -146,20 +169,27 @@ // MOTOR 1 PID-CONTROLLER ////////////////////////////////////////////////////// void Motor1Controller() { - position1 = radPerPulse * Encoder1.getPulses(); - position2 = radPerPulse * Encoder2.getPulses(); - //pc.printf("error %f \r\n", reference1 - position1); - double motor1Value = PID_Controller(reference1 - position1, motor1_KP, - motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err, - motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0, - motor1_f_b1, motor1_f_b2); - //pc.printf("motor value %f \r\n",motor1Value); - RotateMotor1(motor1Value); - double motor2Value = PID_Controller(reference2 - position2, motor2_KP, - motor2_KI, motor2_KD, controller_Ts, motor2_err_int, motor2_prev_err, - motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0, - motor2_f_b1, motor2_f_b2); - RotateMotor2(motor2Value); + if(currentState == MOVING) + { + position1 = radPerPulse * Encoder1.getPulses(); + position2 = radPerPulse * Encoder2.getPulses(); + //pc.printf("error %f \r\n", reference1 - position1); + double motor1Value = PID_Controller(reference1 - position1, motor1_KP, + motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err, + motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0, + motor1_f_b1, motor1_f_b2); + //pc.printf("motor value %f \r\n",motor1Value); + + double motor2Value = PID_Controller(reference2 - position2, motor2_KP, + motor2_KI, motor2_KD, controller_Ts, motor2_err_int, motor2_prev_err, + motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0, + motor2_f_b1, motor2_f_b2); + + //pc.printf("%f, %f \r\n", motor1Value, motor2Value); + + RotateMotor1(motor1Value); + RotateMotor2(motor2Value); + } } // MOTOR 2 PID-CONTROLLER ////////////////////////////////////////////////////// @@ -180,116 +210,257 @@ motor2MagnitudePin = 0; } -// DETERMINE JOINT VELOCITIES ////////////////////////////////////////////////// -void jointVelocities() -{ - position1 = radPerPulse * Encoder1.getPulses(); - position2 = radPerPulse * Encoder2.getPulses(); - - if(!button1 && reference1 > motor1Min && reference2 < motor2Max) xVelocity = velocity; - else if(!button2 && reference1 < motor1Max && reference2 > motor2Min) xVelocity = -velocity; - else xVelocity = 0; + +// EMG ///////////////////////////////////////////////////////////////////////// +// Filter EMG signal of right arm - if(!button3 && reference2 < motor2Max && reference1 > motor2Min) yVelocity = velocity; - else if(!button4 && reference2 > motor2Min && reference1 > motor2Min) yVelocity = -velocity; - else yVelocity = 0; - - //pc.printf("x %f, y %f \r\n", xVelocity, yVelocity); - - // Construct Jacobian - double q[4]; - q[0] = position1, q[1] = -position1; - q[2] = position2, q[3] = -position2; - - double T2[3]; // Second column of the jacobian - double T3[3]; // Third column of the jacobian - double T4[3]; // Fourth column of the jacobian - double T1[6]; - static const signed char b_T1[3] = { 1, 0, 0 }; - double J_data[6]; - - T2[0] = 1.0; - T2[1] = 0.365 * cos(q[0]); - T2[2] = 0.365 * sin(q[0]); - T3[0] = 1.0; - T3[1] = 0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 + - q[0]) + q[1]); - T3[2] = 0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 + - q[0]) + q[1]); - T4[0] = 1.0; - T4[1] = (0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 + - q[0]) + q[1])) + 0.265 * sin((q[0] + q[1]) + q[2]); - T4[2] = (0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 + - q[0]) + q[1])) - 0.265 * cos((q[0] + q[1]) + q[2]); - - for (int i = 0; i < 3; i++) - { - T1[i] = (double)b_T1[i] - T2[i]; - T1[3 + i] = T3[i] - T4[i]; +void filter_r(){ + if(check_calibration_r==1){ + emg_value_r = emg_r.read(); + emgFiltered_r = bqc.step( emg_value_r ); + filteredAbs_r = abs( emgFiltered_r ); + if (avg_emg_r != 0){ + onoffsignal_r=filteredAbs_r/avg_emg_r; //divide the emg_r signal by the max emg_r to calibrate the signal per person + } + } +} + +// Filter EMG signal of left arm +void filter_l(){ + if(check_calibration_l==1){ + emg_value_l = emg_l.read(); + emgFiltered_l = bqc.step( emg_value_l ); + filteredAbs_l = abs( emgFiltered_l ); + if (avg_emg_l != 0){ + onoffsignal_l=filteredAbs_l/avg_emg_l; //divide the emg_r signal by the avg_emg_l wat staat voor avg emg in gespannen toestand + } } - - for (int i = 0; i < 2; i++) - { - for (int j = 0; j < 3; j++) - { - J_data[j + 3 * i] = T1[j + 3 * i]; - } +} + +// Check threshold right arm +void check_emg_r(){ +double filteredAbs_temp_r; + + if((check_calibration_l==1) &&(check_calibration_r==1)){ + for( int i = 0; i<250;i++){ + filter_r(); + filteredAbs_temp_r = filteredAbs_temp_r + onoffsignal_r; + wait(0.0004); // 0.0004 + } + filteredAbs_temp_r = filteredAbs_temp_r/250; + if(filteredAbs_temp_r<=0.3){ //if signal is lower then 0.5 the blue light goes on + led1.write(1); //led 1 is rood en uit + + active_r = false; + } + else if(filteredAbs_temp_r > 0.3){ //if signal does not pass threshold value, blue light goes on + led1.write(0); + active_r = true; + } } - - // Here the first row of the Jacobian is cut off, so we do not take rotation into consideration - // Note: the matrices from now on will we constructed rowwise - double Jvelocity[4]; - Jvelocity[0] = J_data[1]; - Jvelocity[1] = J_data[4]; - Jvelocity[2] = J_data[2]; - Jvelocity[3] = J_data[5]; - - // Creating the inverse Jacobian - double Jvelocity_inv[4]; // The inverse matrix of the jacobian - double determ = Jvelocity[0]*Jvelocity[3]-Jvelocity[1]*Jvelocity[2]; // The determinant of the matrix - Jvelocity_inv[0] = Jvelocity[3]/determ; - Jvelocity_inv[1] = -Jvelocity[1]/determ; - Jvelocity_inv[2] = -Jvelocity[2]/determ; - Jvelocity_inv[3] = Jvelocity[0]/determ; - - // Now the velocity of the joints are found by giving the velocity of the end-effector and the inverse jacobian - double msh[2]; // This is the velocity the joints have to have - msh[0] = xVelocity*Jvelocity_inv[0] + yVelocity*Jvelocity_inv[1]; - msh[1] = xVelocity*Jvelocity_inv[2] + yVelocity*Jvelocity_inv[3]; - - if(reference1 + msh[0]*tickerTs > motor1Max) reference1 = motor1Max; - else if(reference1 + msh[0]*tickerTs < motor1Min) reference1 = motor1Min; - else reference1 = reference1 + msh[0]*tickerTs; - - if(reference2 + msh[1]*tickerTs > motor2Max) reference2 = motor2Max; - else if(reference2 + msh[1]*tickerTs < motor2Min) reference2 = motor2Min; - else reference2 = reference2 + msh[1]*tickerTs; - - //Motor1Controller(), Motor2Controller(); - scope.set(0,reference1); - scope.set(1,position1); - scope.set(2,reference2); - scope.set(3,position2); - scope.send(); - - pc.printf("position 1 %f, 2 %f \r\n", position1/2/pi*360, position2/2/pi*360); - pc.printf("reference 1 %f, 2 %f \r\n", reference1/2/pi*360, reference2/2/pi*360); - //pc.printf("msh*Ts 1 %f, 2 %f \r\n\n", msh[0]*emg_Ts, msh[1]*emg_Ts); +} + // Check threshold left arm +void check_emg_l(){ +double filteredAbs_temp_l; + + if((check_calibration_l)==1 &&(check_calibration_r==1) ){ + for( int i = 0; i<250;i++){ + filter_l(); + filteredAbs_temp_l = filteredAbs_temp_l + onoffsignal_l; + wait(0.0004); // 0.0004 + } + filteredAbs_temp_l = filteredAbs_temp_l/250; + if(filteredAbs_temp_l<=0.3){ //if signal is lower then 0.5 the blue light goes on + // led1.write(1); //led 1 is rood en uit + led2.write(1); //led 2 is blauw en aan + active_l = false; + } + else if(filteredAbs_temp_l > 0.3){ //if signal does not pass threshold value, blue light goes on + // led1.write(0); + led2.write(0); + active_l = true; + } + } + } -// EMG ///////////////////////////////////////////////////////////////////////// -void EmgSample() +// Calibrate right arm +int calibration_r(){ + led3.write(0); + + double signal_verzameling_r = 0; + for(int n =0; n<5000;n++){ + filter_r(); + //read for 5000 samples as calibration + emg_value_r = emg_r.read(); + emgFiltered_r = bqc.step( emg_value_r ); + filteredAbs_r = abs(emgFiltered_r); + + + // signal_verzameling[n]= filteredAbs_r; + signal_verzameling_r = signal_verzameling_r + filteredAbs_r ; + wait(0.0004); + + if (n == 4999){ + avg_emg_r = signal_verzameling_r / n; + + } + } + + led3.write(1); + //pc.printf("calibratie rechts compleet\n\r"); + + check_calibration_r=1; + return 0; +} + +// Calibrate left arm +int calibration_l(){ + led3.write(0); + + double signal_verzameling_l= 0; + for(int n =0; n<5000;n++){ + filter_l(); + //read for 5000 samples as calibration + emg_value_l = emg_l.read(); + emgFiltered_l = bqc.step( emg_value_l ); + filteredAbs_l = abs(emgFiltered_l); + + + // signal_verzameling[n]= filteredAbs_r; + signal_verzameling_l = signal_verzameling_l + filteredAbs_l ; + wait(0.0004); + + if (n == 4999){ + avg_emg_l = signal_verzameling_l / n; + + } + } + led3.write(1); + wait(1); + check_calibration_l=1; + + //pc.printf("calibratie links compleet\n\r"); + // } + return 0; +} + +// DETERMINE JOINT VELOCITIES ////////////////////////////////////////////////// +void JointVelocities() { if(currentState == MOVING) { - //double emgFiltered = bqc.step(emg.read()); // Filter the EMG signal - /* - If EMG signal 1 --> xVelocity / yVelocity = velocity - Else if EMG signal 2 --> xVelocity / yVelocity = -velocity - Else xVelocity / yVelocity = 0 - If both signal 1 and 2 --> Switch - */ - jointVelocities(); + position1 = radPerPulse * Encoder1.getPulses(); + position2 = radPerPulse * Encoder2.getPulses(); + + //check_emg_r(), check_emg_l(); + + if(active_r && reference1 > motor1Min && reference2 < motor2Max) + { + xVelocity = velocity; + //pc.printf("button1 \r\n"); + } + else if(active_l && reference1 < motor1Max && reference2 > motor2Min) + { + xVelocity = -velocity; + //pc.printf(" button2 \r\n"); + } + else xVelocity = 0; + + if(!button3 && reference2 < motor2Max )//&& reference1 > motor2Min) + { + yVelocity = velocity; + //pc.printf(" button3 \r\n"); + } + else if(!button4 && reference2 > motor2Min )//&& reference1 > motor2Min) + { + yVelocity = -velocity; + //pc.printf(" button4 \r\n"); + } + else yVelocity = 0; + + //pc.printf("x %f, y %f \r\n", xVelocity, yVelocity); + + // Construct Jacobian + double q[4]; + q[0] = position1, q[1] = -position1; + q[2] = position2, q[3] = -position2; + + double T2[3]; // Second column of the jacobian + double T3[3]; // Third column of the jacobian + double T4[3]; // Fourth column of the jacobian + double T1[6]; + static const signed char b_T1[3] = { 1, 0, 0 }; + double J_data[6]; + + T2[0] = 1.0; + T2[1] = 0.365 * cos(q[0]); + T2[2] = 0.365 * sin(q[0]); + T3[0] = 1.0; + T3[1] = 0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 + + q[0]) + q[1]); + T3[2] = 0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 + + q[0]) + q[1]); + T4[0] = 1.0; + T4[1] = (0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 + + q[0]) + q[1])) + 0.265 * sin((q[0] + q[1]) + q[2]); + T4[2] = (0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 + + q[0]) + q[1])) - 0.265 * cos((q[0] + q[1]) + q[2]); + + for (int i = 0; i < 3; i++) + { + T1[i] = (double)b_T1[i] - T2[i]; + T1[3 + i] = T3[i] - T4[i]; + } + + for (int i = 0; i < 2; i++) + { + for (int j = 0; j < 3; j++) + { + J_data[j + 3 * i] = T1[j + 3 * i]; + } + } + + // Here the first row of the Jacobian is cut off, so we do not take rotation into consideration + // Note: the matrices from now on will we constructed rowwise + double Jvelocity[4]; + Jvelocity[0] = J_data[1]; + Jvelocity[1] = J_data[4]; + Jvelocity[2] = J_data[2]; + Jvelocity[3] = J_data[5]; + + // Creating the inverse Jacobian + double Jvelocity_inv[4]; // The inverse matrix of the jacobian + double determ = Jvelocity[0]*Jvelocity[3]-Jvelocity[1]*Jvelocity[2]; // The determinant of the matrix + Jvelocity_inv[0] = Jvelocity[3]/determ; + Jvelocity_inv[1] = -Jvelocity[1]/determ; + Jvelocity_inv[2] = -Jvelocity[2]/determ; + Jvelocity_inv[3] = Jvelocity[0]/determ; + + // Now the velocity of the joints are found by giving the velocity of the end-effector and the inverse jacobian + double msh[2]; // This is the velocity the joints have to have + msh[0] = xVelocity*Jvelocity_inv[0] + yVelocity*Jvelocity_inv[1]; + msh[1] = xVelocity*Jvelocity_inv[2] + yVelocity*Jvelocity_inv[3]; + + if(reference1 + msh[0]*sampleTs > motor1Max) reference1 = motor1Max; + else if(reference1 + msh[0]*sampleTs < motor1Min) reference1 = motor1Min; + else reference1 = reference1 + msh[0]*sampleTs; + + if(reference2 + msh[1]*sampleTs > motor2Max) reference2 = motor2Max; + else if(reference2 + msh[1]*sampleTs < motor2Min) reference2 = motor2Min; + else reference2 = reference2 + msh[1]*sampleTs; + + + scope.set(0,reference1); + scope.set(1,position1); + scope.set(2,reference2); + scope.set(3,position2); + scope.send(); + + pc.printf("position 1 %f, 2 %f \r\n", position1/2/pi*360, position2/2/pi*360); + pc.printf("reference 1 %f, 2 %f \r\n", reference1/2/pi*360, reference2/2/pi*360); + //pc.printf("msh*Ts 1 %f, 2 %f \r\n\n", msh[0]*emg_Ts, msh[1]*emg_Ts); + } } @@ -304,7 +475,7 @@ if(stateChanged) { //pc.printf("Entering MOTORS_OFF \r\n" - //"Press button 1 to enter MOVING \r\n"); + //"Press button 1 to enter CALIBRATING \r\n"); TurnMotorsOff(); // Turn motors off stateChanged = false; } @@ -312,10 +483,35 @@ // Home command if(!button1) { + currentState = CALIBRATING; + stateChanged = true; + break; + } + break; + } + + case CALIBRATING: + { + // State initialization + if(stateChanged) + { + //pc.printf("Entering CALIBRATING \r\n" + //"Press button 1 to enter MOVING \r\n"); + stateChanged = false; + calibration_r(); + calibration_l(); + currentState = MOVING; + stateChanged = true; + } + /* + // Home command + if(!button1) + { currentState = MOVING; stateChanged = true; break; } + */ break; } @@ -324,7 +520,7 @@ // State initialization if(stateChanged) { - //pc.printf("Entering MOVING \r\n" + //pc.printf("Entering MOVING \r\n"); //"Press button 2 to enter HITTING \r\n"); stateChanged = false; } @@ -369,12 +565,17 @@ // Serial communication pc.baud(115200); + led1.write(1); + led2.write(1); + led3.write(1); pc.printf("START \r\n"); - //bqc.add(&bq1).add(&bq2); // Make BiQuad Chain + bqc.add( &bq1_low ).add( &bq2_high ).add( &bq3_notch ); - sampleTicker.attach(&EmgSample, tickerTs); // Ticker to sample EMG + sampleTicker.attach(&JointVelocities, sampleTs); // Ticker to sample EMG controllerTicker.attach(&Motor1Controller, controller_Ts); // Ticker to control motor 1 (PID) + emgRight.attach(&check_emg_r, emgTs); //continously execute the motor controller + emgLeft.attach(&check_emg_l, emgTs); motor1MagnitudePin.period_ms(1); motor2MagnitudePin.period_ms(1);