Nieuwe kinematica + potmeter
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of Project_script_union_potmeter by
Diff: main.cpp
- Revision:
- 28:61d1372349c8
- Parent:
- 27:fa493551be99
- Child:
- 29:df10cb76ef26
diff -r fa493551be99 -r 61d1372349c8 main.cpp --- a/main.cpp Wed Oct 31 12:38:00 2018 +0000 +++ b/main.cpp Wed Oct 31 19:14:56 2018 +0000 @@ -10,9 +10,8 @@ // set HIDScope to version 7 // set biquadFilter to version 7 -AnalogIn emg0_in (A0); //First raw EMG signal input -AnalogIn emg1_in (A1); //Second raw EMG signal input -AnalogIn emg2_in (A2); //Third raw EMG signal input +//AnalogIn potmeter1 (A0); //First raw EMG signal input +//AnalogIn potmeter2 (A1); //Second raw EMG signal input InterruptIn encoderA1 (D9); InterruptIn encoderB1 (D8); @@ -38,53 +37,20 @@ //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered //Tickers -Ticker ticker; +Ticker func_tick; +Ticker engine_control1_tick; +Ticker engine_control2_tick; //Global variables const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders??? - -//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) -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??) - -//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 -double Threshold0, Threshold1, Threshold2; - -//Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo) -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 - -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 - -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 +const float T2 = 0.01f; // 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 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 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 // Variërende variabelen inverse kinematics: @@ -104,19 +70,23 @@ //Variables PID controller double PI = 3.14159; -double Kp1 = 17.5; //Motor 1 +double Kp1 = 5.0; //Motor 1 eerst 17.5 , nu 5 double Ki1 = 1.02; double Kd1 = 23.2; double encoder1 = 0; double encoder_radians1=0; -double Kp2 = 17.5; //Motor 2 +double Kp2 = 5.0; //Motor 2 eerst 17.5, nu 5 double Ki2 = 1.02; double Kd2 = 23.2; double encoder2 = 0; double encoder_radians2=0; -double start_control = 0; +int start_control = 0; + +//double potmeter1s = (potmeter1*2)-1.0f; +//double potmeter2s = (potmeter2*2)-1.0f; +double emg_cal = 1; //--------------Functions----------------------------------------------------------------------------------------------------------------------------// @@ -240,178 +210,128 @@ //------------------ 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; -} - -void emg_filtered() //Call all filter functions -{ - EMGFilter0(); - EMGFilter1(); - EMGFilter2(); - MovAg(); -} -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. - - 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 - { - ledr = 1; - ledb = 0; - ledg = 1; - } - 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 - { - ledr = 0; - ledb = 0; - ledg = 0; - } - - if(x==4) //Reset back to x = -1 - { - x = -1; - emg_cal=0; //reset, motors off - } -} - - -void calibrate(void) -{ - switch(x) - { - 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 - { - StoreCal0[j] = emg0_filt; - sum+=StoreCal0[j]; - wait(0.001f); //Does there need to be a wait? - } - Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples) - Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean - break; //Stop. Threshold is calculated, we will use this further in the code - } - 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 - { - StoreCal1[j] = emg1_filt; - sum+=StoreCal1[j]; - wait(0.001f); - } - Mean1 = sum/sizeCal; - Threshold1 = Mean1/2; - break; - } - 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 - { - StoreCal2[j] = emg2_filt; - sum+=StoreCal2[j]; - wait(0.001f); - } - Mean2 = sum/sizeCal; - Threshold2 = Mean2/2; - break; - } - 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 (!!) - - wait(0.001f); - break; - } - default: //Ensures nothing happens if x is not 0,1 or 2. - { - break; - } - } -} -/* -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 -} -*/ //------------------ Inversed Kinematics --------------------------------// + +//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------// +double PID_controller1(double err1) +{ + pc.printf("ik doe het, PDI 1\n\r"); + + 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+0; //+ u_i1 + u_d1; +} + +void start_your_engines1(double u1) +{ + pc.printf("ik doe het, engine start 1\n\r"); + + if(encoder1<5250 && encoder1>-5250) //limits rotation, in counts + { + pwmpin1 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! + directionpin1.write(0); + } + else + { + pwmpin1 = 0; + } +} + +void engine_control1() //Engine 1 is rotational engine, connected with left side pins +{ + //while(start_control == 1) + //{ + pc.printf("ik doe het, engine control 1\n\r"); + encoder_radians1 = encoder1*(2*PI)/8400; + double err1 = q1ref - encoder_radians1; + double u1 = PID_controller1(err1); //PID controller function call + start_your_engines1(u1); + + // break; + //} +} + + + +//---------PID controller motor 2 + start motor 2 -----------------------------------------------------------// +double PID_controller2(double err2) +{ + pc.printf("ik doe het, PDI 2\n\r"); + + 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+0; //+ u_i2 + u_d2; +} + +void start_your_engines2(double u2) +{ + pc.printf("ik doe het, engine start 2\n\r"); + + if(encoder2<12600 && encoder2>-1) //limits translation in counts + { + pwmpin2 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! + directionpin2.write(0); + } + else + { + pwmpin2 = 0; + } + +} + +void engine_control2() //Engine 2 is translational engine, connected with right side wires +{ + pc.printf("ik doe het, engine control 2\n\r"); + + 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 +} + + void inverse_kinematics() { + + pc.printf("ik doe het, inverse kinematics\n\r"); Lq1 = q1ref*r_trans; Cq2 = q2ref/5.0; @@ -424,36 +344,37 @@ q1ref = q1_ii; q2ref = q2_ii; - start_control = 1; + //start_control = 1; + engine_control1(); + engine_control2(); } void v_des_calculate_qref() { - if(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(button1==0) //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 + v_x = 0.5f; //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(button2==0) //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 + v_y = 0.5f; //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(button1==0 && button2==0) //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; + v_x = -0.5f; + v_y = -0.5f; ledr = 1; //Blue ledb = 0; ledg = 1; } + */ else //If not higher than the threshold, motors will not turn at all { v_x = 0; @@ -461,116 +382,12 @@ ledr = 0; //white ledb = 0; ledg = 0; + //pwmpin1 = 0; + //pwmpin2 = 0; } 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 -{ - while(start_control == 1) - { - 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 - - break; - } -} - - - -//---------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 -{ - while(start_control == 1) - { - 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 - - break; - } + } //------------------ Start main function --------------------------// @@ -581,26 +398,14 @@ 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 - - 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 ); + + func_tick.attach(&v_des_calculate_qref,T2); //v_des determined every T + //engine_control1_tick.attach(&engine_control1,T2); + //engine_control2_tick.attach(&engine_control2,T2); + + // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. + while(true) - { - ticker.attach(&emg_filtered,T); //EMG signals filtered + moving average 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. - - 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); - } + {;} }