werkend x-y control
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 9:5140b3a95dc9
- Parent:
- 8:649a5f555b7b
diff -r 649a5f555b7b -r 5140b3a95dc9 main.cpp --- a/main.cpp Fri Oct 23 11:16:46 2015 +0000 +++ b/main.cpp Fri Oct 23 19:30:39 2015 +0000 @@ -86,7 +86,7 @@ double phi_two; // define start up variables (is to create a delayed start up instead of going to the ref value inmediately) - double start_up_time = 2; + double start_up_time = 3; double start_loops = start_up_time*controlfreq; int rc1 = 0; // counters in function to enable slow start up int rc2 = 0; @@ -95,6 +95,9 @@ double reset_phi_one = 0; double reset_phi_two = 0; +double phi_one_curr = 0; +double phi_two_curr = 0; + // REFERENCE SETTINGS double input_threshold = 0.25; // the minimum value the signal must have to change the reference. // Define storage variables for reference values (also start position) @@ -229,17 +232,6 @@ return y; } -// 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) -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; - double yt = b0t*vt+b1t*v1t+b2t*v2t; - v2t = v1t; - v1t = vt; - return yt; -} - // PID Controller given in sheets // 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) @@ -300,20 +292,20 @@ // 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 y1t = biquadfilter( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); + double y2t = biquadfilter( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); + double y3t = biquadfilter( 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); + double y5t = biquadfilter( 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); - //scope.set(1,output2); - //scope.set(0,output1_amp); - //scope.set(1,output2_amp); - //scope.send(); + scope.set(0,output1); + scope.set(1,output2); + scope.set(0,output1_amp); + scope.set(1,output2_amp); + scope.send(); } @@ -418,13 +410,9 @@ if(rc1 < start_loops) { rc1++; - reference1 = ((double) rc1/start_loops)*reference1; + reference1 = phi_one_curr + ((double) rc1/start_loops)*(reference1-phi_one_curr); } - else - { - reference1 = reference1; - } - + 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); @@ -449,13 +437,8 @@ if(rc2 < start_loops) { rc2++; - reference2 = ((double) rc2/start_loops)*reference2; + reference2 = phi_two_curr + ((double) rc2/start_loops)*(reference2-phi_two_curr); } - else - { - reference2 = reference2; - } - double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor double error2 = (reference2 - rads2); // determine the error (reference - position) @@ -541,9 +524,12 @@ { reset = !reset; wait(reset_button.read() == 1); - wait(0.3); + phi_one_curr = phi_one; + phi_two_curr = phi_two; rc1 = 0; - rc2 = 0; + rc2 = 0; + wait(0.3); + } ////////////////////////////////////////////////// // Main Control stuff and options