![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
werkt nog niet
Dependencies: HIDScope MODSERIAL biquadFilter mbed QEI
Fork of Project_script_union by
Diff: main.cpp
- Revision:
- 29:df10cb76ef26
- Parent:
- 28:61d1372349c8
- Child:
- 30:8191e8541a0a
diff -r 61d1372349c8 -r df10cb76ef26 main.cpp --- a/main.cpp Wed Oct 31 19:14:56 2018 +0000 +++ b/main.cpp Thu Nov 01 08:21:31 2018 +0000 @@ -3,6 +3,7 @@ #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) @@ -10,8 +11,8 @@ // 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 +AnalogIn potmeter1 (A0); //First raw EMG signal input +AnalogIn potmeter2 (A1); //Second raw EMG signal input InterruptIn encoderA1 (D9); InterruptIn encoderB1 (D8); @@ -33,17 +34,21 @@ 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); //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered //Tickers Ticker func_tick; Ticker engine_control1_tick; -Ticker engine_control2_tick; +Ticker engine_control2_tick; +Ticker encoder1_read_tick; +Ticker encoder2_read_tick; //Global variables const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders??? -const float T2 = 0.01f; +const float T2 = 0.1f; // Inverse Kinematica variables const double L1 = 0.208; // Hoogte van tafel tot joint 1 @@ -54,280 +59,122 @@ 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; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is -double q2ref = 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 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=0.0; // Desired velocity end effector in x direction --> Determined by EMG signals +double v_y=0.0; // 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; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken -double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken +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; // Reference signal for motorangle q1ref -double q2_ii; // Reference signal for motorangle q2ref +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 = 5.0; //Motor 1 eerst 17.5 , nu 5 +double Kp1 = 1.0; //Motor 1 eerst 17.5 , nu 1 double Ki1 = 1.02; double Kd1 = 23.2; -double encoder1 = 0; double encoder_radians1=0; +double err_integral1 = 0; +double err_prev1 = 0; -double Kp2 = 5.0; //Motor 2 eerst 17.5, nu 5 + +double Kp2 = 1.0; //Motor 2 eerst 17.5, nu 1 double Ki2 = 1.02; double Kd2 = 23.2; -double encoder2 = 0; double encoder_radians2=0; +double err_integral2 = 0; +double err_prev2 = 0; int start_control = 0; - -//double potmeter1s = (potmeter1*2)-1.0f; -//double potmeter2s = (potmeter2*2)-1.0f; double emg_cal = 1; //--------------Functions----------------------------------------------------------------------------------------------------------------------------// -//------------------ Encoder motor 1 --------------------------------// - -void encoderA1_rise() -{ - if(encoderB1==false) - { - encoder1++; - } - else - { - encoder1--; - } -} - -void encoderA1_fall() -{ - if(encoderB1==true) - { - encoder1++; - } - else - { - encoder1--; - } -} - -void encoderB1_rise() -{ - if(encoderA1==true) - { - encoder1++; - } - else - { - encoder1--; - } -} - -void encoderB1_fall() -{ - if(encoderA1==false) - { - encoder1++; - } - else - { - encoder1--; - } -} - -void encoder_count1() -{ - encoderA1.rise(&encoderA1_rise); - encoderA1.fall(&encoderA1_fall); - encoderB1.rise(&encoderB1_rise); - encoderB1.fall(&encoderB1_fall); -} - -//------------------ Encoder motor 2 --------------------------------// - -void encoderA2_rise() -{ - if(encoderB2==false) - { - encoder2++; - } - else - { - encoder2--; - } -} - -void encoderA2_fall() -{ - if(encoderB2==true) - { - encoder2++; - } - else - { - encoder2--; - } -} - -void encoderB2_rise() -{ - if(encoderA2==true) - { - encoder2++; - } - else - { - encoder2--; - } -} - -void encoderB2_fall() -{ - if(encoderA2==false) - { - encoder2++; - } - else - { - encoder2--; - } -} - -void encoder_count2() -{ - encoderA2.rise(&encoderA2_rise); - encoderA2.fall(&encoderA2_fall); - encoderB2.rise(&encoderB2_rise); - encoderB2.fall(&encoderB2_fall); -} - //------------------ Filter EMG + Calibration EMG --------------------------------// -//------------------ Inversed Kinematics --------------------------------// - - -//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------// -double PID_controller1(double err1) +//---------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) { pc.printf("ik doe het, PDI 1\n\r"); - static double err_integral1 = 0; - static double err_prev1 = err1; // initialization with this value only done once! + err_integral = 0; + err_prev = err; // initialization with this value only done once! - static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); + static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); // Proportional part: - double u_k1 = Kp1 * err1; + double u_k = Kp * err; /* Integral part - err_integral1 = err_integral1 + err1 * T; - double u_i1 = Ki1 * err_integral1; + err_integral = err_integral + err * T; + double u_i = Ki * err_integral; // Derivative part - double err_derivative1 = (err1 - err_prev1)/T; - double filtered_err_derivative1 = LowPassFilter1.step(err_derivative1); - double u_d1 = Kd1 * filtered_err_derivative1; - err_prev1 = err1; + 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; */ // Sum all parts and return it - return u_k1+0; //+ u_i1 + u_d1; + return u_k; //+ u_i + u_d; } -void start_your_engines1(double u1) -{ - pc.printf("ik doe het, engine start 1\n\r"); - - if(encoder1<5250 && encoder1>-5250) //limits rotation, in counts - { - pwmpin1 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! - directionpin1.write(0); - } - else - { - pwmpin1 = 0; - } -} - void engine_control1() //Engine 1 is rotational engine, connected with left side pins { //while(start_control == 1) //{ pc.printf("ik doe het, engine control 1\n\r"); - encoder_radians1 = encoder1*(2*PI)/8400; + encoder_radians1 = encoder1.getPulses()*(2*PI)/8400.0; double err1 = q1ref - encoder_radians1; - double u1 = PID_controller1(err1); //PID controller function call - start_your_engines1(u1); + double u1 = PID_control(err1, Kp1, Ki1, Kd1, err_integral1, err_prev1); //PID controller function call + + if(encoder1.getPulses()<5250 && encoder1.getPulses()>-5250) //limits rotation, in counts + { + pwmpin1 = 0.5; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! + directionpin1.write(0); + } + else + { + pwmpin1 = 0; + } // break; //} } - - -//---------PID controller motor 2 + start motor 2 -----------------------------------------------------------// -double PID_controller2(double err2) -{ - pc.printf("ik doe het, PDI 2\n\r"); - - static double err_integral2 = 0; - static double err_prev2 = err2; // initialization with this value only done once! - - static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); - - // 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 = LowPassFilter2.step(err_derivative2); - double u_d2 = Kd2 * filtered_err_derivative2; - err_prev2 = err2; - */ - - // Sum all parts and return it - return u_k2+0; //+ u_i2 + u_d2; -} - -void start_your_engines2(double u2) -{ - pc.printf("ik doe het, engine start 2\n\r"); - - if(encoder2<12600 && encoder2>-1) //limits translation in counts - { - pwmpin2 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! - directionpin2.write(0); - } - else - { - pwmpin2 = 0; - } - -} - void engine_control2() //Engine 2 is translational engine, connected with right side wires { pc.printf("ik doe het, engine control 2\n\r"); - encoder_radians2 = encoder2*(2*PI)/8400; + encoder_radians2 = encoder2.getPulses()*(2*PI)/8400.0; double err2 = q2ref - encoder_radians2; - double u2 = PID_controller2(err2); //PID controller function call - start_your_engines2(u2); //Call start_your_engines function + double u2 = PID_control(err2, Kp2, Ki2, Kd2, err_integral2, err_prev2); //PID controller function call + + if(encoder2.getPulses()<12600 && encoder2.getPulses()>-1) //limits translation in counts + { + pwmpin2 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! + directionpin2.write(0); + } + else + { + pwmpin2 = 0; + } + } +//------------------ Inversed Kinematics --------------------------------// + + void inverse_kinematics() { @@ -338,43 +185,55 @@ 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_ii = q1ref + q1_dot*T; - q2_ii = q2ref + q2_dot*T; + q1_ii = q1ref + (q1_dot/r_trans)*T; + q2_ii = q2ref + (q2_dot*5.0)*T; q1ref = q1_ii; q2ref = q2_ii; //start_control = 1; - engine_control1(); + engine_control1(); engine_control2(); } void v_des_calculate_qref() { - if(button1==0) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn + 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.5f; //beweging in +x direction ledr = 0; //red ledb = 1; ledg = 1; } - else if(button2==0) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn + 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.5f; //beweging in +y direction ledr = 1; //green ledb = 1; ledg = 0; } - /* - else if(button1==0 && button2==0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction + + 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.5f; + 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.5f; ledr = 1; //Blue ledb = 0; ledg = 1; - } - */ + } + else //If not higher than the threshold, motors will not turn at all { v_x = 0; @@ -394,7 +253,8 @@ int main() -{ +{ + wait(1.0f); 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 @@ -405,7 +265,9 @@ // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. - while(true) - {;} + { + pc.printf("encoder1 %d\n\r",encoder1.getPulses()); + pc.printf("Encoder2 %d\n\r",encoder2.getPulses()); + } }