Wik de Haas / Mbed 2 deprecated Aansturing_Groep_4

Aansturing robot groep 4 Biorobotics 2016. Beer bottle remover

Dependencies:   Encoder HIDScope mbed

Fork of Aansturing_knoppen by Wik de Haas

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wikdehaas 3:57b98989b0b1 1 /*
wikdehaas 3:57b98989b0b1 2 if(knop_1 == 0 && knop_2 == 0 && x<x_plek_T)
wikdehaas 3:57b98989b0b1 3
wikdehaas 3:57b98989b0b1 4 //gegevens
wikdehaas 3:57b98989b0b1 5 const double x_plek_T = 0.35, y_streep_TY_hoogte = 0.15;
wikdehaas 3:57b98989b0b1 6 const double x_streep_half = 0.05, x_plek_Y = 0.45, y_midden_Y = 0.09;
wikdehaas 3:57b98989b0b1 7
wikdehaas 3:57b98989b0b1 8 case 5: //Naar onderkant T
wikdehaas 3:57b98989b0b1 9 while(x < x_plek_T)
wikdehaas 3:57b98989b0b1 10 {
wikdehaas 3:57b98989b0b1 11 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 12 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 13 x = x + tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 14 y = y;
wikdehaas 3:57b98989b0b1 15 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 16 }
wikdehaas 3:57b98989b0b1 17 //Naar bovenkant T
wikdehaas 3:57b98989b0b1 18 while(y < (y_base+y_streep_TY_hoogte))
wikdehaas 3:57b98989b0b1 19 {
wikdehaas 3:57b98989b0b1 20 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 21 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 22 y = y + tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 23 x = x;
wikdehaas 3:57b98989b0b1 24 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 25 }
wikdehaas 3:57b98989b0b1 26 //Naar linkerkant bovenkant T
wikdehaas 3:57b98989b0b1 27 while(x > (x_plek_T - x_streep_half))
wikdehaas 3:57b98989b0b1 28 {
wikdehaas 3:57b98989b0b1 29 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 30 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 31 x = x - tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 32 y = y;
wikdehaas 3:57b98989b0b1 33 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 34 }
wikdehaas 3:57b98989b0b1 35 //Naar rechterkant bovenkant T
wikdehaas 3:57b98989b0b1 36 while(x < (x_plek_T + x_streep_half))
wikdehaas 3:57b98989b0b1 37 {
wikdehaas 3:57b98989b0b1 38 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 39 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 40 x = x + tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 41 y = y;
wikdehaas 3:57b98989b0b1 42 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 43 }
wikdehaas 3:57b98989b0b1 44 //Naar midden T
wikdehaas 3:57b98989b0b1 45 while(x > x_plek_T)
wikdehaas 3:57b98989b0b1 46 {
wikdehaas 3:57b98989b0b1 47 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 48 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 49 x = x - tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 50 y = y;
wikdehaas 3:57b98989b0b1 51 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 52 }
wikdehaas 3:57b98989b0b1 53 //Naar onderkant T
wikdehaas 3:57b98989b0b1 54 while(y > y_base)
wikdehaas 3:57b98989b0b1 55 {
wikdehaas 3:57b98989b0b1 56 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 57 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 58 y = y - tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 59 x = x;
wikdehaas 3:57b98989b0b1 60 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 61 }
wikdehaas 3:57b98989b0b1 62 //Naar onderkant Y
wikdehaas 3:57b98989b0b1 63 while(x < x_plek_Y)
wikdehaas 3:57b98989b0b1 64 {
wikdehaas 3:57b98989b0b1 65 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 66 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 67 x = x + tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 68 y = y;
wikdehaas 3:57b98989b0b1 69 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 70 }
wikdehaas 3:57b98989b0b1 71 //Naar midden Y
wikdehaas 3:57b98989b0b1 72 while(y < (y_base+y_midden_Y))
wikdehaas 3:57b98989b0b1 73 {
wikdehaas 3:57b98989b0b1 74 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 75 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 76 y = y + tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 77 x = x;
wikdehaas 3:57b98989b0b1 78 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 79 }
wikdehaas 3:57b98989b0b1 80 //Naar linksboven Y
wikdehaas 3:57b98989b0b1 81 while(y < (y_base+y_streep_TY_hoogte))
wikdehaas 3:57b98989b0b1 82 {
wikdehaas 3:57b98989b0b1 83 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 84 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 85 y = y + tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 86 x = x - tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 87 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 88 }
wikdehaas 3:57b98989b0b1 89 //Naar midden Y
wikdehaas 3:57b98989b0b1 90 while(y > (y_base+y_midden_Y))
wikdehaas 3:57b98989b0b1 91 {
wikdehaas 3:57b98989b0b1 92 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 93 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 94 y = y - tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 95 x = x + tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 96 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 97 }
wikdehaas 3:57b98989b0b1 98 //Naar rechtsboven Y
wikdehaas 3:57b98989b0b1 99 while(y < (y_base+y_streep_TY_hoogte))
wikdehaas 3:57b98989b0b1 100 {
wikdehaas 3:57b98989b0b1 101 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 102 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 103 y = y + tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 104 x = x + tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 105 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 106 }
wikdehaas 3:57b98989b0b1 107 //Naar midden Y
wikdehaas 3:57b98989b0b1 108 while(y > (y_base+y_midden_Y))
wikdehaas 3:57b98989b0b1 109 {
wikdehaas 3:57b98989b0b1 110 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 111 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 112 y = y - tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 113 x = x - tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 114 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 115 }
wikdehaas 3:57b98989b0b1 116 //Naar onderkant Y
wikdehaas 3:57b98989b0b1 117 while(y > y_base)
wikdehaas 3:57b98989b0b1 118 {
wikdehaas 3:57b98989b0b1 119 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 120 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 121 y = y - tijd_verschil*Vy;
wikdehaas 3:57b98989b0b1 122 x = x;
wikdehaas 3:57b98989b0b1 123 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 124 }
wikdehaas 3:57b98989b0b1 125 //Naar basis
wikdehaas 3:57b98989b0b1 126 while(x > x_start)
wikdehaas 3:57b98989b0b1 127 {
wikdehaas 3:57b98989b0b1 128 tijd_nieuw = tijd;
wikdehaas 3:57b98989b0b1 129 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 3:57b98989b0b1 130 x = x - tijd_verschil*Vx;
wikdehaas 3:57b98989b0b1 131 y = y;
wikdehaas 3:57b98989b0b1 132 tijd_oud = tijd_nieuw;
wikdehaas 3:57b98989b0b1 133 }
wikdehaas 3:57b98989b0b1 134 break;
wikdehaas 3:57b98989b0b1 135 */