Jorn-Jan van de Beld
/
PI_standaardsignalen
kom op
Diff: main.cpp
- Revision:
- 2:0686b2132556
- Parent:
- 1:7969189824ff
- Child:
- 3:3b5b85a32c9a
diff -r 7969189824ff -r 0686b2132556 main.cpp --- a/main.cpp Thu Nov 02 09:20:15 2017 +0000 +++ b/main.cpp Thu Nov 02 09:45:25 2017 +0000 @@ -27,6 +27,7 @@ int q1_puls; int q2_puls; double rad2pulses=4200/(2*pi); +double pulses2rad = (2*pi)/4200; double ts = 0.001; double PI1; int n; @@ -41,16 +42,46 @@ double error_I2_2 = 0; double PI2; +// kinematica gegevens +// lengte armen +double L1 = 0.250; +double L2 = 0.355; +double L3 = 0.150; + +// reference position +double q1=0; // positie q1 in radialen +double q2=0; // positie q2 in radialen +double q1_pos; +double q2_pos; + +// EMG Input_k +double vx; +double vy; + + +void kinematica() +{ + // encoders uitlezen + q1_puls = q1_enc.getPulses(); + q1_pos = q1_puls*pulses2rad; // berekent positie q1 in radialen + q2_puls = q2_enc.getPulses(); + q2_pos = q2_puls*pulses2rad; // berekent positie q2 in radialen + + vx = 4*sin( 2*t.read() ); + vy = 0; + + q1 = ((-(L3 + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vx + ((L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * ts + q1_pos; + q2 = (((L3 - L1*sin(q1_pos) + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos))*vx) + ((L1*cos(q1_pos) - L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * ts + q2_pos; + + + ref1 = q1*rad2pulses; + ref2 = q2*rad2pulses; +} + + void controller() { - refrad2 = 1.57f*sin(t.read()); //motor 2 - ref2 = refrad2 * rad2pulses; - refrad1 = -0.5f*cos(t.read()) + 0.6f; //motor 1 - ref1 = refrad2 * rad2pulses; - - // encoders uitlezen - q1_puls = q1_enc.getPulses(); - q2_puls = q2_enc.getPulses(); + kinematica(); //PID 1 error1_1 = ref1 - q1_puls; @@ -91,7 +122,7 @@ } if(n==500){ - printf("error1 = %f error2 = %f\n\r", error1_1, error1_2); + printf("PI1 = %f PI2 = %f\n\r", ref1, ref2); n=0; } else{