Wik de Haas
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 |
diff -r a42b34f9d6ab -r 57b98989b0b1 TY.cpp
--- /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
diff -r a42b34f9d6ab -r 57b98989b0b1 main.cpp
--- 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;