werkend x-y control
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 4:c371fc59749e
- Parent:
- 3:a73502236647
- Child:
- 5:867fe891b990
diff -r a73502236647 -r c371fc59749e main.cpp --- a/main.cpp Wed Oct 21 12:27:45 2015 +0000 +++ b/main.cpp Wed Oct 21 14:02:07 2015 +0000 @@ -4,6 +4,8 @@ #include "HIDScope.h" #include "math.h" + +///// points..... mooi maken calib AnalogOut setpoint_analog(DAC0_OUT); Serial pc(USBTX,USBRX); HIDScope scope(4); // definieerd het aantal kanalen van de scope @@ -50,8 +52,8 @@ AnalogIn potleft(A5); // Analoge input signalen defineren -AnalogIn EMG_in(A2); // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen -AnalogIn EMG_int(A3); // deze leest A3 uit +AnalogIn EMG_in(A0); // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen +AnalogIn EMG_int(A2); // deze leest A3 uit // BUTTONS // control flow @@ -118,8 +120,14 @@ double m2_prev_err = 0; // EMG calibration variables -double emg_gain1 = 0; -double emg_gain2 = 0; +double emg_gain1 = 7; +double emg_gain2 = 7; + +////// MOET NETTER!!!!! +double output1_1 = 0; +double output2_1 = 0; + + double cal_samples = 25; //// FILTER VARIABLES @@ -271,25 +279,24 @@ void calibrate_amp() { - 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/(total1/cal_samples); - // pc.printf("gain1 = %f",emg_gain1); - - double total2 = 0; - for(int i = 0; i<cal_samples; i++) - { - double input2 = EMG_int.read(); - total2 = total2 + input2; - wait(0.1); - } - emg_gain2 = 1/(total2/cal_samples); - // pc.printf("gain2 = %f",emg_gain2); + //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); } ///////////////////////////////////////////////////////////////////// @@ -367,7 +374,8 @@ 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 = y5; // update global variable + 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 @@ -377,7 +385,12 @@ 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 = y5t; // update global variable + 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 @@ -389,9 +402,13 @@ // setpoint_analog= 0.5+(sin(time)/2.0); // time += time_inc; - output1 = potright.read(); + //output1 = potright.read(); - output2 = potleft.read(); + //output2 = potleft.read(); + + if(output1>1) {output1 = 1;} + if(output2>1) {output2 = 1;} + double xx = 50+output1*20; double ymin = - 16; @@ -407,11 +424,8 @@ double phi1 = 4*(theta_one) + 2.8; double phi2 = 4*(theta_one+theta_two) + 1.85; phi2 = -phi2; - - scope.set(0,output1); - scope.set(1,output2); - scope.send(); - + + if(phi1 < limlow1){phi1 = limlow1;} if(phi1 > limhigh1){phi1 = limhigh1;} @@ -420,11 +434,11 @@ 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); + pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two); @@ -454,7 +468,7 @@ int main() { pc.baud(115200); - // main_filter.attach(&EMG_activate, controlstep); + main_filter.attach(&EMG_activate, controlstep); cartesian.attach(&angle_activate, controlstep); controller1.attach(&motor1_activate, controlstep); // call a go-flag controller2.attach(&motor2_activate, controlstep);