werkend x-y control
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 5:867fe891b990
- Parent:
- 4:c371fc59749e
- Child:
- 6:bfd24400e9d0
--- a/main.cpp Wed Oct 21 14:02:07 2015 +0000 +++ b/main.cpp Thu Oct 22 11:34:39 2015 +0000 @@ -6,7 +6,6 @@ ///// points..... mooi maken calib -AnalogOut setpoint_analog(DAC0_OUT); Serial pc(USBTX,USBRX); HIDScope scope(4); // definieerd het aantal kanalen van de scope @@ -39,9 +38,16 @@ /////////////////////////// // Main control loop transfer variables // here all variables that transfer bewtween the primary control functions + +// filter to calibration double output1; double output2; +// filter to x-y +double output1_amp; +double output2_amp; + +// x-y to motor control double phi_one; double phi_two; @@ -78,9 +84,6 @@ // REFERENCE SIGNAL SETTINGS double input_threshold = 0.25; // the minimum value the signal must have to change the reference. - // Define signal amplification (needed with EMG, used in control loop, precise amp determination is a work in progress!!!!) ?? - double signalamp1 = 1; - double signalamp2 = 1; // Define storage variables for reference values double c_reference1 = 0; double c_reference2 = 0; @@ -120,15 +123,11 @@ double m2_prev_err = 0; // EMG calibration variables -double emg_gain1 = 7; -double emg_gain2 = 7; - -////// MOET NETTER!!!!! -double output1_1 = 0; -double output2_1 = 0; - +double emg_gain1 = 1; // set to one for unamplified signal +double emg_gain2 = 1; double cal_samples = 25; +double normalize_emg_value = 1; // set te desired value to calibrate the signal to //// FILTER VARIABLES // storage variables @@ -214,7 +213,6 @@ // This function takes the controller outputvalue and ensures it is between -1 and 1 // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction). -// needs more work to use it for the wind-up prevention. double outputlimiter (double output, double limit) { if(output> limit) @@ -248,6 +246,7 @@ } // biquadfilters die bij het filteren van signaal 2 horen, copy paste, alle waardes zijn veranderd naar +t (t van two of twee) +// (niet netjes maar werkt goed) double biquadfiltert(double ut, double &v1t, double &v2t, const double a1t, const double a2t, const double b0t, const double b1t, const double b2t) { double vt = ut- a1t*v1t-a2t*v2t; @@ -258,7 +257,7 @@ } // PID Controller given in sheets -// aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!) +// aangepast om zelfde filter te gebruiken en om de termen te splitsen double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev) { // Proportional @@ -277,39 +276,103 @@ return output; } -void calibrate_amp() + double angle_limits(double phi, double limlow, double limhigh) { - //double total1 = 0; - //for(int i = 0; i<cal_samples; i++) - //{ - // double input1 = EMG_in.read(); - // total1 = total1 + input1; - // wait(0.1); - //} - emg_gain1 = 1/output1_1; //1/(total1/cal_samples); - - //double total2 = 0; - //for(int i = 0; i<cal_samples; i++) - //{ - // double input2 = output2_1; - // total2 = total2 + input2; - // wait(0.1); - //} - emg_gain2 = 1/output2_1; - pc.printf("gain1 = %f, gain2 = %f",emg_gain1,emg_gain2); + if(phi < limlow) + { + phi = limlow; + } + if(phi > limhigh) + { + phi = limhigh; + } + return phi; +} -} ///////////////////////////////////////////////////////////////////// ////////////////// PRIMARY CONTROL FUNCTIONS /////////////////////// /////////////////////////////////////////////////////////////////// // these functions are called by go-flags and are used to update main variables and send signals to motor +// function that updates the inputs +void EMG_filter() +{ +// filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass + double u1 = EMG_in.read(); + double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); + double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); + double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3); + double y4 = abs(y3); + double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3); + // update global variables + output1 = y5; + output1_amp = y5*emg_gain1; // update global variable + +// filteren van EMG signaal 2 (A2), zelfde proces als signaal 1 + double u1t = EMG_int.read(); + double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); + double y2t = biquadfiltert( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); + double y3t = biquadfiltert( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3); + double y4t = abs(y3t); + double y5t = biquadfiltert( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3); + // update global variables + output2 = y5t; + output2_amp = y5t*emg_gain2; + + scope.set(0,output1_amp); + scope.set(1,output2_amp); + scope.send(); + } + + + // function that updates the required motor angles + void det_angles() +{ + + + if(output1>1) {output1 = 1;} + if(output2>1) {output2 = 1;} + + output1 = potright.read(); + output2 = potleft.read(); + + double xx = 50+output1_amp*20; + + double ymin = - sqrt(4900 - pow(xx,2)); + double ymax = sqrt(4900 - pow(xx,2)); + double yy = ymin+output2_amp*(ymax-ymin); + double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector + double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm + double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r + + double theta_one = (atan2(yy,xx)+beta); + double theta_two = (-pi + alfa); + + double phi1 = 4*(theta_one) + 2.8; + double phi2 = 4*(theta_one+theta_two) + 1.85; + phi2 = -phi2; + + // check the input angles and apply the limits + phi1 = angle_limits(phi1,limlow1,limhigh1); + phi2 = angle_limits(phi2,limlow2,limhigh2); + + // smooth the input signal (lowpass 1Hz) + phi1 = biquadfilter(phi1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2); + phi2 = biquadfilter(phi2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2); + + // write into global variables + phi_one = phi1; + phi_two = phi2; + + pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two); +} // MOTOR 1 void motor1_control() { double reference1 = phi_one; + // add smooth start up if(rc1 < start_loops) { rc1++; @@ -319,7 +382,7 @@ { reference1 = reference1; } - // m_input1 = biquadfilter(m_input1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2); //biquad with diff-filter coefficients to smooth input + double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor double error1 = (reference1 - rads1); // determine the error (reference - position) double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err); @@ -363,84 +426,26 @@ } } - -// function that updates the inputs -void EMG_filter() -{ -// filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass - double u1 = EMG_in.read(); - double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); - double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); - double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3); - double y4 = abs(y3); - double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3); - output1_1 = y5; - output1 = y5*emg_gain1; // update global variable -// versturen van het input signaal u1 en het gefilterde signaal y5 naar HIDScope channel 0 en 1 - -// filteren van EMG signaal 2 (A2), zelfde proces als signaal 1 - double u1t = EMG_int.read(); - double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); - double y2t = biquadfiltert( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); - double y3t = biquadfiltert( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3); - double y4t = abs(y3t); - double y5t = biquadfiltert( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3); - output2_1 = y5t; - output2 = y5t*emg_gain2; // update global variable - - scope.set(0,output1); - scope.set(1,output2); - scope.send(); - } - - // function that updates the required motor angles - void det_angles() +// calibrate the emg-signal +// works bij taking a certain amount of samples adding them and then normalize to the desired value +void calibrate_amp() { - // potmeter debugging - // static double time = 0.0; - // const double time_inc = 0.005; - // setpoint_analog= 0.5+(sin(time)/2.0); - // time += time_inc; - - //output1 = potright.read(); + double total1 = 0; + double total2 = 0; - //output2 = potleft.read(); - - if(output1>1) {output1 = 1;} - if(output2>1) {output2 = 1;} - - double xx = 50+output1*20; - - double ymin = - 16; - double ymax = + 16; - double yy = ymin+output2*(ymax-ymin); - double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector - double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm - double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r - - double theta_one = (atan2(yy,xx)+beta); - double theta_two = (-pi + alfa); - - double phi1 = 4*(theta_one) + 2.8; - double phi2 = 4*(theta_one+theta_two) + 1.85; - phi2 = -phi2; - - - if(phi1 < limlow1){phi1 = limlow1;} - if(phi1 > limhigh1){phi1 = limhigh1;} - - if(phi2 < limlow2){phi2 = limlow2;} - if(phi2 > limhigh2){phi2 = limhigh2;} - - phi1 = biquadfilter(phi1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2); - phi2 = biquadfilter(phi2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2); - - phi_one = phi1; - phi_two = phi2; - - pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two); - - + for(int i = 0; i<cal_samples; i++) + { + EMG_filter(); // run filter + double input1 = output1; + total1 = total1 + input1; // sum inputs + + double input2 = output2; + total2 = total2 + input2; + wait(0.1); + } + emg_gain1 = normalize_emg_value/(total1/cal_samples); // normalize the amplification so that the maximum signal hits the desired one + emg_gain2 = normalize_emg_value/(total2/cal_samples); + pc.printf("gain1 = %f, gain2 = %f",emg_gain1,emg_gain2); } //////////////////////////////////////////////////////////////////