Aansturing
Dependencies: Encoder mbed HIDScope
Revision 3:57b98989b0b1, committed 2016-11-03
- Comitter:
- wikdehaas
- Date:
- Thu Nov 03 11:33:29 2016 +0000
- Parent:
- 2:a42b34f9d6ab
- Commit message:
- TY
Changed in this revision
TY.cpp | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TY.cpp Thu Nov 03 11:33:29 2016 +0000 @@ -0,0 +1,135 @@ +/* +if(knop_1 == 0 && knop_2 == 0 && x<x_plek_T) + +//gegevens +const double x_plek_T = 0.35, y_streep_TY_hoogte = 0.15; +const double x_streep_half = 0.05, x_plek_Y = 0.45, y_midden_Y = 0.09; + +case 5: //Naar onderkant T + while(x < x_plek_T) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + x = x + tijd_verschil*Vx; + y = y; + tijd_oud = tijd_nieuw; + } + //Naar bovenkant T + while(y < (y_base+y_streep_TY_hoogte)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y + tijd_verschil*Vy; + x = x; + tijd_oud = tijd_nieuw; + } + //Naar linkerkant bovenkant T + while(x > (x_plek_T - x_streep_half)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + x = x - tijd_verschil*Vx; + y = y; + tijd_oud = tijd_nieuw; + } + //Naar rechterkant bovenkant T + while(x < (x_plek_T + x_streep_half)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + x = x + tijd_verschil*Vx; + y = y; + tijd_oud = tijd_nieuw; + } + //Naar midden T + while(x > x_plek_T) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + x = x - tijd_verschil*Vx; + y = y; + tijd_oud = tijd_nieuw; + } + //Naar onderkant T + while(y > y_base) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y - tijd_verschil*Vy; + x = x; + tijd_oud = tijd_nieuw; + } + //Naar onderkant Y + while(x < x_plek_Y) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + x = x + tijd_verschil*Vx; + y = y; + tijd_oud = tijd_nieuw; + } + //Naar midden Y + while(y < (y_base+y_midden_Y)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y + tijd_verschil*Vy; + x = x; + tijd_oud = tijd_nieuw; + } + //Naar linksboven Y + while(y < (y_base+y_streep_TY_hoogte)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y + tijd_verschil*Vy; + x = x - tijd_verschil*Vx; + tijd_oud = tijd_nieuw; + } + //Naar midden Y + while(y > (y_base+y_midden_Y)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y - tijd_verschil*Vy; + x = x + tijd_verschil*Vx; + tijd_oud = tijd_nieuw; + } + //Naar rechtsboven Y + while(y < (y_base+y_streep_TY_hoogte)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y + tijd_verschil*Vy; + x = x + tijd_verschil*Vx; + tijd_oud = tijd_nieuw; + } + //Naar midden Y + while(y > (y_base+y_midden_Y)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y - tijd_verschil*Vy; + x = x - tijd_verschil*Vx; + tijd_oud = tijd_nieuw; + } + //Naar onderkant Y + while(y > y_base) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y - tijd_verschil*Vy; + x = x; + tijd_oud = tijd_nieuw; + } + //Naar basis + while(x > x_start) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + x = x - tijd_verschil*Vx; + y = y; + tijd_oud = tijd_nieuw; + } + break; + */ \ No newline at end of file
--- a/main.cpp Wed Nov 02 10:45:10 2016 +0000 +++ b/main.cpp Thu Nov 03 11:33:29 2016 +0000 @@ -40,9 +40,12 @@ const double dt = 0.001; const double dt_f = 0.001; const double Vx = 0.05; //m/s -const double Vy = 0.03; //m/s +const double Vy = 0.05; //m/s const double L2 = 0.35; //m const double y_base = 0.045; +//gegevens +const double x_plek_T = 0.43, y_streep_TY_hoogte = 0.15; +const double x_streep_half = 0.06, x_plek_Y = x_plek_T+0.13, y_midden_Y = 0.09; //filter volatile double tijd_oud, tijd_nieuw, tijd_verschil; volatile double u_1, y_1, u_2, y_2; //ongefilerd signaal emg 1, gefilterd signaal emg 1, ongefilterd signaal emg 2, gefilterd signaal emg 2 @@ -55,7 +58,7 @@ volatile bool opgepakt = false; volatile bool zakpunt = false; volatile bool uitdrukken = false; -volatile double y_oppakken = 0.10, y_stijgen = 0.15, x_zakken = 0.09;//Voorwaarde voor terugbewegen +volatile double y_oppakken = 0.135, y_stijgen = 0.15, x_zakken = 0.09;//Voorwaarde voor terugbewegen volatile bool run_programma = true; //PID const double m1_Kp = 2, m1_Ki = 5, m1_Kd = 0.1, m1_N = 50; @@ -64,6 +67,7 @@ const double m2_Kp = 2, m2_Ki = 5, m2_Kd = 0.1, m2_N = 50; double m2_v1 = 0, m2_v2 = 0; // Memory variables const double m2_Ts = 0.001; // Controller sample time +volatile double compensatie = 0; //Controller PID motors @@ -83,14 +87,14 @@ reference_2 = -1.5*theta_2; //reference plaats_2 = 0.0014959*encoder_2.getPosition(); //positie encodercounts naar hoek error_2 = reference_2 - plaats_2 ; - pwm_2 = PID(error_2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_N, m2_v1, m2_v2 ); + pwm_2 = PID(error_2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_N, m2_v1, m2_v2 )-compensatie; if (pwm_2<0){motor_2 = 0;} else {motor_2 = 1;} pwm_motor_2 = fabs(pwm_2); - scope.set(0,error_1); - scope.set(1,reference_1); - scope.set(2,pwm_1); - scope.set(3,plaats_1); + scope.set(0,u_1); + scope.set(1,y_1); + scope.set(2,u_2); + scope.set(3,y_2); scope.send(); } //Ticker filterwaardes @@ -151,12 +155,12 @@ tijd_nieuw = tijd; tijd_verschil = tijd_nieuw - tijd_oud; tijd_oud = tijd_nieuw; - + if (knop_1 == 0 && knop_2 == 1){mode = 1;} else if (knop_1 == 1 && knop_2 == 0){mode = 2;} else if (knop_1 == 0 && knop_2 == 0 && x>(x_start+x_zakken)){mode = 3;} + else if (knop_1 == 0 && knop_2 == 0 && x<x_plek_T) {mode = 5;} else {mode = 4;} //default - /* if (y_1 >= 0.4 && y_2 < 0.4) {mode = 1;} else if (y_1 < 0.4 && y_2 >= 0.4) {mode = 2;} @@ -177,11 +181,11 @@ { tijd_nieuw = tijd; tijd_verschil = tijd_nieuw - tijd_oud; - y = y - tijd_verschil*Vy*3; + y = y - tijd_verschil*Vy*5; x = x; tijd_oud = tijd_nieuw; } - wait(1); + wait(0.05); tijd_nieuw = tijd; tijd_oud = tijd_nieuw; while(y < (y_base+y_stijgen)) @@ -207,18 +211,150 @@ x = x; tijd_oud = tijd_nieuw; } - while(x>x_start-0.02) + while(x>x_start-0.03) { tijd_nieuw = tijd; tijd_verschil = tijd_nieuw - tijd_oud; y = y_base; x = x - tijd_verschil*Vx*2; + if (x<x_start) + { + compensatie = 0.4; + } tijd_oud = tijd_nieuw; } + compensatie = 0; wait(0.5); x = x_start; y = y_base; break; + case 5: //Naar onderkant T + while(x < x_plek_T) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + x = x + tijd_verschil*Vx; + y = y; + tijd_oud = tijd_nieuw; + } + //Naar bovenkant T + while(y < (y_base+y_streep_TY_hoogte)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y + tijd_verschil*Vy; + x = x; + tijd_oud = tijd_nieuw; + } + //Naar linkerkant bovenkant T + while(x > (x_plek_T - x_streep_half)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + x = x - tijd_verschil*Vx; + y = y; + tijd_oud = tijd_nieuw; + } + //Naar rechterkant bovenkant T + while(x < (x_plek_T + x_streep_half)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + x = x + tijd_verschil*Vx; + y = y; + tijd_oud = tijd_nieuw; + } + //Naar midden T + while(x > x_plek_T) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + x = x - tijd_verschil*Vx; + y = y; + tijd_oud = tijd_nieuw; + } + //Naar onderkant T + while(y > y_base) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y - tijd_verschil*Vy; + x = x; + tijd_oud = tijd_nieuw; + } + //Naar onderkant Y + while(x < x_plek_Y) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + x = x + tijd_verschil*Vx; + y = y; + tijd_oud = tijd_nieuw; + } + //Naar midden Y + while(y < (y_base+y_midden_Y)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y + tijd_verschil*Vy; + x = x; + tijd_oud = tijd_nieuw; + } + //Naar linksboven Y + while(y < (y_base+y_streep_TY_hoogte)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y + tijd_verschil*Vy; + x = x - tijd_verschil*Vx; + tijd_oud = tijd_nieuw; + } + //Naar midden Y + while(y > (y_base+y_midden_Y)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y - tijd_verschil*Vy; + x = x + tijd_verschil*Vx; + tijd_oud = tijd_nieuw; + } + //Naar rechtsboven Y + while(y < (y_base+y_streep_TY_hoogte)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y + tijd_verschil*Vy; + x = x + tijd_verschil*Vx; + tijd_oud = tijd_nieuw; + } + //Naar midden Y + while(y > (y_base+y_midden_Y)) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y - tijd_verschil*Vy; + x = x - tijd_verschil*Vx; + tijd_oud = tijd_nieuw; + } + //Naar onderkant Y + while(y > y_base) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + y = y - tijd_verschil*Vy; + x = x; + tijd_oud = tijd_nieuw; + } + //Naar basis + while(x > x_start) + { + tijd_nieuw = tijd; + tijd_verschil = tijd_nieuw - tijd_oud; + x = x - tijd_verschil*Vx; + y = y; + tijd_oud = tijd_nieuw; + } + break; default: x = x; y = y; break;