control flow doet nog niks
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 6:9c8e91bb1316
- Parent:
- 5:67afe491a1e3
- Child:
- 7:4df9f6ebe744
diff -r 67afe491a1e3 -r 9c8e91bb1316 main.cpp --- a/main.cpp Sat Oct 10 20:02:16 2015 +0000 +++ b/main.cpp Sat Oct 10 20:38:38 2015 +0000 @@ -37,12 +37,20 @@ AnalogIn potright(A0); // define the potmeter outputpins AnalogIn potleft(A1); -// BUTTONS +// BUTTONS + // control flow DigitalIn buttonlinks(PTA4); DigitalIn buttonrechts(PTC6); // init values bool loop_start = false; bool calib_start = false; + + // direction control + DigitalIn reverse_button_links(D0); + DigitalIn reverse_button_rechts(D1); + // init values + bool reverse_links = false; + bool reverse_rechts = false; // LED DigitalOut ledred(LED1); @@ -53,11 +61,7 @@ // 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 gain and offset of the input(input between 0 and 1 is optimized). For EMG use 0, 1 and false, for POT use 0.5, 2 and true - double offset = 0.5; - double gain = 2; - bool POT_in = true; // show potmeter is attached, increases the range for which 0 is the output. -// Define storage variables for reference values + // Define storage variables for reference values double c_reference1 = 0; double c_reference2 = 0; // Define the maximum rate of change for the reference (velocity) @@ -88,8 +92,8 @@ double m_f_v2 = 0; // 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. +// 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 @@ -150,12 +154,7 @@ // to create a reference that moves with a variable speed. It is now set up for potmeter values. double reference_f(double input, double &c_reference) { - double potset = (input-offset)*gain; - if(POT_in == true && potset < 0.15 && potset > -0.15) // larger area for potmeter to get a zero value - { - potset = 0; - } - double reference = c_reference + potset * controlstep * Vmax ; + double reference = c_reference + input * controlstep * Vmax ; c_reference = reference; // change the global variable to the latest location. return reference; } @@ -252,12 +251,12 @@ { double input1 = potright.read()*signalamp1; + if(input1 < 0.25) {input1 = 0;} + if(input1 > 1) {input1 = 1;} + if(reverse_rechts == true) {input1 = -input1;} 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 double error1 = (reference1 - rads1); // determine the error (reference - position) - scope.set(0,reference1); - scope.set(1,rads1); - 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); if(output1 > 0) { // uses the calculated output to determine the direction of the motor @@ -273,13 +272,12 @@ void motor2_control() { 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 + if(input2 < 0.25) {input2 = 0;} + if(input2 > 1) {input2 = 1;} + if(reverse_links == true) {input2 = -input2;} + double reference2 = reference_f(input2, c_reference2); // determine the reference that has been set by the clamped inputsignal double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor double error2 = (reference2 - rads2); // determine the error (reference - position) - // scope.set(3,reference2); - // scope.set(4,rads2); - // scope.send(); - double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err); if(output2 > 0) { // uses the calculated output to determine the direction of the motor motor2_rich.write(0); @@ -312,21 +310,33 @@ // controller2.attach(&motor2_activate, controlstep); Disabled while debugging PID-controller ?? while(true) { - // button functions + // button press functions + // flow buttons if(buttonlinks.read() == 0) { - loop_start = !loop_start; // reverse the boolean loop_start value + loop_start = !loop_start; wait(buttonlinks.read() == 1); - wait(0.3); // check if it causes instability!! ?? + wait(0.3); } - if(buttonrechts.read() == 0) { - calib_start = !calib_start; // reverse the boolean calib_start value + calib_start = !calib_start; wait(buttonrechts.read() == 1); - wait(0.3); // check if it causes instability!! ?? + wait(0.3); } - + // reverse buttons + if(reverse_button_links.read() == 0) + { + reverse_links = !reverse_links; + wait(reverse_button_links.read() == 1); + wait(0.3); + } + if(reverse_button_rechts.read() == 0) + { + reverse_rechts = !reverse_rechts; + wait(reverse_button_rechts.read() == 1); + wait(0.3); + } // Main Control stuff and options if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops {