![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
werkt nog niet
Dependencies: HIDScope MODSERIAL biquadFilter mbed QEI
Fork of Project_script_union by
main.cpp
- Committer:
- MarijkeZondag
- Date:
- 2018-11-01
- Revision:
- 31:481ad25a40c3
- Parent:
- 30:8191e8541a0a
- Child:
- 32:137d0f27e5a8
File content as of revision 31:481ad25a40c3:
#include "mbed.h" #include "MODSERIAL.h" #include "BiQuad.h" #include "HIDScope.h" #include <math.h> #include "QEI.h" //ATTENTION: set mBed to version 151 // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder) // set MODSERIAL to version 44 // set HIDScope to version 7 // set biquadFilter to version 7 AnalogIn potmeter1 (A0); //First raw EMG signal input AnalogIn potmeter2 (A1); //Second raw EMG signal input InterruptIn button1 (D10); InterruptIn button2 (D11); DigitalOut directionpin1 (D4); DigitalOut directionpin2 (D7); PwmOut pwmpin1 (D5); PwmOut pwmpin2 (D6); DigitalOut ledr (LED_RED); DigitalOut ledb (LED_BLUE); DigitalOut ledg (LED_GREEN); MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off 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 printTicker; //Global variables const float T = 0.02f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders??? const float T2 = 0.02f; // Inverse Kinematica variables const double L1 = 0.208; // Hoogte van tafel tot joint 1 //const double L2 = 0.288; // Hoogte van tafel tot joint 2 const double L3 = 0.212; // Lengte van de arm const double L4 = 0.089; // Afstand van achterkant base tot joint 1 //const double L5 = 0.030; // Afstand van joint 1 naar joint 2 const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation // Variërende variabelen inverse kinematics: double q1ref = 0.0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is double q2ref = 0.0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals double Lq1; // Translatieafstand als gevolg van motor rotation joint 1 double Cq2; // Joint angle of the system (corrected for gear ratio 1:5) double q1_dot=0.0; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken double q2_dot=0.0; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken double q1_ii=0.0; // Reference signal for motorangle q1ref double q2_ii=0.0; // Reference signal for motorangle q2ref //Variables PID controller double PI = 3.14159; double Kp1 = 17.5; //Motor 1 eerst 17.5 , nu 1 double Ki1 = 1.02; double Kd1 = 15.5; double encoder_radians1=0; double err_integral1 = 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 = 15.5; double encoder_radians2=0; double err_integral2 = 0; double u1, u2; int start_control = 0; double emg_cal = 1; //--------------Functions----------------------------------------------------------------------------------------------------------------------------// //------------------ Filter EMG + Calibration EMG --------------------------------// //---------PID controller motor 1 + motor control 1 & 2-----------------------------------------------------------// void PID_control1() { //pc.printf("ik doe het, PDI \n\r"); // Proportional part: double u_k1 = Kp1 * err1; //Integral part err_integral1 = err_integral1 + err1 * T; double u_i1 = Ki1 * err_integral1; // Derivative part 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 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 = (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); //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; // } } void engine_control2() //Engine 2 is rotational engine, connected with right side wires { 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 // { pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! directionpin2.write(u2>0); // } //else // { // pwmpin2 = 0; // } } //------------------ Inversed Kinematics --------------------------------// void inverse_kinematics() { //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)); //RKI systeem q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); // 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); //start_control = 1; engine_control1(); engine_control2(); } void v_des_calculate_qref() { double m1 = (potmeter1*2.0)-1.0; double m2 = (potmeter2*2.0)-1.0; if(m1>0.5) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn { v_x = 0.5; //beweging in +x direction v_y = 0.0; ledr = 0; //red ledb = 1; ledg = 1; } else if(m2>0.5) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn { v_y = 0.5; //beweging in +y direction v_x = 0.0; ledr = 1; //green ledb = 1; ledg = 0; } else if(m1 < -0.5) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction { v_x = -0.5; v_y = 0; ledr = 1; //Blue ledb = 0; ledg = 1; } else if(m2 < -0.5) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction { v_x = 0; v_y = -0.5; ledr = 1; //Blue ledb = 0; ledg = 1; } else //If not higher than the threshold, motors will not turn at all { v_x = 0; v_y = 0; ledr = 0; //white ledb = 0; ledg = 0; //pwmpin1 = 0; //pwmpin2 = 0; } inverse_kinematics(); //Call inverse kinematics function } void printFunctie() { pc.printf("%f, %f\n\r", u1, u2); } //------------------ Start main function --------------------------// int main() { pc.baud(115200); pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off. 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) { //engine_control1_tick.attach(&engine_control1,T2); //engine_control2_tick.attach(&engine_control2,T2); // 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); ; } }