![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
kom op
Diff: main.cpp
- Revision:
- 3:3b5b85a32c9a
- Parent:
- 2:0686b2132556
- Child:
- 4:e98ad06f227c
diff -r 0686b2132556 -r 3b5b85a32c9a main.cpp --- a/main.cpp Thu Nov 02 09:45:25 2017 +0000 +++ b/main.cpp Thu Nov 02 13:01:45 2017 +0000 @@ -15,6 +15,7 @@ Timer t; Ticker aansturing; Ticker stepres; +Ticker kinmat; double ref1 = 0; double refrad1; @@ -26,7 +27,7 @@ double Ki2 = 0; int q1_puls; int q2_puls; -double rad2pulses=4200/(2*pi); +double rad2pulses=(2100/pi); double pulses2rad = (2*pi)/4200; double ts = 0.001; double PI1; @@ -62,26 +63,41 @@ void kinematica() { // encoders uitlezen - q1_puls = q1_enc.getPulses(); - q1_pos = q1_puls*pulses2rad; // berekent positie q1 in radialen + 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 + q2_pos = q2_puls*pulses2rad; // berekent positie q2 in radialen - vx = 4*sin( 2*t.read() ); + vx = 0.04f*sin( t.read()/20 ); 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; + 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) * 5 + 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) * 5 + q2_pos; - ref1 = q1*rad2pulses; ref2 = q2*rad2pulses; + + if (ref1 >=0) + { + ref1 = ceil(ref1); + } + else if (ref1<0) + { + ref1 = floor(ref1); + } + if (ref1 >=0) + { + ref2 = ceil(ref2); + } + else if (ref1<0) + { + ref2 = floor(ref2); + } } void controller() -{ - kinematica(); +{ //PID 1 error1_1 = ref1 - q1_puls; @@ -113,16 +129,16 @@ if (PI2>=0) { motor2DirectionPin = 1; - //motor2MagnitudePin = fabs(PI2); + motor2MagnitudePin = fabs(PI2); } else if(PI2<0) { motor2DirectionPin = 0; - //motor2MagnitudePin = fabs(PI2); + motor2MagnitudePin = fabs(PI2); } if(n==500){ - printf("PI1 = %f PI2 = %f\n\r", ref1, ref2); + printf("PI1 = %f PI2 = %f ref1 = %f ref2 = %f\n\r", PI1, PI2, ref1, ref2); n=0; } else{ @@ -147,6 +163,7 @@ pc.baud(115200); t.start(); aansturing.attach_us(&controller, 1000); + kinmat.attach(&kinematica, 0.1); //stepres.attach(&stapfunc, 3); while(true){