![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
final version
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of Project_script_union_final by
Diff: main.cpp
- Revision:
- 33:976be2825a23
- Parent:
- 32:56a8bd82e971
- Child:
- 34:b8b18ba0c336
diff -r 56a8bd82e971 -r 976be2825a23 main.cpp --- a/main.cpp Thu Nov 01 18:58:28 2018 +0000 +++ b/main.cpp Fri Nov 02 11:57:51 2018 +0000 @@ -185,7 +185,7 @@ wait(0.001f); //Does there need to be a wait? } Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples) - Threshold0 = Mean0*0.8; //Threshold calculation calve = 0.8*mean + Threshold0 = Mean0*0.5; //Threshold calculation calve = 0.8*mean break; //Stop. Threshold is calculated, we will use this further in the code } case 1: //If calibration state 1: @@ -348,14 +348,13 @@ // 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; + err1 = q1_motor - encoder_radians1; //pc.printf("err1 = %f\n\r",err1); PID_control1(); //PID controller function call @@ -377,7 +376,7 @@ 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; + err2 = q2_motor - encoder_radians2; //pc.printf("err2 = %f\n\r",err2); PID_control2(); //PID controller function call //pc.printf("u2 = %f\n\r",u2); @@ -393,17 +392,55 @@ // } } + +/*void engine_control1() //Engine 1 is translational engine, connected with left side pins +{ + encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0; + err1 = q1_motor - encoder_radians1; + PID_control1(); //PID controller function call + + if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600 + { + pwmpin1 = fabs(u1); + directionpin1.write(u1<0); + } + else + { + pwmpin1 = fabs(u1); + directionpin1.write(u1>0); + } +} + +void engine_control2() //Engine 2 is rotational engine, connected with right side wires +{ + encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0; + err2 = q2_motor - encoder_radians2; + PID_control2(); //PID controller function call + + 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 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! + directionpin2.write(u2<0); + } +} +*/ + //------------------ Inversed Kinematics --------------------------------// 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*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref); //RKI systeem - q2_dot = v_y/(L3*cos(q2ref)); // Misschien gain toevoegen om te kijken of het dan werkt. Translatie gaat veel trager en moeizamer dan rotatie + q2_dot = v_y/(L3*cos(q2ref)); // q1_ii = q1ref + q1_dot*T; //Omgezet naar motorhoeken q2_ii = q2ref + q2_dot*T; @@ -411,16 +448,12 @@ q1ref = q1_ii; q2ref = q2_ii; - q1_motor = q1ref*5.0; - q2_motor = q2ref/r_trans; - + q1_motor = -q1ref/r_trans; + q2_motor = q2ref*5.0; - //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() @@ -429,7 +462,7 @@ { if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0) { - v_x = 0.5; //movement in +x direction + v_x = 0.05; //movement in +x direction v_y = 0.0; ledr = 0; //red @@ -438,7 +471,7 @@ } else if(movAg2>Threshold2 && movAg0<Threshold0) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0) { - v_y = 0.5; //Movement in +y direction + v_y = 0.05; //Movement in +y direction v_x = 0.0; ledr = 1; //Green @@ -449,7 +482,7 @@ else if(movAg0>Threshold0 && movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0) { v_y = 0.0; //Movement in -x direction - v_x = -0.5; + v_x = -0.05; ledr = 0; //Purple ledb = 0; @@ -458,7 +491,7 @@ else if(movAg0>Threshold0 && movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0) { - v_y = -0.5; //Movement in -y direction + v_y = -0.05; //Movement in -y direction v_x = 0.0; ledr = 1; //Blue