Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 18:2b6f41f39a7f
- Parent:
- 17:f8dd5b8e4b52
- Child:
- 19:6f720e4fcb47
diff -r f8dd5b8e4b52 -r 2b6f41f39a7f main.cpp --- a/main.cpp Thu Nov 02 14:03:10 2017 +0000 +++ b/main.cpp Thu Nov 02 16:23:26 2017 +0000 @@ -13,18 +13,18 @@ HIDScope scope(4); // STATES ////////////////////////////////////////////////////////////////////// -enum states{MOTORS_OFF, CALIBRATING, MOVING, HITTING}; +enum states{MOTORS_OFF, CALIBRATING, MOVING}; states currentState = MOTORS_OFF; // Start with motors off bool stateChanged = true; // Make sure the initialization of first state is executed // ENCODER ///////////////////////////////////////////////////////////////////// -QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D10, ENC1B TO D11 -QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D12, ENC2B TO D13 +QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D12, ENC1B TO D13 +QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D10, ENC2B TO D11 // PINS //////////////////////////////////////////////////////////////////////// DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW PwmOut motor1MagnitudePin(D5); -DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or CCW? +DigitalOut motor2DirectionPin(D7); // Value 0: CW; 1: CCW PwmOut motor2MagnitudePin(D6); InterruptIn button1(D2); // CONNECT BUT1 TO D2 InterruptIn button2(D3); // CONNECT BUT2 TO D3 @@ -42,7 +42,7 @@ // MOTOR CONTROL /////////////////////////////////////////////////////////////// Ticker controllerTicker; // Ticker for the controller -const double controller_Ts = 1/200.1; // Time step for controllerTicker [s] +const double controllerTs = 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] volatile double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start @@ -50,7 +50,7 @@ // MOTOR 1 volatile double position1; // Position of motor 1 [rad] -volatile double reference1 = -5; // Desired rotation of motor 1 [rad] +volatile double reference1 = 2*pi*-5/360; // Desired rotation of motor 1 [rad] const double motor1Max = 0; // Maximum rotation of motor 1 [rad] const double motor1Min = 2*pi*-40/360; // Minimum rotation of motor 1 [rad] // Controller gains @@ -80,21 +80,21 @@ // Filter variables double motor2_f_v1 = 0, motor2_f_v2 = 0; -// EMG ////////////////////////////////////////////////////////////////// -Ticker emgLeft; -Ticker emgRight; -const double emgTs = 0.5; +// EMG ///////////////////////////////////////////////////////////////////////// +Ticker emgLeft; // Ticker for EMG of left arm +Ticker emgRight; // Ticker for EMG of right arm +const double emgTs = 0.491; // Time step for EMG sampling // 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); +BiQuadChain bqc; +BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004); // High pass filter +BiQuad bq3_notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780); // Notch filter +BiQuad bq1_low(3.65747e-2, 7.31495e-2, 3.65747e-2, -1.390892, 0.537191); // Low pass filter // 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 bool check_calibration_r = false; volatile double avg_emg_r; volatile bool active_r = false; // Left arm @@ -102,16 +102,16 @@ volatile double filteredAbs_l; volatile double emg_value_l; volatile double onoffsignal_l; -volatile bool check_calibration_l=0; +volatile bool check_calibration_l = false; volatile double avg_emg_l; volatile bool active_l = false; - -Ticker sampleTicker; -const double sampleTs = 0.05; -volatile bool xdir = true, ydir = false; -volatile bool timer = false; -volatile int count = 0; +// PROCESS EMG SIGNALS ///////////////////////////////////////////////////////// +Ticker processTicker; // Ticker for processing of EMG +const double processTs = 0.1; // Time step for processing of EMG + +volatile bool xdir = true, ydir = false; // Direction the EMG signal moves the end effector +volatile int count = 0; // Counter to change direction // FUNCTIONS /////////////////////////////////////////////////////////////////// // BIQUAD FILTER FOR DERIVATIVE OF ERROR /////////////////////////////////////// @@ -133,19 +133,17 @@ // Derivative double e_der = (e - e_prev)/Ts; // Calculate the derivative of error e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); // Filter the derivative of error - //e_der = bq1.step(e_der); e_prev = e; // Integral e_int = e_int + Ts*e; // Calculate the integral of error // PID - //pc.printf("%f --> %f \r\n", e_der, e_derf); - return Kp*e + Ki*e_int + Kd*e_der; + return Kp*e + Ki*e_int + Kd*e_der; // Calculate motor value } // MOTOR 1 ///////////////////////////////////////////////////////////////////// void RotateMotor1(double motor1Value) { - if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING + if(currentState == MOVING) // Check if state is MOVING { if(motor1Value >= 0) motor1DirectionPin = 0; // Rotate motor 1 CW else motor1DirectionPin = 1; // Rotate motor 1 CCW @@ -159,10 +157,10 @@ // MOTOR 2 ///////////////////////////////////////////////////////////////////// void RotateMotor2(double motor2Value) { - if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING + if(currentState == MOVING) // Check if state is MOVING { - if(motor2Value >= 0) motor2DirectionPin = 1; // Rotate motor 1 CW - else motor2DirectionPin = 0; // Rotate motor 1 CCW + if(motor2Value >= 0) motor2DirectionPin = 1; // Rotate motor 2 CW + else motor2DirectionPin = 0; // Rotate motor 2 CCW if(fabs(motor2Value) > 1) motor2MagnitudePin = 1; else motor2MagnitudePin = fabs(motor2Value); @@ -170,43 +168,29 @@ else motor2MagnitudePin = 0; } -// MOTOR 1 PID-CONTROLLER ////////////////////////////////////////////////////// -void Motor1Controller() +// MOTOR PID-CONTROLLER ////////////////////////////////////////////////////// +void MotorController() { 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, + position1 = radPerPulse * Encoder1.getPulses(); // Get current position of motor 1 + position2 = radPerPulse * Encoder2.getPulses(); // Get current position of motor 2 + + double motor1Value = PID_Controller(reference1 - position1, motor1_KP, // Calculate motor value motor 1 + motor1_KI, motor1_KD, controllerTs, 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, + double motor2Value = PID_Controller(reference2 - position2, motor2_KP, // Calculate motor value motor 2 + motor2_KI, motor2_KD, controllerTs, 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); + RotateMotor1(motor1Value); // Rotate motor 1 + RotateMotor2(motor2Value); // Rotate motor 2 } } - -// MOTOR 2 PID-CONTROLLER ////////////////////////////////////////////////////// -/*void Motor2Controller() -{ - position2 = radPerPulse * Encoder2.getPulses(); - double motor2Value = PID_Controller(reference2 - position2, motor2_KP, - motor2_KI, motor2_KD, controller2_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); -} -*/ + // TURN OFF MOTORS ///////////////////////////////////////////////////////////// void TurnMotorsOff() { @@ -214,139 +198,140 @@ motor2MagnitudePin = 0; } - // EMG ///////////////////////////////////////////////////////////////////////// // Filter EMG signal of right arm - -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 - } +void filter_r() +{ + if(check_calibration_r == 1) + { + emg_value_r = emg_r.read(); // Get EMG signal + emgFiltered_r = bqc.step(emg_value_r); // Filter EMG signal using BiQuad Chain + filteredAbs_r = abs(emgFiltered_r); // Takes absolute value + + 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){ +void filter_l() +{ + if(check_calibration_l == 1) + { emg_value_l = emg_l.read(); - emgFiltered_l = bqc.step( emg_value_l ); + 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 - } + if (avg_emg_l != 0) + { + onoffsignal_l = filteredAbs_l/avg_emg_l; + } } } // Check threshold right arm -void check_emg_r(){ -double filteredAbs_temp_r; +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; - } + 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); + } + 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; + } } } - // Check threshold left arm -void check_emg_l(){ -double filteredAbs_temp_l; + +// 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; - } + 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); + } + 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 + { + led2.write(1); + active_l = false; + } + else if(filteredAbs_temp_l > 0.3) //if signal does not pass threshold value, blue light goes on + { + led2.write(0); + active_l = true; + } } - } // Calibrate right arm -int calibration_r(){ - led3.write(0); +void 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); + double signal_collection_r = 0; + for(int n =0; n < 5000; n++) //read for 5000 samples as calibration + { + filter_r(); + 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); + signal_collection_r = signal_collection_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; + if (n == 4999) + { + avg_emg_r = signal_collection_r / n; + } + } + led3.write(1); + check_calibration_r = 1; } // Calibrate left arm -int calibration_l(){ - led3.write(0); +void 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); + double signal_collection_l = 0; + for(int n = 0; n < 5000; n++) //read for 5000 samples as calibration + { + filter_l(); + emg_value_l = emg_l.read(); + emgFiltered_l = bqc.step(emg_value_l); + filteredAbs_l = abs(emgFiltered_l); + signal_collection_l = signal_collection_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; + if (n == 4999) + { + avg_emg_l = signal_collection_l / n; + } + } + led3.write(1); + wait(1); + check_calibration_l = 1; } // DETERMINE JOINT VELOCITIES ////////////////////////////////////////////////// @@ -354,13 +339,13 @@ { if(currentState == MOVING) { - position1 = radPerPulse * Encoder1.getPulses(); - position2 = radPerPulse * Encoder2.getPulses(); + //position1 = radPerPulse * Encoder1.getPulses(); + //position2 = radPerPulse * Encoder2.getPulses(); - if(active_l && active_r) + if(active_l && active_r) // If both left and right EMG are active for 1 second the direction of control changes { count += 1; - if(count == 40) + if(count == 20) { active_l = false; active_r = false; @@ -374,45 +359,43 @@ } else count = 0; - if(xdir) + if(xdir) // Control in x-direction { - if(active_r && count == 0 && reference1 > motor1Min && reference2 < motor2Max) + if(active_r && count == 0 && // Checks whether EMG is active, changing direction and max rotation of motors + reference1 > motor1Min && reference2 < motor2Max) { - xVelocity = velocity; - pc.printf("x positive \r\n"); + xVelocity = velocity; // Give velocity to end effector } - else if(active_l && count == 0 && reference1 < motor1Max && reference2 > motor2Min) + else if(active_l && count == 0 && + reference1 < motor1Max && reference2 > motor2Min) { xVelocity = -velocity; - pc.printf("x negative \r\n"); } else xVelocity = 0; } - else if(ydir) + else if(ydir) // Control in y-direction { - if(active_r && count == 0 && reference2 < motor2Max )//&& reference1 > motor2Min) + if(active_r && count == 0 && + reference2 < motor2Max ) //&& reference1 > motor2Min) { yVelocity = velocity; - pc.printf("y positive \r\n"); } - else if(active_l && count == 0 && reference2 > motor2Min )//&& reference1 > motor2Min) + else if(active_l && count == 0 + && reference2 > motor2Min ) //&& reference1 > motor2Min) { yVelocity = -velocity; - pc.printf("y negative \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 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]; @@ -421,15 +404,17 @@ 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]); + 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]); + 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++) { @@ -454,36 +439,36 @@ 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 + 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 + double msh[2]; // 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; + // Determine reference position of motor 1 + if(reference1 + msh[0]*processTs > motor1Max) reference1 = motor1Max; + else if(reference1 + msh[0]*processTs < motor1Min) reference1 = motor1Min; + else reference1 = reference1 + msh[0]*processTs; - if(reference2 + msh[1]*sampleTs > motor2Max) reference2 = motor2Max; - else if(reference2 + msh[1]*sampleTs < motor2Min) reference2 = motor2Min; - else reference2 = reference2 + msh[1]*sampleTs; + // Determine reference position of motor 2 + if(reference2 + msh[1]*processTs > motor2Max) reference2 = motor2Max; + else if(reference2 + msh[1]*processTs < motor2Min) reference2 = motor2Min; + else reference2 = reference2 + msh[1]*processTs; - scope.set(0,reference1); + /*scope.set(0,reference1); scope.set(1,position1); scope.set(2,reference2); scope.set(3,position2); - scope.send(); + 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); - } } @@ -503,7 +488,7 @@ stateChanged = false; } - // Home command + // Continue button if(!button1) { currentState = CALIBRATING; @@ -519,22 +504,13 @@ if(stateChanged) { pc.printf("Entering CALIBRATING \r\n" - "Press button 1 to enter MOVING \r\n"); + "Tighten muscles until green LED is off \r\n"); stateChanged = false; - calibration_r(); - calibration_l(); + calibration_r(); // Calibrate right arm + calibration_l(); // Calibrate left arm currentState = MOVING; stateChanged = true; } - /* - // Home command - if(!button1) - { - currentState = MOVING; - stateChanged = true; - break; - } - */ break; } @@ -544,35 +520,8 @@ if(stateChanged) { pc.printf("Entering MOVING \r\n"); - //"Press button 2 to enter HITTING \r\n"); stateChanged = false; } - - - - // Hit command - /*if(!button2) - { - currentState = HITTING; - stateChanged = true; - break; - } - */ - break; - } - - case HITTING: - { - // State initialization - if(stateChanged) - { - //pc.printf("Entering HITTING \r\n"); - stateChanged = false; - //HitBall(); // Hit the ball - currentState = MOVING; - stateChanged = true; - break; - } break; } @@ -590,6 +539,7 @@ // Serial communication pc.baud(115200); + // Start values of LEDs led1.write(1); led2.write(1); led3.write(1); @@ -598,16 +548,15 @@ pc.printf("START \r\n"); - bqc.add( &bq1_low ).add( &bq2_high ).add( &bq3_notch ); + bqc.add(&bq1_low ).add(&bq2_high ).add(&bq3_notch ); // Make BiQuad Chain - 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); + processTicker.attach(&JointVelocities, processTs); // Ticker to process EMG + controllerTicker.attach(&MotorController, controllerTs); // Ticker to control motor 1 (PID) + emgRight.attach(&check_emg_r, emgTs); // Ticker to sample EMG of right arm + emgLeft.attach(&check_emg_l, emgTs); // Ticker to sample EMG of left arm - motor1MagnitudePin.period_ms(1); - motor2MagnitudePin.period_ms(1); - TurnMotorsOff(); + motor1MagnitudePin.period_ms(1); // PWM frequency of motor 1 (Should actually be 5 - 10 kHz) + motor2MagnitudePin.period_ms(1); // PWM frequency of motor 2 (Should actually be 5 - 10 kHz) while(true) {