control flow doet nog niks
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 5:67afe491a1e3
- Parent:
- 4:072b99947fc6
- Child:
- 6:9c8e91bb1316
diff -r 072b99947fc6 -r 67afe491a1e3 main.cpp --- a/main.cpp Sat Oct 10 08:26:48 2015 +0000 +++ b/main.cpp Sat Oct 10 20:02:16 2015 +0000 @@ -90,6 +90,35 @@ // Filter coefficients // differential action filter const double m_f_a1 = 1.0, m_f_a2 = 2.0, m_f_b0 = 1.0, m_f_b1 = 3.0, m_f_b2 = 4.0; // coefficients from sheets are used as first test. + +// tweede orde notch filter 50 Hz +// biquad 1 coefficienten + const double numnotch50biq1_1 = 1; + const double numnotch50biq1_2 = -1.61816178466632; + const double numnotch50biq1_3 = 1.00000006127058; + const double dennotch50biq1_2 = -1.59325742941798; + const double dennotch50biq1_3 = 0.982171881701431; +// biquad 2 coefficienten + const double numnotch50biq2_1 = 1; + const double numnotch50biq2_2 = -1.61816171933244; + const double numnotch50biq2_3 = 0.999999938729428; + const double dennotch50biq2_2 = -1.61431180968071; + const double dennotch50biq2_3 = 0.982599066293075; +// highpass filter 20 Hz coefficienten + const double numhigh20_1 = 0.837089190566345; + const double numhigh20_2 = -1.67417838113269; + const double numhigh20_3 = 0.837089190566345; + const double denhigh20_2 = -1.64745998107698; + const double denhigh20_3 = 0.700896781188403; +// lowpass 5 Hz coefficienten + const double numlow5_1 =0.000944691843840162; + const double numlow5_2 =0.00188938368768032; + const double numlow5_3 =0.000944691843840162; + const double denlow5_2 =-1.91119706742607; + const double denlow5_3 =0.914975834801434; + +// Define the storage variables and filter coeicients for two filters +double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0,f4_v1 = 0, f4_v2 = 0; @@ -98,7 +127,14 @@ ////////////////////////////////////////////////////////////// // these functions are tailored to perform 1 specific function - +// this funtion flips leds on and off accordin to input with 0 being on +void LED(int red,int green,int blue) +{ + ledred.write(red); + ledgreen.write(green); + ledblue.write(blue); +} + // counts 2 radians // this function takes counts from the encoder and converts it to the amount of radians from the zero position. // It has been set up for standard 2X DECODING!!!!!! @@ -115,7 +151,7 @@ double reference_f(double input, double &c_reference) { double potset = (input-offset)*gain; - if(POT_in == true && potset < 0.1 && potset > -0.1) // larger area for potmeter to get a zero value + if(POT_in == true && potset < 0.15 && potset > -0.15) // larger area for potmeter to get a zero value { potset = 0; } @@ -161,6 +197,16 @@ return y; } +double EMG_Filter(double u1) +{ + // double u1 = potright.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); + return y5; +} // PID Controller given in sheets // aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!) @@ -204,6 +250,7 @@ // MOTOR 1 void motor1_control() { + double input1 = potright.read()*signalamp1; double reference1 = reference_f(input1, c_reference1); // determine the reference that has been set by the inputsignal double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor @@ -213,11 +260,6 @@ scope.send(); double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err); pc.printf("output 1 %f \n",output1); - - // ws best locatie om output te blokkeren als grenzen bereikt zijn, simpel if-loopje met rad1?? - - - // onderstaand kan vervangen worden door een functie voor beide motoren?? if(output1 > 0) { // uses the calculated output to determine the direction of the motor motor1_rich.write(0); motor1_aan.write(output1); @@ -230,7 +272,7 @@ // MOTOR 2 void motor2_control() { - double input2 = potleft.read()*signalamp2; + double input2 = potleft.read()*signalamp2; // replace potleft with filter double reference2 = reference_f(input2, c_reference2); // determine the reference that has been set by the inputsignal double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor double error2 = (reference2 - rads2); // determine the error (reference - position) @@ -288,9 +330,7 @@ // Main Control stuff and options if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops { - ledred.write(1); - ledgreen.write(1); - ledblue.write(0); + LED(1,1,0); // turn blue led on if(motor1_go) { @@ -305,21 +345,15 @@ } if(loop_start == false && calib_start == true) // start calibration procedures { - ledred.write(1); - ledgreen.write(0); - ledblue.write(1); + LED(1,0,1); // turn green led on } if(loop_start == true && calib_start == true) // check if both buttons are enabled and stop everything { - ledred.write(0); - ledgreen.write(1); - ledblue.write(1); + LED(0,1,1); // turn red led on } else { - ledred.write(1); - ledgreen.write(1); - ledblue.write(1); + LED(1,1,1); // turn leds off (both buttons false) } } } \ No newline at end of file