Aansturing

Dependencies:   Encoder mbed HIDScope

Files at this revision

API Documentation at this revision

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;