![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
begin van episch avontuur
Diff: main.cpp
- Revision:
- 7:9e965efc430e
- Parent:
- 6:c97264a28123
- Child:
- 8:668afaa63c96
diff -r c97264a28123 -r 9e965efc430e main.cpp --- a/main.cpp Tue Oct 31 21:37:31 2017 +0000 +++ b/main.cpp Tue Oct 31 22:49:47 2017 +0000 @@ -19,8 +19,8 @@ Ticker tick_wasd; //ticker voor toetsenbord aansturing char key; double ts=0.001; -int q1_puls; -int q2_puls; +double q1_puls; +double q2_puls; int n = 0; // kinematica gegevens @@ -36,8 +36,8 @@ double q2_pos; // EMG Input_k -double vx = 0; -double vy = 0; +double vx; +double vy; // PID gegevens @@ -63,6 +63,29 @@ double Kp2 = 0.00015; // proportional coefficient motor 2 double Ki2 = 0.0035; // integrating coefficient motor 2 +void toetsen() +{ + if(pc.readable()==true) + { key = pc.getc(); + if (key=='a') + { + vx= vx + 5; //reference wordt 500 pulses + } + else if(key=='d') + { + vx= vx - 5; //reference wordt 0 pulses + } + if (key=='w') + { + vy= vy + 5; //reference wordt 500 pulses + } + else if(key=='s') + { + vy= vy -5; //reference wordt 0 pulses + } + } +} + void wasd() { static char oldkey = 'p'; @@ -74,23 +97,23 @@ if (key=='a') { - vx = 0.04; //referentie snelheid m/s - vy = 0; + vx = vx - 10; //referentie snelheid m/s + vy = vy; } else if(key=='d') { - vx = -0.04; - vy = 0; + vx = vx - 10; + vy = vy; } else if(key=='w') { - vx = 0; - vy = 0.04; + vx = vx; + vy = vy + 25; } else if(key=='s') { - vx = 0; - vy = -0.04; + vx = vx; + vy = vy - 25; } else { @@ -125,6 +148,10 @@ void controller() { +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 //PID 1 error1_1 = ref1 - q1_puls; error_I_1 = error_I2_1 + ts*((error1_1 - error2_1)/2); @@ -189,7 +216,14 @@ kinematics(); // PD controller gebruikt PD control en stuurt motor aan - controller(); + controller(); + if(n==500){ + printf("PI1 = %f PI2 = %f\n\r", ref1, ref2); + n=0; + } + else{ + n=n+1; + } } @@ -198,7 +232,7 @@ pc.baud(115200); pulses2rad=(2*pi)/4200; rad2pulses=4200/(2*pi); - tick_wasd.attach_us(&wasd, 1000); + tick_wasd.attach_us(&toetsen, 1000); tick_sample.attach_us(&aansturing, 1000); //sample frequency 1000 Hz; led_g = 0;