Jorn-Jan van de Beld
/
kin_PD_project
begin van episch avontuur
Diff: main.cpp
- Revision:
- 2:821ae727bf8c
- Parent:
- 1:b6a079ca16e0
- Child:
- 3:329acce2487c
diff -r b6a079ca16e0 -r 821ae727bf8c main.cpp --- a/main.cpp Fri Oct 27 12:08:44 2017 +0000 +++ b/main.cpp Tue Oct 31 12:16:16 2017 +0000 @@ -5,20 +5,23 @@ // Connecties Serial pc(USBTX, USBRX); //Serial PC connectie +DigitalOut led_g(LED_GREEN); //Groene led op k64f bord DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord) PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord) -DigitalOut motor2DirectionPin(D6); //Motorrichting op D4 (connected op het bord) -PwmOut motor2MagnitudePin(D7); //Motorkracht op D5 (connected op het bord) +DigitalOut motor2DirectionPin(D7); //Motorrichting op D4 (connected op het bord) +PwmOut motor2MagnitudePin(D6); //Motorkracht op D5 (connected op het bord) QEI q1_enc(D13, D12, NC, 32); //encoder motor 1 instellen QEI q2_enc(D11, D10, NC, 32); // encoder motor 2 instellen const double pi = 3.1415926535897; // waarde voor pi aanmaken // globale gegevens Ticker tick_sample; // ticker voor aanroepen aansturing +Ticker tick_wasd; //ticker voor toetsenbord aansturing char key; double ts=0.001; int q1_puls; int q2_puls; +int n = 0; // kinematica gegevens // lengte armen @@ -44,19 +47,24 @@ double ref2; double PD1; double PD2; -double qset1=0; -double qset1m=0; -double qset2=0; -double qset2m=0; +double error1=0; +double error1m=0; +double error2=0; +double error2m=0; double Kp; // proportional coefficient ( double Kd; // differential coefficient -void wasd(double& vx, double& vy) +void wasd() { + static char oldkey = 'p'; + static double oldvx = 0; + static double oldvy = 0; + if(pc.readable()==true) { key = pc.getc(); + if (key=='a') { vx = 0.04; //referentie snelheid m/s @@ -77,53 +85,60 @@ vx = 0; vy = -0.04; } - else + else { - vx=0; - vy=0; + key = oldkey; + vx = oldvx; + vy = oldvy; } - } - else + + } //einde eerste if statemnet + + else if (pc.readable()==false) { vx=0; - vy=0; + vy=0; + key='p'; } +oldkey = key; +oldvx = vx; +oldvy = vy; + } -double kinematics(double& q1, double& q2, double q1_pos, double q2_pos, double vx, double vy ) +void kinematics() { - q1 = ((-(L3 + L2*cos(q1_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vx + (-(L2*sin(q2_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vy) * ts + q1_pos; - q2 = (((L3 + L2*cos(q2_pos) - L1*sin(q1_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos)) * vx) + ((L1*cos(q1_pos) + L2*sin(q2_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vy) * ts + q2_pos; - return 0; + q1 = ((-(L3 + L2*cos(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vx + ((L2*sin(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vy) * ts + q1_pos; + q2 = (((L3 - L1*sin(q1) + L2*cos(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1))*vx) + ((L1*cos(q1) - L2*sin(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vy) * ts + q2_pos; } -void controller(double qset1, double qset2) +void controller(double q1ref, double q2ref) { // error = qset // referentie bepalen - ref1 = qset1 + q1_pos; // genereert het referentiesignaal - ref2 = qset2 + q2_pos; + error1 = q1ref - q1_pos; + error2 = q2ref - q2_pos; //PD - PD1 = Kp*(qset1) + Kd*((qset1m-qset1)/ts) ; // PD sum; PD = proportianal + differential - qset1m = qset1; // waarde voor qset wordt opgeslagen (m = memory) - PD2 = Kp*(qset2) + Kd*((qset2m-qset2)/ts) ; - qset2m = qset2; + PD1 = Kp*(error1) + Kd*((error1m-error1)/ts) ; // PD sum; PD = proportianal + differential + error1m = error1; // waarde voor qset wordt opgeslagen (m = memory) + PD2 = Kp*(error2) + Kd*((error2m-error2)/ts) ; + error2m = error2; //Motor control if (PD1<0 && PD2<0 ) { motor1MagnitudePin = fabs(PD1); - motor1DirectionPin = 1; + motor1DirectionPin = 0; motor2MagnitudePin = fabs(PD2); motor2DirectionPin = 1; } else if (PD1>0 && PD2<0) { motor1MagnitudePin = fabs(PD1); - motor1DirectionPin = 0; + motor1DirectionPin = 1; motor2MagnitudePin = fabs(PD2); motor2DirectionPin = 1; } @@ -131,7 +146,7 @@ else if (PD1<0 && PD2>0) { motor1MagnitudePin = fabs(PD1); - motor1DirectionPin = 1; + motor1DirectionPin = 0; motor2MagnitudePin = fabs(PD2); motor2DirectionPin = 0; } @@ -139,7 +154,7 @@ else if (PD1>0 && PD2>0) { motor1MagnitudePin = fabs(PD1); - motor1DirectionPin = 0; + motor1DirectionPin = 1; motor2MagnitudePin = fabs(PD2); motor2DirectionPin = 0; } @@ -155,16 +170,20 @@ 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 - - // pijltjestoetsen - wasd(vx, vy); - + // kinematica bepaald gewenste q1 en q2 referenties afhankelijk van ingegeven vx en vy - kinematics(q1, q2, q1_pos, q2_pos, vx, vy); - + kinematics(); + // PD controller gebruikt PD control en stuurt motor aan controller(q1, q2); - + + if(n==500){ + printf("motor1 = %f motor2 = %f\n\r", PD1, PD2); + n=0; + } + else{ + n=n+1; + } } @@ -172,12 +191,15 @@ { pc.baud(115200); pulses2rad=(2*pi)/4200; - Kp = 0.5/pi; - Kd = 0.1*Kp; - tick_sample.attach_us(&aansturing, 1000); //sample frequency 1 mHz; - + Kp = 2.5; + Kd = 0.2*Kp; + tick_sample.attach_us(&aansturing, 1000); //sample frequency 1000 Hz; + tick_wasd.attach(&wasd, 0.1); + led_g = 0; + while(true) { + }