werkt nog niet
Dependencies: HIDScope MODSERIAL biquadFilter mbed QEI
Fork of Project_script_union by
Diff: main.cpp
- Revision:
- 31:481ad25a40c3
- Parent:
- 30:8191e8541a0a
- Child:
- 32:137d0f27e5a8
diff -r 8191e8541a0a -r 481ad25a40c3 main.cpp --- a/main.cpp Thu Nov 01 12:46:20 2018 +0000 +++ b/main.cpp Thu Nov 01 14:14:03 2018 +0000 @@ -14,11 +14,6 @@ AnalogIn potmeter1 (A0); //First raw EMG signal input AnalogIn potmeter2 (A1); //Second raw EMG signal input -InterruptIn encoderA1 (D9); -InterruptIn encoderB1 (D8); -InterruptIn encoderA2 (D12); -InterruptIn encoderB2 (D13); - InterruptIn button1 (D10); InterruptIn button2 (D11); @@ -34,13 +29,14 @@ MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off -QEI encoder1 (D9, D8, NC, 8400 , QEI::X4_ENCODING); -QEI encoder2 (D12, D13, NC, 8400 , QEI::X4_ENCODING); +QEI encoder2 (D9, D8, NC, 8400,QEI::X4_ENCODING); +QEI encoder1 (D12, D13, NC, 8400,QEI::X4_ENCODING); //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered //Tickers -Ticker func_tick; +Ticker func_tick; +Ticker printTicker; //Global variables @@ -74,18 +70,23 @@ double PI = 3.14159; double Kp1 = 17.5; //Motor 1 eerst 17.5 , nu 1 double Ki1 = 1.02; -double Kd1 = 23.2; +double Kd1 = 15.5; double encoder_radians1=0; double err_integral1 = 0; -double err_prev1 = 0; +double err_prev1, err_prev2; +double err1, err2; + +BiQuad LowPassFilterDer1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); +BiQuad LowPassFilterDer2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); double Kp2 = 17.5; //Motor 2 eerst 17.5, nu 1 double Ki2 = 1.02; -double Kd2 = 23.2; +double Kd2 = 15.5; double encoder_radians2=0; double err_integral2 = 0; -double err_prev2 = 0; +double u1, u2; + int start_control = 0; double emg_cal = 1; @@ -98,100 +99,117 @@ //---------PID controller motor 1 + motor control 1 & 2-----------------------------------------------------------// -double PID_control(double err, const double Kp, const double Ki, const double Kd, double &err_integral, double &err_prev) +void PID_control1() { - pc.printf("ik doe het, PDI \n\r"); - - err_integral = 0; - err_prev = err; // initialization with this value only done once! - - static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); + //pc.printf("ik doe het, PDI \n\r"); // Proportional part: - double u_k = Kp * err; + double u_k1 = Kp1 * err1; //Integral part - err_integral = err_integral + err * T; - double u_i = Ki * err_integral; + err_integral1 = err_integral1 + err1 * T; + double u_i1 = Ki1 * err_integral1; // Derivative part - double err_derivative = (err - err_prev)/T; - double filtered_err_derivative = LowPassFilter.step(err_derivative); - double u_d = Kd * filtered_err_derivative; - err_prev = err; + double err_derivative1 = (err1 - err_prev1)/T; + double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1); + double u_d1 = Kd1 * filtered_err_derivative1; + err_prev1 = err1; // Sum all parts and return it - return u_k + u_i + u_d; - } + u1 = u_k1 + u_i1 + u_d1; +} + +void PID_control2() +{ + //pc.printf("ik doe het, PDI \n\r"); + + // Proportional part: + double u_k2 = Kp2 * err2; + + //Integral part + err_integral2 = err_integral2 + err2 * T; + double u_i2 = Ki2 * err_integral2; + + // Derivative part + double err_derivative2 = (err2 - err_prev2)/T; + double filtered_err_derivative2 = LowPassFilterDer2.step(err_derivative2); + double u_d2 = Kd2 * filtered_err_derivative2; + err_prev2 = err2; + + + // Sum all parts and return it + u2 = u_k2 + u_i2 + u_d2; +} void engine_control1() //Engine 1 is translational engine, connected with left side pins { - //pc.printf("ik doe het, engine control 1\n\r"); - encoder_radians1 = encoder1.getPulses()*(2*PI)/8400.0; - pc.printf("encoder1 %d \n\r",encoder1.getPulses()); - pc.printf("encoder_radians1 %f \n\r",encoder_radians1); - double err1 = q1ref - encoder_radians1; - pc.printf("err1 = %f\n\r",err1); - double u1 = PID_control(err1, Kp1, Ki1, Kd1, err_integral1, err_prev1); //PID controller function call + //pc.printf("ik doe het, engine control 1\n\r"); + encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0; + //pc.printf("encoder1 %f \n\r", (float)encoder1.getPulses()); + //pc.printf("encoder_radians1 %f \n\r",(float) encoder_radians1); + err1 = q1ref - encoder_radians1; + //pc.printf("err1 = %f\n\r",err1); + PID_control1(); //PID controller function call - pc.printf("u1 = %f\n\r",u1); + //pc.printf("u1 = %f\n\r",u1); - if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600 - { + //if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600 + //{ pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! directionpin1.write(u1<0); - } - else - { - pwmpin1 = 0; - } + //} + //else + // { + // pwmpin1 = 0; + // } } void engine_control2() //Engine 2 is rotational engine, connected with right side wires { - encoder_radians2 = encoder2.getPulses()*(2*PI)/8400.0; - pc.printf("encoder2 %f \n\r",encoder2.getPulses()); - pc.printf("encoder_radians2 %d \n\r",encoder_radians2); - double err2 = q2ref - encoder_radians2; - pc.printf("err2 = %f\n\r",err2); - double u2 = PID_control(err2, Kp2, Ki2, Kd2, err_integral2, err_prev2); //PID controller function call - pc.printf("u2 = %f\n\r",u2); + encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0; + //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses()); + //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2); + err2 = q2ref - encoder_radians2; + //pc.printf("err2 = %f\n\r",err2); + PID_control2(); //PID controller function call + //pc.printf("u2 = %f\n\r",u2); - if(encoder2.getPulses()<5250 && encoder2.getPulses()>-5250) //limits rotation, in counts - { + //if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts + // { pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! directionpin2.write(u2>0); - } - else - { - pwmpin2 = 0; - } + // } + //else + // { + // pwmpin2 = 0; + // } } //------------------ Inversed Kinematics --------------------------------// -void inverse_kinematics(double v_x, double v_y) +void inverse_kinematics() { - pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y); + //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y); Lq1 = q1ref*r_trans; Cq2 = q2ref/5.0; - q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); - q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); + q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); //RKI systeem + q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); // - q1_ii = q1ref + (q1_dot/r_trans)*T; + q1_ii = q1ref + (q1_dot/r_trans)*T; //Omgezet naar motorhoeken q2_ii = q2ref + (q2_dot*5.0)*T; q1ref = q1_ii; q2ref = q2_ii; - pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref); + //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref); //start_control = 1; @@ -252,13 +270,19 @@ //pwmpin2 = 0; } - inverse_kinematics(v_x, v_y); //Call inverse kinematics function + inverse_kinematics(); //Call inverse kinematics function } +void printFunctie() +{ + pc.printf("%f, %f\n\r", u1, u2); +} + //------------------ Start main function --------------------------// + int main() { pc.baud(115200); @@ -266,6 +290,7 @@ pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz func_tick.attach(&v_des_calculate_qref,T2); //v_des determined every T + printTicker.attach(&printFunctie, 0.01); while(true) @@ -275,8 +300,9 @@ // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. - pc.printf("Encoder engine 1 %d\n\r",encoder2.getPulses()); - pc.printf("Encoder engine 2 %d\n\r",encoder1.getPulses()); - wait(0.1f); + //pc.printf("Encoder engine 1 %d\n\r",encoder2.getPulses()); + //pc.printf("Encoder engine 2 %d\n\r",encoder1.getPulses()); + //wait(0.1f); + ; } }