![](/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:
- 26:ac5656aa35c7
- Parent:
- 25:bbef09ff226b
- Child:
- 27:fa493551be99
--- a/main.cpp Wed Oct 31 09:28:45 2018 +0000 +++ b/main.cpp Wed Oct 31 10:36:01 2018 +0000 @@ -14,11 +14,17 @@ AnalogIn emg1_in (A1); //Second raw EMG signal input AnalogIn emg2_in (A2); //Third raw EMG signal input -InterruptIn button1 (D10); //Is this one available? We need to make a map of which pins are used for what. +InterruptIn encoderA1 (D9); +InterruptIn encoderB1 (D8); +InterruptIn encoderA2 (D12); +InterruptIn encoderB2 (D13); + +InterruptIn button1 (D10); InterruptIn button2 (D11); DigitalOut directionpin1 (D7); DigitalOut directionpin2 (D4); + PwmOut pwmpin1 (D6); PwmOut pwmpin2 (D5); @@ -73,14 +79,13 @@ 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 -// Inverse Kinematica variabelen +// 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 T = 0.002f; // Ticker value // Variërende variabelen inverse kinematics: double q1ref = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is @@ -97,9 +102,199 @@ double q1_ii; // Reference signal for motorangle q1ref double q2_ii; // Reference signal for motorangle q2ref +//Variables PID controller +double PI = 3.14159; +double Kp1 = 17.5; //Motor 1 +double Ki1 = 1.02; +double Kd1 = 23.2; +double encoder1 = 0; +double encoder_radians1=0; -//Functions +double Kp2 = 17.5; //Motor 2 +double Ki2 = 1.02; +double Kd2 = 23.2; +double encoder2 = 0; +double encoder_radians2=0; + + +//--------------Functions----------------------------------------------------------------------------------------------------------------------------// + + +//------------------ Encoder motor 1 --------------------------------// + +void encoderA1_rise() +{ + if(encoderB1==false) + { + encoder1++; + } + else + { + encoder1--; + } +} + +void encoderA1_fall() +{ + if(encoderB1==true) + { + encoder1++; + } + else + { + encoder1--; + } +} + +void encoderB1_rise() +{ + if(encoderA1==true) + { + encoder1++; + } + else + { + encoder1--; + } +} + +void encoderB1_fall() +{ + if(encoderA1==false) + { + encoder1++; + } + else + { + encoder1--; + } +} + +void encoder_count1() +{ + encoderA1.rise(&encoderA1_rise); + encoderA1.fall(&encoderA1_fall); + encoderB1.rise(&encoderB1_rise); + encoderB1.fall(&encoderB1_fall); +} + +//------------------ Encoder motor 2 --------------------------------// + +void encoderA2_rise() +{ + if(encoderB2==false) + { + encoder2++; + } + else + { + encoder2--; + } +} +void encoderA2_fall() +{ + if(encoderB2==true) + { + encoder2++; + } + else + { + encoder2--; + } +} + +void encoderB2_rise() +{ + if(encoderA2==true) + { + encoder2++; + } + else + { + encoder2--; + } +} + +void encoderB2_fall() +{ + if(encoderA2==false) + { + encoder2++; + } + else + { + encoder2--; + } +} + +void encoder_count2() +{ + encoderA2.rise(&encoderA2_rise); + encoderA2.fall(&encoderA2_fall); + encoderB2.rise(&encoderB2_rise); + encoderB2.fall(&encoderB2_fall); +} + +//------------------ Filter EMG + Calibration EMG --------------------------------// + +void EMGFilter0() +{ + emg0_raw = emg0_in.read(); //give name to raw EMG0 data calve + 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. +} + +void EMGFilter1() +{ + emg1_raw = emg1_in.read(); //give name to raw EMG1 data bicep 1 + 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. +} + +void EMGFilter2() +{ + emg2_raw = emg2_in.read(); //Give name to raw EMG1 data bicep 2 + 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. +} + +void MovAg() //Calculate moving average (MovAg) +{ + 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. + 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 + StoreArray1[0] = emg1_filt; + StoreArray2[0] = emg2_filt; + + sum1 = 0.0; + sum2 = 0.0; + sum3 = 0.0; + + 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 + 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 +{ + EMGFilter0(); + EMGFilter1(); + EMGFilter2(); +} void switch_to_calibrate() { x++; //Every time function gets called, x increases. Every button press --> new calibration state. @@ -193,80 +388,24 @@ } } } - +/* void HIDScope_sample() { - /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ - //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 -} + 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 -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. -} - -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. -} - -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. + scope.send(); //Send data to HIDScope server } - -void MovAg() //Calculate moving average (MovAg), klopt nog niet!! -{ - 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. - 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 - StoreArray1[0] = emg1_filt; - StoreArray2[0] = emg2_filt; - - sum1 = 0.0; - sum2 = 0.0; - sum3 = 0.0; - - 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 - 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 -{ - EMGFilter0(); - EMGFilter1(); - EMGFilter2(); -} +//------------------ Inversed Kinematics --------------------------------// void inverse_kinematics() { @@ -285,51 +424,148 @@ 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) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn + if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn { - v_x = 1.0; //beweging in +x direction - ledr = 0; //red + v_x = 1.0; //beweging in +x direction + ledr = 0; //red ledb = 1; ledg = 1; } - else if(movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn + else if(movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn { - v_y = 1.0; //beweging in +y direction - ledr = 1; //green + v_y = 1.0; //beweging in +y direction + ledr = 1; //green ledb = 1; ledg = 0; } - else if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction + else if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction { v_x = -v_x; v_y = -v_y; - ledr = 1; //Blue + ledr = 1; //Blue ledb = 0; ledg = 1; } - else //If not higher than the threshold, motors will not turn at all + else //If not higher than the threshold, motors will not turn at all { v_x = 0; v_y = 0; - ledr = 0; //white + ledr = 0; //white ledb = 0; ledg = 0; } break; } - inverse_kinematics(); + + inverse_kinematics(); //Call inverse kinematics function +} + +//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------// +double PID_controller1(double err1) +{ + static double err_integral1 = 0; + static double err_prev1 = err1; // initialization with this value only done once! + + static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); + + // Proportional part: + double u_k1 = Kp1 * err1; + + // Integral part + err_integral1 = err_integral1 + err1 * T; + double u_i1 = Ki1 * err_integral1; + + // Derivative part + double err_derivative1 = (err1 - err_prev1)/T; + double filtered_err_derivative1 = LowPassFilter1.step(err_derivative1); + double u_d1 = Kd1 * filtered_err_derivative1; + err_prev1 = err1; + + // Sum all parts and return it + return u_k1 + u_i1 + u_d1; +} + +void start_your_engines1(double u1) +{ + if(encoder1<5250 && encoder1>-5250) //limits rotation, in counts + { + pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! + directionpin1.write(u1 < 0.0f); + } + else + { + pwmpin1 = 0; + } +} + + void engine_control1() //Engine 1 is rotational engine, connected with left side pins +{ + encoder_radians1 = encoder1*(2*PI)/8400; + double err1 = q1ref - encoder_radians1; + double u1 = PID_controller1(err1); //PID controller function call + start_your_engines1(u1); //Call start_your_engines function } + +//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------// +double PID_controller2(double err2) +{ + static double err_integral2 = 0; + static double err_prev2 = err2; // initialization with this value only done once! + + static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); + + // Proportional part: + double u_k2 = Kp2 * err2; + + // Integral part + err_integral2 = err_integral2 + err2 * T; + double u_i2 = Ki2 * err_integral2; + + // Derivative part + double err_derivative2 = (err2 - err_prev2)/T; + double filtered_err_derivative2 = LowPassFilter2.step(err_derivative2); + double u_d2 = Kd2 * filtered_err_derivative2; + err_prev2 = err2; + + // Sum all parts and return it + return u_k2 + u_i2 + u_d2; +} + +void start_your_engines2(double u2) +{ + if(encoder2<12600 && encoder2>-1) //limits translation in counts + { + pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! + directionpin2.write(u2 < 0.0f); + } + else + { + pwmpin2 = 0; + } +} + +void engine_control2() //Engine 2 is translational engine, connected with right side wires +{ + encoder_radians2 = encoder2*(2*PI)/8400; + double err2 = q2ref - encoder_radians2; + double u2 = PID_controller2(err2); //PID controller function call + start_your_engines2(u2); //Call start_your_engines 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 + 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 emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //attach biquad elements to chain emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 ); @@ -337,20 +573,20 @@ while(true) { - ticker.attach(&emg_filtered,T); //EMG signals filtered every T sec. - ticker.attach(&MovAg,T); //Moving average calculation every T sec. - ticker.attach(&v_des_calculate_qref,T); //v_des determined every T + ticker.attach(&emg_filtered,T); //EMG signals filtered every T sec. + ticker.attach(&MovAg,T); //Moving average calculation every T sec. + ticker.attach(&v_des_calculate_qref,T); //v_des determined every T - // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. + // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. - 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) + 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 - pc.printf("x is %i\n\r",x); - 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); - //wait(2.0f); + pc.printf("x is %i\n\r",x); + 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); + //wait(2.0f); } }