![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
final version
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of Project_script_union_final by
Diff: main.cpp
- Revision:
- 35:63c890ac71ff
- Parent:
- 34:b8b18ba0c336
- Child:
- 36:650a9245bc44
--- a/main.cpp Mon Nov 05 15:19:42 2018 +0000 +++ b/main.cpp Mon Nov 05 16:45:17 2018 +0000 @@ -29,11 +29,11 @@ DigitalOut ledg (LED_GREEN); -MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off +MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off QEI encoder2 (D9, D8, NC, 8400,QEI::X4_ENCODING); QEI encoder1 (D12, D13, NC, 8400,QEI::X4_ENCODING); -//HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered +//HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered //Tickers Ticker func_tick; @@ -43,58 +43,59 @@ //Global variables -const float T = 0.002f; //Ticker period EMG, engine control -const float T2 = 0.2f; //Ticker print function +const float T = 0.002f; //Ticker period EMG, engine control +const float T2 = 0.2f; //Ticker print function //EMG filter double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2 double emg0_raw, emg1_raw, emg2_raw; double emg0_filt_x, emg1_filt_x, emg2_filt_x; -const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number) +const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated double sum, sum1, sum2, sum3; //variables used to sum elements in array double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg -double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??) +double movAg0, movAg1, movAg2; //Outcome of MovAg //Calibration variables int x = -1; //Start switch, colour LED is blue. -int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!) -const int sizeCal = 1500; //size of the dataset used for calibration, eerst 2000 -double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in -double Mean0,Mean1,Mean2; //average of maximum tightening +int emg_cal = 0; //If emg_cal is set to 1, motors can begin to work in this code +const int sizeCal = 1500; //Size of the dataset used for calibration +double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //Arrays to put the dataset of the calibration in +double Mean0,Mean1,Mean2; //Average of maximum contraction double Threshold0, Threshold1, Threshold2; -//Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo) +//Biquad //Variables for the biquad band filters BiQuadChain emg0filter; BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); -BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients +BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients BiQuadChain emg1filter; BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); -BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter +BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); BiQuadChain emg2filter; BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); -BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter +BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Variables PID controller -double PI = 3.14159; -double Kp1 = 20.0; //Motor 1 -double Ki1 = 1.02; -double Kd1 = 1.0; -double encoder_radians1=0; -double err_integral1 = 0; -double err_prev1, err_prev2; -double err1, err2; -BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //sample frequency 500 Hz, cutoff 20Hz low pass +double PI = 3.14159; //Pi value -double Kp2 = 20.0; //Motor 2 +double Kp1 = 20.0; //Proportional gain motor 1 +double Ki1 = 1.02; //Integrative term motor 1 +double Kd1 = 1.0; //Differential term motor 1 +double encoder_radians1=0; //Inital encoder value motor 1 +double err_integral1 = 0; //Initial error integral value motor 1 +double err_prev1, err_prev2; //Variables called previous error motor 1 and motor 2 +double err1, err2; //Variables called error motor 1 and motor 2 +BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //Low_pass differential term Sample frequency 500 Hz, cutoff 20Hz low pass + +double Kp2 = 20.0; / //Motor 2 double Ki2 = 1.02; double Kd2 = 1.0; double encoder_radians2=0; @@ -103,27 +104,27 @@ BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); // Inverse Kinematica variables -//const double L1 = 0.208; // Hoogte van tafel tot joint 1 -//const double L2 = 0.288; // Hoogte van tafel tot joint 2 -const double L3 = 0.212; // Lengte van de arm -//const double L4 = 0.089; // Afstand van achterkant base tot joint 1 -//const double L5 = 0.030; // Afstand van joint 1 naar joint 2 -const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation +//const double L1 = 0.208; // Height table to joint 1 +//const double L2 = 0.288; // Height table to joint 2 +const double L3 = 0.212; // Length arm +//const double L4 = 0.089; // Distance backside base to joint 1 +//const double L5 = 0.030; // Distance from joint 1 to joint 2 +const double r_trans = 0.035; // Calculate translation to shaft rotation // Variërende variabelen inverse kinematics: -double q1ref = 0.0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is -double q2ref = 0.0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is -double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals -double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals +double q1ref = 0.0; // Current motor angle of joint 1 as determined out of reference signal +double q2ref = 0.0; // Current motor angle of joint 2 as determined out of reference signal +double v_x; // Desired velocity of end effector in x direction --> Determined by EMG signals +double v_y; // Desired velocity of end effector in y direction --> Determined by EMG signals -//double Lq1; // Translatieafstand als gevolg van motor rotation joint 1 -//double Cq2; // Joint angle of the system (corrected for gear ratio 1:5) +//double Lq1; // Translational distance due to motor rotation joint 1 +//double Cq2; // Joint angle of the system (corrected for gear ratio 1:5) -double q1_dot=0.0; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken -double q2_dot=0.0; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken +double q1_dot=0.0; // Required angular velocity of motor 1 to reach v_des +double q2_dot=0.0; // Required angular velocity of motor 2 to reach v_des -double q1_ii=0.0; // Reference signal for motorangle q1ref -double q2_ii=0.0; // Reference signal for motorangle q2ref +double q1_ii=0.0; // Reference signal for motorangle q1ref +double q2_ii=0.0; // Reference signal for motorangle q2ref double q1_motor; double q2_motor; @@ -131,42 +132,45 @@ //--------------Functions----------------------------------------------------------------------------------------------------------------------------// -//------------------ Filter EMG + Calibration EMG --------------------------------// +//------------------ Filter EMG + Calibration EMG --------------------------------------------------------------------------------------------------// void switch_to_calibrate() { - x++; //Every time function gets called, x increases. Every button press --> new calibration state. - //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1. + //Every time function gets called, x increases. Every button press --> new calibration state. + //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1. + + x++; + - if(x==0) //If x = 0, led is red + if(x==0) //If x = 0, led is red { ledr = 0; ledb = 1; ledg = 1; } - else if (x==1) //If x = 1, led is blue + else if (x==1) //If x = 1, led is blue { ledr = 1; ledb = 0; ledg = 1; } - else if (x==2) //If x = 2, led is green + else if (x==2) //If x = 2, led is green { ledr = 1; ledb = 1; ledg = 0; } - else //If x = 3 or 4, led is white + else //If x = 3 or 4, led is white { ledr = 0; ledb = 0; ledg = 0; } - if(x==4) //Reset back to x = -1 + if(x==4) //Reset back to x = -1 { x = -1; - emg_cal=0; //reset, motors off + emg_cal=0; //Reset, motors off } } @@ -175,23 +179,23 @@ { switch(x) { - case 0: //If calibration state 0: + case 0: //If calibration state 0: { sum = 0.0; - for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0 + for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0 { StoreCal0[j] = emg0_filt; sum+=StoreCal0[j]; - wait(0.001f); //Does there need to be a wait? + wait(0.001f); } - Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples) - Threshold0 = Mean0*0.5; //Threshold calculation calve = 0.8*mean - break; //Stop. Threshold is calculated, we will use this further in the code + Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set + Threshold0 = Mean0*0.5; //Threshold calculation calve = 0.8*mean + break; //Stop. Threshold is calculated. } - case 1: //If calibration state 1: + case 1: //If calibration state 1: { sum = 0.0; - for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1 + for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1 { StoreCal1[j] = emg1_filt; sum+=StoreCal1[j]; @@ -200,11 +204,11 @@ Mean1 = sum/sizeCal; Threshold1 = Mean1/2; break; - } - case 2: //If calibration state 2: + } + case 2: //If calibration state 2: { sum = 0.0; - for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2 + for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2 { StoreCal2[j] = emg2_filt; sum+=StoreCal2[j]; @@ -214,14 +218,14 @@ Threshold2 = Mean2/2; break; } - case 3: //EMG is calibrated, robot can be set to Home position. + case 3: //EMG is calibrated, robot can be set to Home position. { - emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!) + emg_cal = 1; //This is the setting for which the motors can begin turning in this code wait(0.001f); break; } - default: //Ensures nothing happens if x is not 0,1 or 2. + default: //Ensures nothing happens if x is not 0,1 or 2 { break; } @@ -230,35 +234,35 @@ void EMGFilter0() { - emg0_raw = emg0_in.read(); //give name to raw EMG0 data - emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data - emg0_filt = abs(emg0_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken. + emg0_raw = emg0_in.read(); //Give name to raw EMG0 data + emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data + emg0_filt = abs(emg0_filt_x); //Rectifier } void EMGFilter1() { - emg1_raw = emg1_in.read(); //give name to raw EMG1 data - emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data - emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch. + emg1_raw = emg1_in.read(); //Give name to raw EMG1 data + emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data + emg1_filt = abs(emg1_filt_x); //Rectifier } void EMGFilter2() { - emg2_raw = emg2_in.read(); //Give name to raw EMG1 data - emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data - emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier. + emg2_raw = emg2_in.read(); //Give name to raw EMG1 data + emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data + emg2_filt = abs(emg2_filt_x); //Rectifier } -void MovAg() //Calculate moving average (MovAg), klopt nog niet!! +void MovAg() //Calculate moving average (MovAg) { - for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals + for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals { - StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average. + StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average. StoreArray1[i] = StoreArray1[i-1]; StoreArray2[i] = StoreArray2[i-1]; } - StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array + StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array StoreArray1[0] = emg1_filt; StoreArray2[0] = emg2_filt; @@ -266,71 +270,47 @@ sum2 = 0.0; sum3 = 0.0; - for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays + for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays { sum1 += StoreArray0[a]; sum2 += StoreArray1[a]; sum3 += StoreArray2[a]; } - movAg0 = sum1/windowsize; //calculates an average in the array + movAg0 = sum1/windowsize; //calculates an average in the array movAg1 = sum2/windowsize; movAg2 = sum3/windowsize; - //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout. } -void emg_filtered() //Call all filter functions +void emg_filtered() //Call all filter functions { EMGFilter0(); EMGFilter1(); EMGFilter2(); } -/* -void HIDScope_sample() -{ - scope.set(0,emg0_raw); - scope.set(1,emg0_filt); - scope.set(1,movAg0); //als moving average werkt - scope.set(2,emg1_raw); - scope.set(3,emg1_filt); - scope.set(3,movAg1); //als moving average werkt - scope.set(4,emg2_raw); - scope.set(5,emg2_filt); - scope.set(5,movAg2); //als moving average werkt - - scope.send(); //Send data to HIDScope server -} -*/ - - -//---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------// +//---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------------------------------------------// void PID_control1() { - //pc.printf("ik doe het, PDI \n\r"); - // Proportional part: - double u_k1 = Kp1 * err1; + double u_k1 = Kp1 * err1; //Proportional gain times calculated error //Integral part - err_integral1 = err_integral1 + err1 * T; - double u_i1 = Ki1 * err_integral1; + err_integral1 = err_integral1 + err1 * T; //Adds the error*T + double u_i1 = Ki1 * err_integral1; //Integral term times the integral // Derivative part - double err_derivative1 = (err1 - err_prev1)/T; - double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1); - double u_d1 = Kd1 * filtered_err_derivative1; - err_prev1 = err1; + double err_derivative1 = (err1 - err_prev1)/T; //error - previous error /T + double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1); //Filter the derivative term for stabilization + double u_d1 = Kd1 * filtered_err_derivative1; //Derivative term times the derivative error + err_prev1 = err1; //Sets the current error to previous error (remember) - - // Sum all parts and return it - u1 = u_k1 + u_i1 + u_d1; + // Sum all parts and return it + u1 = u_k1 + u_i1 + u_d1; } void PID_control2() { - //pc.printf("ik doe het, PDI \n\r"); - // Proportional part: double u_k2 = Kp2 * err2; @@ -344,196 +324,124 @@ double u_d2 = Kd2 * filtered_err_derivative2; err_prev2 = err2; - - // Sum all parts and return it + // Sum all parts and return it u2 = u_k2 + u_i2 + u_d2; } -void engine_control1() //Engine 1 is translational engine, connected with left side pins +void engine_control1() //Engine 1 is translational engine, connected with left side pins { encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0; - err1 = q1_motor - encoder_radians1; - PID_control1(); //PID controller function call - - //if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600 - //{ - pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! - directionpin1.write(u1<0); - //} - //else - // { - // pwmpin1 = 0; - // } + err1 = q1_motor - encoder_radians1; //Calculate error between desired angle 1 and current angle 1 + PID_control1(); //PID 1 controller function call + pwmpin1 = fabs(u1); //Motor 1 speed set + directionpin1.write(u1<0); //Direction motor 1 set } -void engine_control2() //Engine 2 is rotational engine, connected with right side wires +void engine_control2() //Engine 2 is rotational engine, connected with right side wires { encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0; - //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses()); - //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2); - err2 = q2_motor - encoder_radians2; - //pc.printf("err2 = %f\n\r",err2); - PID_control2(); //PID controller function call - //pc.printf("u2 = %f\n\r",u2); - - //if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts - // { - pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! - directionpin2.write(u2>0); - // } - //else - // { - // pwmpin2 = 0; - // } + err2 = q2_motor - encoder_radians2; //Calculate error between desired angle 2 and current angle 2 + PID_control2(); //PID 2 controller function call + pwmpin2 = fabs(u2); //Motor 2 speed set + directionpin2.write(u2>0); //Direction motor 2 set } -/*void engine_control1() //Engine 1 is translational engine, connected with left side pins -{ - encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0; - err1 = q1_motor - encoder_radians1; - PID_control1(); //PID controller function call - - if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600 - { - pwmpin1 = fabs(u1); - directionpin1.write(u1<0); - } - else - { - pwmpin1 = fabs(u1); - directionpin1.write(u1>0); - } -} - -void engine_control2() //Engine 2 is rotational engine, connected with right side wires -{ - encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0; - err2 = q2_motor - encoder_radians2; - PID_control2(); //PID controller function call - - if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts - { - pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! - directionpin2.write(u2>0); - } - else - { - pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! - directionpin2.write(u2<0); - } -} -*/ - -//------------------ Inversed Kinematics --------------------------------// +//------------------ Inversed Kinematics -----------------------------------------------------------------------------------------------------------// void inverse_kinematics() { - q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref); //RKI systeem - q2_dot = v_y/(L3*cos(q2ref)); // - - q1_ii = q1ref + q1_dot*T; //Omgezet naar motorhoeken - q2_ii = q2ref + q2_dot*T; + q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref); //Calculate desired angular velocity of motor 1 + q2_dot = v_y/(L3*cos(q2ref)); //Calculate desired angular velocity of motor 2 + + q1_ii = q1ref + q1_dot*T; //Adds the desired angle of motor 1 to the reference angle + q2_ii = q2ref + q2_dot*T; //Adds the desired angle of motor 2 to the reference angle - q1ref = q1_ii; - q2ref = q2_ii; + q1ref = q1_ii; //Makes new qref + q2ref = q2_ii; //Makes new qref - q1_motor = -q1ref/r_trans; - q2_motor = q2ref*5.0; + q1_motor = -q1ref/r_trans; //Sets the angle at which motor 1 needs to go, with ratio rotation/translation + q2_motor = q2ref*5.0; //Sets the angle at which motor 2 needs to go, scaled by 5 - engine_control1(); - engine_control2(); + engine_control1(); //Call engine_control 1 function + engine_control2(); //Call engine_control 2 function } void v_des_calculate_qref() { - while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. + while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. { - if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0) + if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0) { - v_x = 0.05; //movement in +x direction + v_x = 0.05; //Movement in +x direction v_y = 0.0; - ledr = 0; //red + ledr = 0; //Led is red ledb = 1; ledg = 1; } - else if(movAg2>Threshold2 && movAg0<Threshold0) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0) + else if(movAg2>Threshold2 && movAg0<Threshold0) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0) { - v_y = 0.05; //Movement in +y direction + v_y = 0.05; //Movement in +y direction v_x = 0.0; - ledr = 1; //Green + ledr = 1; //Led is green ledb = 1; ledg = 0; } - else if(movAg0>Threshold0 && movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0) + else if(movAg0>Threshold0 && movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0) { - v_y = 0.0; //Movement in -x direction + v_y = 0.0; //Movement in -x direction v_x = -0.05; - ledr = 0; //Purple + ledr = 0; //Led is purple ledb = 0; ledg = 1; } - else if(movAg0>Threshold0 && movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0) + else if(movAg0>Threshold0 && movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0) { - v_y = -0.05; //Movement in -y direction + v_y = -0.05; //Movement in -y direction v_x = 0.0; - ledr = 1; //Blue + ledr = 1; //Led is blue ledb = 0; ledg = 1; } - else //If not higher than any threshold, motors will not turn at all + else //If not higher than any threshold, motors will not turn at all { v_x = 0; v_y = 0; - ledr = 0; //White + ledr = 0; //Led is white ledb = 0; ledg = 0; } - inverse_kinematics(); //Call inverse kinematics function + inverse_kinematics(); //Call inverse kinematics function break; } } -void printFunction() -{ - pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2); - pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2); -} - - - -//------------------ Start main function --------------------------// - +//------------------ Start main function -----------------------------------------------------------------------------------------------------------// int main() { - pc.baud(115200); - pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off. - pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz + pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz - emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //attach biquad elements to chain + emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //Attach biquad elements to chain emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 ); emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( ¬ch3 ); - emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec. + emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec. movag_tick.attach(&MovAg,T); - func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T - print_tick.attach(&printFunction,T2); + func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T - button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle) - //wait(0.2f); //Wait to avoid bouncing of button - button2.rise(calibrate); //Calibrate threshold for 3 muscles - //wait(0.2f); //Wait to avoid bouncing of button + button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle) + button2.rise(calibrate); //Calibrate threshold for 3 muscles while(true) {