Revision 2:a42b34f9d6ab, committed 2016-11-02

Comitter:

wikdehaas

Date:

Wed Nov 02 10:45:10 2016 +0000

Parent:

1:a644028231b5

Child:

3:57b98989b0b1

Commit message:

aansturing werkend (nu voor knoppen ingesteld);

Changed in this revision

--- a/Filter.cpp	Thu Oct 27 13:51:14 2016 +0000
+++ b/Filter.cpp	Wed Nov 02 10:45:10 2016 +0000
@@ -1,14 +1,14 @@
 #include "Filter.h"
 
-double Filter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2, const double gain)
+double Filter(double u_E, double &v1_E, double &v2_E, const double a1_E, const double a2_E, const double b0_E, const double b1_E, const double b2_E, const double gain_E)
 {
     
-    double v = u - a1*v1 - a2*v2;
-    double y = gain * (b0*v + b1*v1 + b2*v2);
+    double v_E = u_E - a1_E*v1_E - a2_E*v2_E;
+    double y_E = gain_E * (b0_E*v_E + b1_E*v1_E + b2_E*v2_E);
     
-    v2 = v1;
-    v1 = v;
+    v2_E = v1_E;
+    v1_E = v_E;
     
-    return y;
+    return y_E;
     
 }
\ No newline at end of file

--- a/Filter.h	Thu Oct 27 13:51:14 2016 +0000
+++ b/Filter.h	Wed Nov 02 10:45:10 2016 +0000
@@ -1,3 +1,3 @@
 #include "mbed.h"
 
-double Filter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2, const double gain);
\ No newline at end of file
+double Filter(double u_E, double &v1_E, double &v2_E, const double a1_E, const double a2_E, const double b0_E, const double b1_E, const double b2_E, const double gain_E);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter2.cpp	Wed Nov 02 10:45:10 2016 +0000
@@ -0,0 +1,14 @@
+#include "Filter2.h"
+
+double Filter2(double u_E, double &w1_E, double &w2_E, const double a1_E, const double a2_E, const double b0_E, const double b1_E, const double b2_E, const double gain_E)
+{
+    
+    double w_E = u_E - a1_E*w1_E - a2_E*w2_E;
+    double y_E = gain_E * (b0_E*w_E + b1_E*w1_E + b2_E*w2_E);
+    
+    w2_E = w1_E;
+    w1_E = w_E;
+    
+    return y_E;
+    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter2.h	Wed Nov 02 10:45:10 2016 +0000
@@ -0,0 +1,3 @@
+#include "mbed.h"
+
+double Filter2(double u_E, double &w1_E, double &w2_E, const double a1_E, const double a2_E, const double b0_E, const double b1_E, const double b2_E, const double gain_E);
\ No newline at end of file

--- a/FilterDesign.cpp	Thu Oct 27 13:51:14 2016 +0000
+++ b/FilterDesign.cpp	Wed Nov 02 10:45:10 2016 +0000
@@ -1,30 +1,66 @@
 #include "FilterDesign.h"
 #include "Filter.h"
 
+    // Notch 25 Hz 
+    double v1_25 = 0, v2_25 = 0;
+    const double a1_25 = -1.52753439307, a2_25 = 0.60559524000;
+    const double b0_25 = 1.00000000000, b1_25 = -1.90211303259, b2_25 = 1.00000000000;
+    const double gain_25 = 0.80000000000;
+    
+    // Notch 50 Hz 
     double v1_50 = 0, v2_50 = 0;
-    const double a1_50 = -1.52337295428, a2_50 = 0.93195385841;
-    const double b0_50 = 1.00000000000, b1_50 = -1.61854514929, b2_50 = 1.00000000000;
-    const double gain_50 = 1.00000000000;
+    const double a1_50 = -1.53713228931, a2_50 = 0.90250000000;
+    const double b0_50 = 1.00000000000, b1_50 = -1.61803398875, b2_50 = 1.00000000000;
+    const double gain_50 = 0.95500000000;
     
-    double v1_LP = 0, v2_LP = 0;
-    const double a1_LP = -1.93503824887, a2_LP = 0.93708289663;
-    const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
-    const double gain_LP = 0.000511;
+    // Notch 100 Hz
+    double v1_100 = 0, v2_100 = 0;
+    const double a1_100 = -0.58713228931, a2_100 = 0.90250000000;
+    const double b0_100 = 1.00000000000, b1_100 = -0.61803398875, b2_100 = 1.00000000000;
+    const double gain_100 = 0.95200000000;
+    
+    // Notch 200 Hz
+    double v1_200 = 0, v2_200 = 0;
+    const double a1_200 = 1.53713228931, a2_200 = 0.90250000000;
+    const double b0_200 = 1.00000000000, b1_200 = 1.61803398875, b2_200 = 1.00000000000;
+    const double gain_200 = 0.95500000000;
     
+    // High pass 10 Hz (oud)
     double v1_HP = 0, v2_HP = 0;
-    const double a1_HP = -0.76475499450, a2_HP = 0.27692273367;
-    const double b0_HP = 1.00000000000, b1_HP = -2.0, b2_HP = 1.00000000000;
-    const double gain_HP = 0.510419;
+    const double a1_HP = -1.91258127190, a2_HP = 0.91680625000;
+    const double b0_HP = 1.00000000000, b1_HP = -2.00000000000, b2_HP = 1.00000000000;
+    const double gain_HP = 0.95500000000;
+    /*
+    // High pass 50 Hz 
+    double v1_HP = 0, v2_HP = 0;
+    const double a1_HP = -1.47413760026, a2_HP = 0.60062500000;
+    const double b0_HP = 1.00000000000, b1_HP = -2.00000000000, b2_HP = 1.00000000000;
+    const double gain_HP = 0.77000000000;
+    
+    // Low pass 0.7 Hz
+    double v1_LP = 0, v2_LP = 0;
+    const double a1_LP =  -1.80000000000, a2_LP = 0.81000000000;
+    const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
+    const double gain_LP = 0.0025100000000;
+    */
+    // Low pass 2 Hz
+    double v1_LP = 0, v2_LP = 0;
+    const double a1_LP =  -1.99277224437, a2_LP = 0.99281296000;
+    const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
+    const double gain_LP = 0.0001000000000;
 
-double FilterDesign(double u)
+double FilterDesign(double u_E)
 {
-    
-    double y_50 = Filter(u,v1_50,v2_50,a1_50,a2_50,b0_50,b1_50,b2_50,gain_50);
+    double y_25 = Filter(u_E,v1_25,v2_25,a1_25,a2_25,b0_25,b1_25,b2_25,gain_25);
+    double y_50 = Filter(y_25,v1_50,v2_50,a1_50,a2_50,b0_50,b1_50,b2_50,gain_50);
+    // double y_100 = Filter(y_50,v1_100,v2_100,a1_100,a2_100,b0_100,b1_100,b2_100,gain_100);
+    //double y_200 = Filter(y_100,v1_200,v2_200,a1_200,a2_200,b0_200,b1_200,b2_200,gain_200);
     double y_HP = Filter(y_50,v1_HP,v2_HP,a1_HP,a2_HP,b0_HP,b1_HP,b2_HP,gain_HP);
     double y_abs = fabs(y_HP);
     double y_LP = Filter(y_abs,v1_LP,v2_LP,a1_LP,a2_LP,b0_LP,b1_LP,b2_LP,gain_LP);
-    double y = 10 + y_LP;
     
-    return y; 
+    double y_E = y_LP;
+    
+    return y_E; 
     
 }
\ No newline at end of file

--- a/FilterDesign.h	Thu Oct 27 13:51:14 2016 +0000
+++ b/FilterDesign.h	Wed Nov 02 10:45:10 2016 +0000
@@ -1,3 +1,3 @@
 #include "mbed.h"
 
-double FilterDesign(double u);
\ No newline at end of file
+double FilterDesign(double u_E);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FilterDesign2.cpp	Wed Nov 02 10:45:10 2016 +0000
@@ -0,0 +1,88 @@
+#include "FilterDesign2.h"
+#include "Filter2.h"
+    
+    // Notch 50 Hz 
+    double w1_50 = 0, w2_50 = 0;
+    const double a1_50 = -1.53713228931, a2_50 = 0.90250000000;
+    const double b0_50 = 1.00000000000, b1_50 = -1.61803398875, b2_50 = 1.00000000000;
+    const double gain_50 = 0.95500000000;
+    
+    // Notch 25 Hz 
+    double w1_25 = 0, w2_25 = 0;
+    const double a1_25 = -1.52753439307, a2_25 = 0.60559524000;
+    const double b0_25 = 1.00000000000, b1_25 = -1.90211303259, b2_25 = 1.00000000000;
+    const double gain_25 = 0.80000000000;
+    
+    // Notch 100 Hz
+    double w1_100 = 0, w2_100 = 0;
+    const double a1_100 = -0.58713228931, a2_100 = 0.90250000000;
+    const double b0_100 = 1.00000000000, b1_100 = -0.61803398875, b2_100 = 1.00000000000;
+    const double gain_100 = 0.95200000000;
+    
+    // Notch 200 Hz
+    double w1_200 = 0, w2_200 = 0;
+    const double a1_200 = 1.53713228931, a2_200 = 0.90250000000;
+    const double b0_200 = 1.00000000000, b1_200 = 1.61803398875, b2_200 = 1.00000000000;
+    const double gain_200 = 0.95500000000;
+    
+    // High pass 10 Hz (oud)
+    double w1_HP = 0, w2_HP = 0;
+    const double a1_HP = -1.91258127190, a2_HP = 0.91680625000;
+    const double b0_HP = 1.00000000000, b1_HP = -2.00000000000, b2_HP = 1.00000000000;
+    const double gain_HP = 0.95500000000;
+    /*
+    // High pass 40 Hz
+    double w1_HP = 0, w2_HP = 0;
+    const double a1_HP = -1.51877533120, a2_HP = 0.60062500000;
+    const double b0_HP = 1.00000000000, b1_HP = -2.00000000000, b2_HP = 1.00000000000;
+    const double gain_HP = 0.78000000000;
+    */
+    /*
+    // High pass 50 Hz 
+    double w1_HP = 0, w2_HP = 0;
+    const double a1_HP = -1.47413760026, a2_HP = 0.60062500000;
+    const double b0_HP = 1.00000000000, b1_HP = -2.00000000000, b2_HP = 1.00000000000;
+    const double gain_HP = 0.77000000000;
+    */
+    /*
+    // Low pass 11 Hz (oud)
+    double w1_LP = 0, w2_LP = 0;
+    const double a1_LP =  -1.67736610515, a2_LP = 0.72250000000;
+    const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
+    const double gain_LP = 0.0120000000000;
+    
+    // Low pass 0.7 Hz
+    double w1_LP = 0, w2_LP = 0;
+    const double a1_LP =  -1.80000000000, a2_LP = 0.81000000000;
+    const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
+    const double gain_LP = 0.0025100000000;
+    */
+    /*
+    // Low pass 3 Hz
+    double w1_LP = 0, w2_LP = 0;
+    const double a1_LP =  -1.70000000000, a2_LP = 0.72250000000;
+    const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
+    const double gain_LP = 0.0060000000000;
+    */
+    // Low pass 2 Hz
+    double w1_LP = 0, w2_LP = 0;
+    const double a1_LP =  -1.99277224437, a2_LP = 0.99281296000;
+    const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
+    const double gain_LP = 0.0001000000000;
+
+    
+double FilterDesign2(double u_E)
+{
+    double y_25 = Filter2(u_E,w1_25,w2_25,a1_25,a2_25,b0_25,b1_25,b2_25,gain_25);
+    double y_50 = Filter2(y_25,w1_50,w2_50,a1_50,a2_50,b0_50,b1_50,b2_50,gain_50);
+    // double y_100 = Filter2(u,w1_100,w2_100,a1_100,a2_100,b0_100,b1_100,b2_100,gain_100);
+    // double y_200 = Filter2(y_100,w1_200,w2_200,a1_200,a2_200,b0_200,b1_200,b2_200,gain_200);
+    double y_HP = Filter2(y_50,w1_HP,w2_HP,a1_HP,a2_HP,b0_HP,b1_HP,b2_HP,gain_HP);
+    double y_abs = fabs(y_HP);
+    double y_LP = Filter2(y_abs,w1_LP,w2_LP,a1_LP,a2_LP,b0_LP,b1_LP,b2_LP,gain_LP);
+    
+    double y_E = y_LP;
+    
+    return y_E; 
+    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FilterDesign2.h	Wed Nov 02 10:45:10 2016 +0000
@@ -0,0 +1,3 @@
+#include "mbed.h"
+
+double FilterDesign2(double u_E);
\ No newline at end of file

--- a/HIDScope.lib	Thu Oct 27 13:51:14 2016 +0000
+++ b/HIDScope.lib	Wed Nov 02 10:45:10 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/janwillembisschop/code/HIDScope/#b66f5585d5e0
+https://developer.mbed.org/users/wikdehaas/code/HIDScope/#b66f5585d5e0

--- a/controller.h	Thu Oct 27 13:51:14 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,21 +0,0 @@
-void controller_1()
-{
-double theta_1 = acos((sqrt(pow(x,2)+pow(yc,2))/(2*L2))+asin(yc/(sqrt(pow(x,2)+pow(yc,2))));
-double theta_2 = acos((x/L2)-cos(theta_1));
-
-double reference_1 = theta_1;     //reference
-double plaats_1 = 0.0014959*encoder_1.getPosition(); //positie encodercounts naar hoek
-double error_1 = reference_1 - plaats_1 ;
-double pwm_1 = PID(error_1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_N, m1_v1, m1_v2 );
-if (pwm_1<0){motor_1 = 1;}
-else {motor_1 = 0;}
-pwm_motor_1 = fabs(pwm_1);
-
-double reference_2 = theta_2;     //reference
-double plaats_2 = 0.0014959*encoder_2.getPosition(); //positie encodercounts naar hoek
-double error_2 = reference_2 - plaats_2 ;
-double pwm_2 = PID(error_2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_N, m2_v1, m2_v2 );
-if (pwm_2<0){motor_2 = 0;}
-else {motor_2 = 1;}
-pwm_motor_2 = fabs(pwm_2);
-}

--- a/main.cpp	Thu Oct 27 13:51:14 2016 +0000
+++ b/main.cpp	Wed Nov 02 10:45:10 2016 +0000
@@ -3,17 +3,25 @@
 #include "encoder.h"
 #include "math.h"
 #include "HIDScope.h"
+#include "Filter.h"
+#include "FilterDesign.h"
+#include "Filter2.h"
+#include "FilterDesign2.h"
 
 //D8 doet het niet
 
 DigitalIn    knop_1(D2); //Motor 1
 DigitalIn    knop_2(D3); //Motor 2
 
+AnalogIn    emg1(A0);
+AnalogIn    emg2(A1);
+
 DigitalOut motor_1(D7); //richting (1 of 0)
 PwmOut pwm_motor_1(D6); //snelheid (tussen 0 en 1)
 DigitalOut motor_2(D4); //richting (1 of 0)
 PwmOut pwm_motor_2(D5); //snelheid (tussen 0 en 1)
 InterruptIn stop(SW3); //stoppen
+InterruptIn beginopnieuw(SW2); //stoppen
 
 Serial pc(USBTX, USBRX); //USB ports aanroepen, D0 en D1
 
@@ -23,18 +31,18 @@
 HIDScope scope(4);
 
 Ticker PID_ticker;
+Ticker FILTER_ticker;
 
 Timer tijd;
 
 //constante waardes
-const double x_start = 0.27;
+const double x_start = 0.255;
 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 L1=0.30; //m
-const double L2=0.35; //m
-const double L3=0.13; //m
-const double y_base = 0.22;
+const double L2 = 0.35; //m
+const double y_base = 0.045;
 //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
@@ -44,24 +52,24 @@
 //beginpositie
 volatile double x = x_start; //m
 volatile double y = y_base; //m
-volatile double yc= y + L3 - L1; //m
 volatile bool opgepakt = false;
 volatile bool zakpunt = false;
 volatile bool uitdrukken = false;
-volatile double y_oppakken = 0.125, y_stijgen = 0.125, x_zakken = 0.1;
+volatile double y_oppakken = 0.10, y_stijgen = 0.15, x_zakken = 0.09;//Voorwaarde voor terugbewegen
+volatile bool run_programma = true;
 //PID
-const double m1_Kp = 20.0, m1_Ki = 0.5, m1_Kd = 0.6, m1_N = 50;
+const double m1_Kp = 2, m1_Ki = 5, m1_Kd = 0.1, m1_N = 50;
 double m1_v1 = 0, m1_v2 = 0; // Memory variables
-const double m1_Ts = 0.01; // Controller sample time
-const double m2_Kp = 20.0, m2_Ki = 0.5, m2_Kd = 0.6, m2_N = 50;
+const double m1_Ts = 0.001; // Controller sample time
+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.01; // Controller sample time
+const double m2_Ts = 0.001; // Controller sample time
 
 
 //Controller PID motors
 void Controller()
 {
-    theta_1 = acos((sqrt(pow(x,2)+pow(yc,2)))/(2*L2))+asin(yc/(sqrt(pow(x,2)+pow(yc,2))));
+    theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
     theta_2 = acos((x/L2)-cos(theta_1));
 
     reference_1 = -1.5*theta_1;     //reference
@@ -79,73 +87,108 @@
     if (pwm_2<0){motor_2 = 0;}
     else {motor_2 = 1;}
     pwm_motor_2 = fabs(pwm_2);
-    scope.set(0,pwm_1);
-    scope.set(1,error_1);
-    scope.set(2,pwm_2);
-    scope.set(3,error_2);
+    scope.set(0,error_1);
+    scope.set(1,reference_1);
+    scope.set(2,pwm_1);
+    scope.set(3,plaats_1);
     scope.send();
 }
+//Ticker filterwaardes
+void Filter_Samples()
+{
+        u_1 = emg1.read();
+        y_1 = FilterDesign(u_1);
+        u_2 = emg2.read();
+        y_2 = FilterDesign2(u_2);
+}
 
 //Failsave
 void Stop() //Zet allebei de snelheden op nul
 {
+    PID_ticker.detach();
     pwm_motor_1 = 0;
     pwm_motor_2 = 0;
-    while(true){}
+    run_programma = false;
+}
+
+void Beginopnieuw()
+{
+    run_programma = true;
+    x = x_start; y = y_base; m1_v1 = 0; m1_v2 = 0; m2_v1 = 0; m2_v2 = 0;
+    theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
+    theta_2 = acos((x/L2)-cos(theta_1));
+    double positie_motor_1 = -1.5*theta_1;
+    double positie_motor_2 = -1.5*theta_2;
+    encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
+    encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
+    PID_ticker.attach(&Controller,dt);
+    tijd.reset();
+    tijd.start();
 }
 
 int main()
 {
     pc.baud(115200);
     stop.fall(&Stop); //stop de snelheden van de motoren bij indrukken
-    tijd.reset();
-    tijd.start();
-    theta_1 = acos((sqrt(pow(x,2)+pow(yc,2)))/(2*L2))+asin(yc/(sqrt(pow(x,2)+pow(yc,2))));
+    beginopnieuw.fall(&Beginopnieuw);
+    pwm_motor_1.period_us(1);
+    pwm_motor_2.period_us(1);
+    theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
     theta_2 = acos((x/L2)-cos(theta_1));
     double positie_motor_1 = -1.5*theta_1;
     double positie_motor_2 = -1.5*theta_2;
     encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
     encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
+    FILTER_ticker.attach(&Filter_Samples,dt_f);
     PID_ticker.attach(&Controller,dt);
-    
+    wait(1);
+    tijd.reset();
+    tijd.start();
     while(true)
     {
+    while(run_programma)
+    {
     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){mode = 3;}
-    else                              {mode = 4;} //default
+    else if (knop_1 == 0 && knop_2 == 0 && x>(x_start+x_zakken)){mode = 3;}
+    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;} 
+    else if (y_1 >= 0.4 && y_2 >= 0.4 && x>(x_start+x_zakken)){mode = 3;}
+    else                                                      {mode = 4;} //default
+    */
     switch (mode)
     {
         case 1: x = x + tijd_verschil*Vx;
                 if (x>0.6){x = 0.6;}
                 y = y_base;
-                yc= y + L3 - L1;
                 break;
         case 2: x = x - tijd_verschil*Vx;
                 if (x<x_start){x = x_start;}
                 y = y_base;
-                yc= y + L3 - L1;
                 break;
         case 3: while(y > (y_base-y_oppakken))    
                 {
                     tijd_nieuw = tijd;
                     tijd_verschil = tijd_nieuw - tijd_oud;        
-                    y = y - tijd_verschil*Vy*4;
-                    yc= y + L3 - L1;
+                    y = y - tijd_verschil*Vy*3;
                     x = x;
                     tijd_oud = tijd_nieuw;
                 }
+                wait(1);
+                tijd_nieuw = tijd;
+                tijd_oud = tijd_nieuw;
                 while(y < (y_base+y_stijgen))
                 {
                     tijd_nieuw = tijd;
                     tijd_verschil = tijd_nieuw - tijd_oud;        
-                    y = y + tijd_verschil*Vy*2;
-                    yc= y + L3 - L1;
+                    y = y + tijd_verschil*Vy*5;
                     x = x;
                     tijd_oud = tijd_nieuw;
                 }
@@ -153,36 +196,33 @@
                 {
                     tijd_nieuw = tijd;
                     tijd_verschil = tijd_nieuw - tijd_oud;        
-                    x = x-tijd_verschil*Vx;
-                    yc = yc;
+                    x = x-tijd_verschil*Vx*2;
                     tijd_oud = tijd_nieuw;
                 }
                 while(y>y_base)
                 {
                     tijd_nieuw = tijd;
                     tijd_verschil = tijd_nieuw - tijd_oud;    
-                    y = y - tijd_verschil*Vy;
-                    yc= y + L3 - L1;
+                    y = y - tijd_verschil*Vy*2;
                     x = x;
                     tijd_oud = tijd_nieuw;
                 }
-                while(x>x_start-0.04)
+                while(x>x_start-0.02)
                 {
                     tijd_nieuw = tijd;
                     tijd_verschil = tijd_nieuw - tijd_oud;   
                     y = y_base;
-                    yc= y + L3 - L1;
-                    x = x - tijd_verschil*Vx;
+                    x = x - tijd_verschil*Vx*2;
                     tijd_oud = tijd_nieuw;
                 }
                 wait(0.5);
                     x = x_start;
                     y = y_base;
-                    yc= y + L3 - L1;
                 break;
         default: x = x;
-                 yc = yc;
+                 y = y;
                  break;
     }    
     }
+    }
 }
\ No newline at end of file