Wik de Haas / Mbed 2 deprecated Aansturing_knoppen

Aansturing

Dependencies:   Encoder mbed HIDScope

main.cpp@3:57b98989b0b1, 2016-11-03 (annotated)

Committer:
wikdehaas
Date:
Thu Nov 03 11:33:29 2016 +0000
Revision:
3:57b98989b0b1
Parent:
2:a42b34f9d6ab 
TY

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wikdehaas 0:55f9447aa02b 1 #include "mbed.h"
wikdehaas 0:55f9447aa02b 2 #include "PID.h"
wikdehaas 0:55f9447aa02b 3 #include "encoder.h"
wikdehaas 0:55f9447aa02b 4 #include "math.h"
wikdehaas 1:a644028231b5 5 #include "HIDScope.h"
wikdehaas 2:a42b34f9d6ab 6 #include "Filter.h"
wikdehaas 2:a42b34f9d6ab 7 #include "FilterDesign.h"
wikdehaas 2:a42b34f9d6ab 8 #include "Filter2.h"
wikdehaas 2:a42b34f9d6ab 9 #include "FilterDesign2.h"
wikdehaas 0:55f9447aa02b 10
wikdehaas 0:55f9447aa02b 11 //D8 doet het niet
wikdehaas 0:55f9447aa02b 12
wikdehaas 0:55f9447aa02b 13 DigitalIn knop_1(D2); //Motor 1
wikdehaas 0:55f9447aa02b 14 DigitalIn knop_2(D3); //Motor 2
wikdehaas 0:55f9447aa02b 15
wikdehaas 2:a42b34f9d6ab 16 AnalogIn emg1(A0);
wikdehaas 2:a42b34f9d6ab 17 AnalogIn emg2(A1);
wikdehaas 2:a42b34f9d6ab 18
wikdehaas 0:55f9447aa02b 19 DigitalOut motor_1(D7); //richting (1 of 0)
wikdehaas 0:55f9447aa02b 20 PwmOut pwm_motor_1(D6); //snelheid (tussen 0 en 1)
wikdehaas 0:55f9447aa02b 21 DigitalOut motor_2(D4); //richting (1 of 0)
wikdehaas 0:55f9447aa02b 22 PwmOut pwm_motor_2(D5); //snelheid (tussen 0 en 1)
wikdehaas 0:55f9447aa02b 23 InterruptIn stop(SW3); //stoppen
wikdehaas 2:a42b34f9d6ab 24 InterruptIn beginopnieuw(SW2); //stoppen
wikdehaas 0:55f9447aa02b 25
wikdehaas 0:55f9447aa02b 26 Serial pc(USBTX, USBRX); //USB ports aanroepen, D0 en D1
wikdehaas 0:55f9447aa02b 27
wikdehaas 0:55f9447aa02b 28 Encoder encoder_1(D13,D12);
wikdehaas 0:55f9447aa02b 29 Encoder encoder_2(D10,D9);
wikdehaas 0:55f9447aa02b 30
wikdehaas 1:a644028231b5 31 HIDScope scope(4);
wikdehaas 1:a644028231b5 32
wikdehaas 0:55f9447aa02b 33 Ticker PID_ticker;
wikdehaas 2:a42b34f9d6ab 34 Ticker FILTER_ticker;
wikdehaas 0:55f9447aa02b 35
wikdehaas 0:55f9447aa02b 36 Timer tijd;
wikdehaas 0:55f9447aa02b 37
wikdehaas 0:55f9447aa02b 38 //constante waardes
wikdehaas 2:a42b34f9d6ab 39 const double x_start = 0.255;
wikdehaas 0:55f9447aa02b 40 const double dt = 0.001;
wikdehaas 2:a42b34f9d6ab 41 const double dt_f = 0.001;
wikdehaas 0:55f9447aa02b 42 const double Vx = 0.05; //m/s
wikdehaas 3:57b98989b0b1 43 const double Vy = 0.05; //m/s
wikdehaas 2:a42b34f9d6ab 44 const double L2 = 0.35; //m
wikdehaas 2:a42b34f9d6ab 45 const double y_base = 0.045;
wikdehaas 3:57b98989b0b1 46 //gegevens
wikdehaas 3:57b98989b0b1 47 const double x_plek_T = 0.43, y_streep_TY_hoogte = 0.15;
wikdehaas 3:57b98989b0b1 48 const double x_streep_half = 0.06, x_plek_Y = x_plek_T+0.13, y_midden_Y = 0.09;
wikdehaas 0:55f9447aa02b 49 //filter
wikdehaas 0:55f9447aa02b 50 volatile double tijd_oud, tijd_nieuw, tijd_verschil;
wikdehaas 0:55f9447aa02b 51 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
wikdehaas 0:55f9447aa02b 52 volatile int mode; //mode waarin de motors moeten bewegen
wikdehaas 0:55f9447aa02b 53 //controller
wikdehaas 0:55f9447aa02b 54 volatile double theta_1, theta_2, reference_1, plaats_1, error_1, pwm_1, reference_2, plaats_2, error_2, pwm_2;
wikdehaas 0:55f9447aa02b 55 //beginpositie
wikdehaas 0:55f9447aa02b 56 volatile double x = x_start; //m
wikdehaas 0:55f9447aa02b 57 volatile double y = y_base; //m
wikdehaas 0:55f9447aa02b 58 volatile bool opgepakt = false;
wikdehaas 0:55f9447aa02b 59 volatile bool zakpunt = false;
wikdehaas 0:55f9447aa02b 60 volatile bool uitdrukken = false;
wikdehaas 3:57b98989b0b1 61 volatile double y_oppakken = 0.135, y_stijgen = 0.15, x_zakken = 0.09;//Voorwaarde voor terugbewegen
wikdehaas 2:a42b34f9d6ab 62 volatile bool run_programma = true;
wikdehaas 0:55f9447aa02b 63 //PID
wikdehaas 2:a42b34f9d6ab 64 const double m1_Kp = 2, m1_Ki = 5, m1_Kd = 0.1, m1_N = 50;
wikdehaas 0:55f9447aa02b 65 double m1_v1 = 0, m1_v2 = 0; // Memory variables
wikdehaas 2:a42b34f9d6ab 66 const double m1_Ts = 0.001; // Controller sample time
wikdehaas 2:a42b34f9d6ab 67 const double m2_Kp = 2, m2_Ki = 5, m2_Kd = 0.1, m2_N = 50;
wikdehaas 0:55f9447aa02b 68 double m2_v1 = 0, m2_v2 = 0; // Memory variables
wikdehaas 2:a42b34f9d6ab 69 const double m2_Ts = 0.001; // Controller sample time
wikdehaas 3:57b98989b0b1 70 volatile double compensatie = 0;
wikdehaas 0:55f9447aa02b 71
wikdehaas 0:55f9447aa02b 72
wikdehaas 0:55f9447aa02b 73 //Controller PID motors
wikdehaas 0:55f9447aa02b 74 void Controller()
wikdehaas 0:55f9447aa02b 75 {
wikdehaas 2:a42b34f9d6ab 76 theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
wikdehaas 0:55f9447aa02b 77 theta_2 = acos((x/L2)-cos(theta_1));
wikdehaas 0:55f9447aa02b 78
wikdehaas 1:a644028231b5 79 reference_1 = -1.5*theta_1; //reference
wikdehaas 0:55f9447aa02b 80 plaats_1 = 0.0014959*encoder_1.getPosition(); //positie encodercounts naar hoek
wikdehaas 0:55f9447aa02b 81 error_1 = reference_1 - plaats_1 ;
wikdehaas 0:55f9447aa02b 82 pwm_1 = PID(error_1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_N, m1_v1, m1_v2 );
wikdehaas 0:55f9447aa02b 83 if (pwm_1<0){motor_1 = 1;}
wikdehaas 0:55f9447aa02b 84 else {motor_1 = 0;}
wikdehaas 0:55f9447aa02b 85 pwm_motor_1 = fabs(pwm_1);
wikdehaas 0:55f9447aa02b 86
wikdehaas 1:a644028231b5 87 reference_2 = -1.5*theta_2; //reference
wikdehaas 0:55f9447aa02b 88 plaats_2 = 0.0014959*encoder_2.getPosition(); //positie encodercounts naar hoek
wikdehaas 0:55f9447aa02b 89 error_2 = reference_2 - plaats_2 ;
wikdehaas 3:57b98989b0b1 90 pwm_2 = PID(error_2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_N, m2_v1, m2_v2 )-compensatie;
wikdehaas 0:55f9447aa02b 91 if (pwm_2<0){motor_2 = 0;}
wikdehaas 0:55f9447aa02b 92 else {motor_2 = 1;}
wikdehaas 0:55f9447aa02b 93 pwm_motor_2 = fabs(pwm_2);
wikdehaas 3:57b98989b0b1 94 scope.set(0,u_1);
wikdehaas 3:57b98989b0b1 95 scope.set(1,y_1);
wikdehaas 3:57b98989b0b1 96 scope.set(2,u_2);
wikdehaas 3:57b98989b0b1 97 scope.set(3,y_2);
wikdehaas 1:a644028231b5 98 scope.send();
wikdehaas 0:55f9447aa02b 99 }
wikdehaas 2:a42b34f9d6ab 100 //Ticker filterwaardes
wikdehaas 2:a42b34f9d6ab 101 void Filter_Samples()
wikdehaas 2:a42b34f9d6ab 102 {
wikdehaas 2:a42b34f9d6ab 103 u_1 = emg1.read();
wikdehaas 2:a42b34f9d6ab 104 y_1 = FilterDesign(u_1);
wikdehaas 2:a42b34f9d6ab 105 u_2 = emg2.read();
wikdehaas 2:a42b34f9d6ab 106 y_2 = FilterDesign2(u_2);
wikdehaas 2:a42b34f9d6ab 107 }
wikdehaas 0:55f9447aa02b 108
wikdehaas 0:55f9447aa02b 109 //Failsave
wikdehaas 0:55f9447aa02b 110 void Stop() //Zet allebei de snelheden op nul
wikdehaas 0:55f9447aa02b 111 {
wikdehaas 2:a42b34f9d6ab 112 PID_ticker.detach();
wikdehaas 0:55f9447aa02b 113 pwm_motor_1 = 0;
wikdehaas 0:55f9447aa02b 114 pwm_motor_2 = 0;
wikdehaas 2:a42b34f9d6ab 115 run_programma = false;
wikdehaas 2:a42b34f9d6ab 116 }
wikdehaas 2:a42b34f9d6ab 117
wikdehaas 2:a42b34f9d6ab 118 void Beginopnieuw()
wikdehaas 2:a42b34f9d6ab 119 {
wikdehaas 2:a42b34f9d6ab 120 run_programma = true;
wikdehaas 2:a42b34f9d6ab 121 x = x_start; y = y_base; m1_v1 = 0; m1_v2 = 0; m2_v1 = 0; m2_v2 = 0;
wikdehaas 2:a42b34f9d6ab 122 theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
wikdehaas 2:a42b34f9d6ab 123 theta_2 = acos((x/L2)-cos(theta_1));
wikdehaas 2:a42b34f9d6ab 124 double positie_motor_1 = -1.5*theta_1;
wikdehaas 2:a42b34f9d6ab 125 double positie_motor_2 = -1.5*theta_2;
wikdehaas 2:a42b34f9d6ab 126 encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
wikdehaas 2:a42b34f9d6ab 127 encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
wikdehaas 2:a42b34f9d6ab 128 PID_ticker.attach(&Controller,dt);
wikdehaas 2:a42b34f9d6ab 129 tijd.reset();
wikdehaas 2:a42b34f9d6ab 130 tijd.start();
wikdehaas 0:55f9447aa02b 131 }
wikdehaas 0:55f9447aa02b 132
wikdehaas 0:55f9447aa02b 133 int main()
wikdehaas 0:55f9447aa02b 134 {
wikdehaas 0:55f9447aa02b 135 pc.baud(115200);
wikdehaas 0:55f9447aa02b 136 stop.fall(&Stop); //stop de snelheden van de motoren bij indrukken
wikdehaas 2:a42b34f9d6ab 137 beginopnieuw.fall(&Beginopnieuw);
wikdehaas 2:a42b34f9d6ab 138 pwm_motor_1.period_us(1);
wikdehaas 2:a42b34f9d6ab 139 pwm_motor_2.period_us(1);
wikdehaas 2:a42b34f9d6ab 140 theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
wikdehaas 0:55f9447aa02b 141 theta_2 = acos((x/L2)-cos(theta_1));
wikdehaas 1:a644028231b5 142 double positie_motor_1 = -1.5*theta_1;
wikdehaas 1:a644028231b5 143 double positie_motor_2 = -1.5*theta_2;
wikdehaas 1:a644028231b5 144 encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
wikdehaas 1:a644028231b5 145 encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
wikdehaas 2:a42b34f9d6ab 146 FILTER_ticker.attach(&Filter_Samples,dt_f);
wikdehaas 1:a644028231b5 147 PID_ticker.attach(&Controller,dt);
wikdehaas 2:a42b34f9d6ab 148 wait(1);
wikdehaas 2:a42b34f9d6ab 149 tijd.reset();
wikdehaas 2:a42b34f9d6ab 150 tijd.start();
wikdehaas 0:55f9447aa02b 151 while(true)
wikdehaas 0:55f9447aa02b 152 {
wikdehaas 2:a42b34f9d6ab 153 while(run_programma)
wikdehaas 2:a42b34f9d6ab 154 {
wikdehaas 0:55f9447aa02b 155 tijd_nieuw = tijd;
wikdehaas 0:55f9447aa02b 156 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 0:55f9447aa02b 157 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 158
wikdehaas 0:55f9447aa02b 159 if (knop_1 == 0 && knop_2 == 1){mode = 1;}
wikdehaas 0:55f9447aa02b 160 else if (knop_1 == 1 && knop_2 == 0){mode = 2;}
wikdehaas 2:a42b34f9d6ab 161 else if (knop_1 == 0 && knop_2 == 0 && x>(x_start+x_zakken)){mode = 3;}
wikdehaas 3:57b98989b0b1 162 else if (knop_1 == 0 && knop_2 == 0 && x<x_plek_T) {mode = 5;}
wikdehaas 2:a42b34f9d6ab 163 else {mode = 4;} //default
wikdehaas 2:a42b34f9d6ab 164 /*
wikdehaas 2:a42b34f9d6ab 165 if (y_1 >= 0.4 && y_2 < 0.4) {mode = 1;}
wikdehaas 2:a42b34f9d6ab 166 else if (y_1 < 0.4 && y_2 >= 0.4) {mode = 2;}
wikdehaas 2:a42b34f9d6ab 167 else if (y_1 >= 0.4 && y_2 >= 0.4 && x>(x_start+x_zakken)){mode = 3;}
wikdehaas 2:a42b34f9d6ab 168 else {mode = 4;} //default
wikdehaas 2:a42b34f9d6ab 169 */
wikdehaas 0:55f9447aa02b 170 switch (mode)
wikdehaas 0:55f9447aa02b 171 {
wikdehaas 1:a644028231b5 172 case 1: x = x + tijd_verschil*Vx;
wikdehaas 1:a644028231b5 173 if (x>0.6){x = 0.6;}
wikdehaas 0:55f9447aa02b 174 y = y_base;
wikdehaas 0:55f9447aa02b 175 break;
wikdehaas 1:a644028231b5 176 case 2: x = x - tijd_verschil*Vx;
wikdehaas 1:a644028231b5 177 if (x<x_start){x = x_start;}
wikdehaas 0:55f9447aa02b 178 y = y_base;
wikdehaas 0:55f9447aa02b 179 break;
wikdehaas 0:55f9447aa02b 180 case 3: while(y > (y_base-y_oppakken))
wikdehaas 0:55f9447aa02b 181 {
wikdehaas 0:55f9447aa02b 182 tijd_nieuw = tijd;
wikdehaas 0:55f9447aa02b 183 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 184 y = y - tijd_verschil*Vy*5;
wikdehaas 0:55f9447aa02b 185 x = x;
wikdehaas 0:55f9447aa02b 186 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 187 }
wikdehaas 3:57b98989b0b1 188 wait(0.05);
wikdehaas 2:a42b34f9d6ab 189 tijd_nieuw = tijd;
wikdehaas 2:a42b34f9d6ab 190 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 191 while(y < (y_base+y_stijgen))
wikdehaas 0:55f9447aa02b 192 {
wikdehaas 0:55f9447aa02b 193 tijd_nieuw = tijd;
wikdehaas 0:55f9447aa02b 194 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 2:a42b34f9d6ab 195 y = y + tijd_verschil*Vy*5;
wikdehaas 0:55f9447aa02b 196 x = x;
wikdehaas 0:55f9447aa02b 197 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 198 }
wikdehaas 0:55f9447aa02b 199 while(x > (x_start+x_zakken))
wikdehaas 0:55f9447aa02b 200 {
wikdehaas 0:55f9447aa02b 201 tijd_nieuw = tijd;
wikdehaas 0:55f9447aa02b 202 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 2:a42b34f9d6ab 203 x = x-tijd_verschil*Vx*2;
wikdehaas 0:55f9447aa02b 204 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 205 }
wikdehaas 0:55f9447aa02b 206 while(y>y_base)
wikdehaas 0:55f9447aa02b 207 {
wikdehaas 0:55f9447aa02b 208 tijd_nieuw = tijd;
wikdehaas 0:55f9447aa02b 209 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 2:a42b34f9d6ab 210 y = y - tijd_verschil*Vy*2;
wikdehaas 0:55f9447aa02b 211 x = x;
wikdehaas 0:55f9447aa02b 212 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 213 }
wikdehaas 3:57b98989b0b1 214 while(x>x_start-0.03)
wikdehaas 0:55f9447aa02b 215 {
wikdehaas 0:55f9447aa02b 216 tijd_nieuw = tijd;
wikdehaas 0:55f9447aa02b 217 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 0:55f9447aa02b 218 y = y_base;
wikdehaas 2:a42b34f9d6ab 219 x = x - tijd_verschil*Vx*2;
wikdehaas 3:57b98989b0b1 220 if (x<x_start)
wikdehaas 3:57b98989b0b1 221 {
wikdehaas 3:57b98989b0b1 222 compensatie = 0.4;
wikdehaas 3:57b98989b0b1 223 }
wikdehaas 0:55f9447aa02b 224 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 225 }
wikdehaas 3:57b98989b0b1 226 compensatie = 0;
wikdehaas 1:a644028231b5 227 wait(0.5);
wikdehaas 0:55f9447aa02b 228 x = x_start;
wikdehaas 0:55f9447aa02b 229 y = y_base;
wikdehaas 0:55f9447aa02b 230 break;
wikdehaas 3:57b98989b0b1 231 case 5: //Naar onderkant T
wikdehaas 3:57b98989b0b1 232 while(x < x_plek_T)
wikdehaas 3:57b98989b0b1 233 {
wikdehaas 3:57b98989b0b1 234 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 235 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 236 x = x + tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 237 y = y;
wikdehaas 3:57b98989b0b1 238 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 239 }
wikdehaas 3:57b98989b0b1 240 //Naar bovenkant T
wikdehaas 3:57b98989b0b1 241 while(y < (y_base+y_streep_TY_hoogte))
wikdehaas 3:57b98989b0b1 242 {
wikdehaas 3:57b98989b0b1 243 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 244 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 245 y = y + tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 246 x = x;
wikdehaas 3:57b98989b0b1 247 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 248 }
wikdehaas 3:57b98989b0b1 249 //Naar linkerkant bovenkant T
wikdehaas 3:57b98989b0b1 250 while(x > (x_plek_T - x_streep_half))
wikdehaas 3:57b98989b0b1 251 {
wikdehaas 3:57b98989b0b1 252 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 253 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 254 x = x - tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 255 y = y;
wikdehaas 3:57b98989b0b1 256 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 257 }
wikdehaas 3:57b98989b0b1 258 //Naar rechterkant bovenkant T
wikdehaas 3:57b98989b0b1 259 while(x < (x_plek_T + x_streep_half))
wikdehaas 3:57b98989b0b1 260 {
wikdehaas 3:57b98989b0b1 261 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 262 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 263 x = x + tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 264 y = y;
wikdehaas 3:57b98989b0b1 265 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 266 }
wikdehaas 3:57b98989b0b1 267 //Naar midden T
wikdehaas 3:57b98989b0b1 268 while(x > x_plek_T)
wikdehaas 3:57b98989b0b1 269 {
wikdehaas 3:57b98989b0b1 270 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 271 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 272 x = x - tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 273 y = y;
wikdehaas 3:57b98989b0b1 274 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 275 }
wikdehaas 3:57b98989b0b1 276 //Naar onderkant T
wikdehaas 3:57b98989b0b1 277 while(y > y_base)
wikdehaas 3:57b98989b0b1 278 {
wikdehaas 3:57b98989b0b1 279 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 280 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 281 y = y - tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 282 x = x;
wikdehaas 3:57b98989b0b1 283 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 284 }
wikdehaas 3:57b98989b0b1 285 //Naar onderkant Y
wikdehaas 3:57b98989b0b1 286 while(x < x_plek_Y)
wikdehaas 3:57b98989b0b1 287 {
wikdehaas 3:57b98989b0b1 288 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 289 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 290 x = x + tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 291 y = y;
wikdehaas 3:57b98989b0b1 292 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 293 }
wikdehaas 3:57b98989b0b1 294 //Naar midden Y
wikdehaas 3:57b98989b0b1 295 while(y < (y_base+y_midden_Y))
wikdehaas 3:57b98989b0b1 296 {
wikdehaas 3:57b98989b0b1 297 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 298 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 299 y = y + tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 300 x = x;
wikdehaas 3:57b98989b0b1 301 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 302 }
wikdehaas 3:57b98989b0b1 303 //Naar linksboven Y
wikdehaas 3:57b98989b0b1 304 while(y < (y_base+y_streep_TY_hoogte))
wikdehaas 3:57b98989b0b1 305 {
wikdehaas 3:57b98989b0b1 306 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 307 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 308 y = y + tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 309 x = x - tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 310 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 311 }
wikdehaas 3:57b98989b0b1 312 //Naar midden Y
wikdehaas 3:57b98989b0b1 313 while(y > (y_base+y_midden_Y))
wikdehaas 3:57b98989b0b1 314 {
wikdehaas 3:57b98989b0b1 315 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 316 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 317 y = y - tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 318 x = x + tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 319 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 320 }
wikdehaas 3:57b98989b0b1 321 //Naar rechtsboven Y
wikdehaas 3:57b98989b0b1 322 while(y < (y_base+y_streep_TY_hoogte))
wikdehaas 3:57b98989b0b1 323 {
wikdehaas 3:57b98989b0b1 324 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 325 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 326 y = y + tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 327 x = x + tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 328 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 329 }
wikdehaas 3:57b98989b0b1 330 //Naar midden Y
wikdehaas 3:57b98989b0b1 331 while(y > (y_base+y_midden_Y))
wikdehaas 3:57b98989b0b1 332 {
wikdehaas 3:57b98989b0b1 333 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 334 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 335 y = y - tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 336 x = x - tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 337 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 338 }
wikdehaas 3:57b98989b0b1 339 //Naar onderkant Y
wikdehaas 3:57b98989b0b1 340 while(y > y_base)
wikdehaas 3:57b98989b0b1 341 {
wikdehaas 3:57b98989b0b1 342 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 343 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 344 y = y - tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 345 x = x;
wikdehaas 3:57b98989b0b1 346 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 347 }
wikdehaas 3:57b98989b0b1 348 //Naar basis
wikdehaas 3:57b98989b0b1 349 while(x > x_start)
wikdehaas 3:57b98989b0b1 350 {
wikdehaas 3:57b98989b0b1 351 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 352 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 353 x = x - tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 354 y = y;
wikdehaas 3:57b98989b0b1 355 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 356 }
wikdehaas 3:57b98989b0b1 357 break;
wikdehaas 0:55f9447aa02b 358 default: x = x;
wikdehaas 2:a42b34f9d6ab 359 y = y;
wikdehaas 0:55f9447aa02b 360 break;
wikdehaas 0:55f9447aa02b 361 }
wikdehaas 0:55f9447aa02b 362 }
wikdehaas 2:a42b34f9d6ab 363 }
wikdehaas 0:55f9447aa02b 364 }