Jitse Giesen / Mbed 2 deprecated master

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   biquadFilter mbed MODSERIAL

main.cpp@0:51a6e38a4d4a, 2017-10-26 (annotated)

Committer:
Jitse_Giesen
Date:
Thu Oct 26 12:49:05 2017 +0000
Revision:
0:51a6e38a4d4a
Child:
1:9e871647a074
even saven voor ik het verneuk;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jitse_Giesen 0:51a6e38a4d4a 1 #include "mbed.h"
Jitse_Giesen 0:51a6e38a4d4a 2 #include "math.h"
Jitse_Giesen 0:51a6e38a4d4a 3 #include "encoder.h"
Jitse_Giesen 0:51a6e38a4d4a 4 #include "QEI.h"
Jitse_Giesen 0:51a6e38a4d4a 5 #include "BiQuad.h"
Jitse_Giesen 0:51a6e38a4d4a 6
Jitse_Giesen 0:51a6e38a4d4a 7 bool Move_done=false;
Jitse_Giesen 0:51a6e38a4d4a 8
Jitse_Giesen 0:51a6e38a4d4a 9
Jitse_Giesen 0:51a6e38a4d4a 10
Jitse_Giesen 0:51a6e38a4d4a 11 Serial pc(USBTX, USBRX);
Jitse_Giesen 0:51a6e38a4d4a 12
Jitse_Giesen 0:51a6e38a4d4a 13 //Defining all in- and outputs
Jitse_Giesen 0:51a6e38a4d4a 14 //EMG input
Jitse_Giesen 0:51a6e38a4d4a 15 AnalogIn emgBR( A0 ); //Right Biceps
Jitse_Giesen 0:51a6e38a4d4a 16 AnalogIn emgBL( A1 ); //Left Biceps
Jitse_Giesen 0:51a6e38a4d4a 17
Jitse_Giesen 0:51a6e38a4d4a 18 //Output motor 1 and reading Encoder motor 1
Jitse_Giesen 0:51a6e38a4d4a 19 DigitalOut motor1DirectionPin(D4);
Jitse_Giesen 0:51a6e38a4d4a 20 PwmOut motor1MagnitudePin(D5);
Jitse_Giesen 0:51a6e38a4d4a 21 QEI Encoder1(D12,D13,NC,32);
Jitse_Giesen 0:51a6e38a4d4a 22
Jitse_Giesen 0:51a6e38a4d4a 23
Jitse_Giesen 0:51a6e38a4d4a 24 //Output motor 2 and reading Encoder motor 2
Jitse_Giesen 0:51a6e38a4d4a 25 DigitalOut motor2DirectionPin(D7);
Jitse_Giesen 0:51a6e38a4d4a 26 PwmOut motor2MagnitudePin(D6);
Jitse_Giesen 0:51a6e38a4d4a 27 QEI Encoder2(D10,D11,NC,32);
Jitse_Giesen 0:51a6e38a4d4a 28
Jitse_Giesen 0:51a6e38a4d4a 29 //LED output, needed for feedback
Jitse_Giesen 0:51a6e38a4d4a 30 DigitalOut led_R(LED_RED);
Jitse_Giesen 0:51a6e38a4d4a 31 DigitalOut led_G(LED_GREEN);
Jitse_Giesen 0:51a6e38a4d4a 32 DigitalOut led_B(LED_BLUE);
Jitse_Giesen 0:51a6e38a4d4a 33
Jitse_Giesen 0:51a6e38a4d4a 34 //Setting Tickers for sampling EMG and determing if the threshold is met
Jitse_Giesen 0:51a6e38a4d4a 35 Ticker sample_timer;
Jitse_Giesen 0:51a6e38a4d4a 36 Ticker threshold_timer;
Jitse_Giesen 0:51a6e38a4d4a 37
Jitse_Giesen 0:51a6e38a4d4a 38 InterruptIn button(SW2); // Wordt uiteindelijk vervangen door EMG
Jitse_Giesen 0:51a6e38a4d4a 39
Jitse_Giesen 0:51a6e38a4d4a 40 Timer t;
Jitse_Giesen 0:51a6e38a4d4a 41 double speedfactor1; // = 0.01; snelheid in, zonder potmeter gebruik <- waarom is dit zo?
Jitse_Giesen 0:51a6e38a4d4a 42 double speedfactor2;
Jitse_Giesen 0:51a6e38a4d4a 43 // Getting the counts from the Encoder
Jitse_Giesen 0:51a6e38a4d4a 44 int counts1 = Encoder1.getPulses();
Jitse_Giesen 0:51a6e38a4d4a 45 int counts2 = Encoder2.getPulses();
Jitse_Giesen 0:51a6e38a4d4a 46
Jitse_Giesen 0:51a6e38a4d4a 47 // Defining variables delta (the difference between position and desired position) <- Is dit zo?
Jitse_Giesen 0:51a6e38a4d4a 48 int delta1;
Jitse_Giesen 0:51a6e38a4d4a 49 int delta2;
Jitse_Giesen 0:51a6e38a4d4a 50
Jitse_Giesen 0:51a6e38a4d4a 51 /* Defining all the different BiQuad filters, which contain a Notch filter,
Jitse_Giesen 0:51a6e38a4d4a 52 High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
Jitse_Giesen 0:51a6e38a4d4a 53 between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz
Jitse_Giesen 0:51a6e38a4d4a 54 and the Low-pass filter cancels out all frequencies below 4 Hz */
Jitse_Giesen 0:51a6e38a4d4a 55
Jitse_Giesen 0:51a6e38a4d4a 56 /* Defining all the normalized values of b and a in the Notch filter for the
Jitse_Giesen 0:51a6e38a4d4a 57 creation of the Notch BiQuad */
Jitse_Giesen 0:51a6e38a4d4a 58
Jitse_Giesen 0:51a6e38a4d4a 59 // Initial input value for X
Jitse_Giesen 0:51a6e38a4d4a 60 int Xin=0;
Jitse_Giesen 0:51a6e38a4d4a 61 int Xin_new;
Jitse_Giesen 0:51a6e38a4d4a 62 double huidigetijdX;
Jitse_Giesen 0:51a6e38a4d4a 63
Jitse_Giesen 0:51a6e38a4d4a 64 // Feedback system for counting values of X
Jitse_Giesen 0:51a6e38a4d4a 65 void ledtX(){
Jitse_Giesen 0:51a6e38a4d4a 66 t.reset();
Jitse_Giesen 0:51a6e38a4d4a 67 Xin++;
Jitse_Giesen 0:51a6e38a4d4a 68 pc.printf("Xin is %i\n",Xin);
Jitse_Giesen 0:51a6e38a4d4a 69 led_G=0;
Jitse_Giesen 0:51a6e38a4d4a 70 led_R=1;
Jitse_Giesen 0:51a6e38a4d4a 71 wait(0.2);
Jitse_Giesen 0:51a6e38a4d4a 72 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 73 led_R=0;
Jitse_Giesen 0:51a6e38a4d4a 74 wait(0.5);
Jitse_Giesen 0:51a6e38a4d4a 75 }
Jitse_Giesen 0:51a6e38a4d4a 76
Jitse_Giesen 0:51a6e38a4d4a 77 // Couting system for values of X
Jitse_Giesen 0:51a6e38a4d4a 78 int tellerX(){
Jitse_Giesen 0:51a6e38a4d4a 79 if (Move_done == true) {
Jitse_Giesen 0:51a6e38a4d4a 80 t.reset();
Jitse_Giesen 0:51a6e38a4d4a 81 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 82 led_B=1;
Jitse_Giesen 0:51a6e38a4d4a 83 led_R=0;
Jitse_Giesen 0:51a6e38a4d4a 84 while(true){
Jitse_Giesen 0:51a6e38a4d4a 85 button.fall(ledtX);
Jitse_Giesen 0:51a6e38a4d4a 86 t.start();
Jitse_Giesen 0:51a6e38a4d4a 87 huidigetijdX=t.read();
Jitse_Giesen 0:51a6e38a4d4a 88 if (huidigetijdX>2){
Jitse_Giesen 0:51a6e38a4d4a 89 led_R=1; //Go to the next program (counting values for Y)
Jitse_Giesen 0:51a6e38a4d4a 90 Xin_new = Xin;
Jitse_Giesen 0:51a6e38a4d4a 91 Xin = 0;
Jitse_Giesen 0:51a6e38a4d4a 92
Jitse_Giesen 0:51a6e38a4d4a 93 return Xin_new;
Jitse_Giesen 0:51a6e38a4d4a 94 }
Jitse_Giesen 0:51a6e38a4d4a 95
Jitse_Giesen 0:51a6e38a4d4a 96 }
Jitse_Giesen 0:51a6e38a4d4a 97
Jitse_Giesen 0:51a6e38a4d4a 98 }
Jitse_Giesen 0:51a6e38a4d4a 99 return 0;
Jitse_Giesen 0:51a6e38a4d4a 100 }
Jitse_Giesen 0:51a6e38a4d4a 101
Jitse_Giesen 0:51a6e38a4d4a 102 // Initial values needed for Y (see comments at X function)
Jitse_Giesen 0:51a6e38a4d4a 103 int Yin=0;
Jitse_Giesen 0:51a6e38a4d4a 104 int Yin_new;
Jitse_Giesen 0:51a6e38a4d4a 105 double huidigetijdY;
Jitse_Giesen 0:51a6e38a4d4a 106
Jitse_Giesen 0:51a6e38a4d4a 107 //Feedback system for couting values of Y
Jitse_Giesen 0:51a6e38a4d4a 108 void ledtY(){
Jitse_Giesen 0:51a6e38a4d4a 109 t.reset();
Jitse_Giesen 0:51a6e38a4d4a 110 Yin++;
Jitse_Giesen 0:51a6e38a4d4a 111 pc.printf("Yin is %i\n",Yin);
Jitse_Giesen 0:51a6e38a4d4a 112 led_G=0;
Jitse_Giesen 0:51a6e38a4d4a 113 led_B=1;
Jitse_Giesen 0:51a6e38a4d4a 114 wait(0.2);
Jitse_Giesen 0:51a6e38a4d4a 115 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 116 led_B=0;
Jitse_Giesen 0:51a6e38a4d4a 117 wait(0.5);
Jitse_Giesen 0:51a6e38a4d4a 118 }
Jitse_Giesen 0:51a6e38a4d4a 119
Jitse_Giesen 0:51a6e38a4d4a 120 // Couting system for values of Y
Jitse_Giesen 0:51a6e38a4d4a 121 int tellerY(){
Jitse_Giesen 0:51a6e38a4d4a 122 if (Move_done == true) {
Jitse_Giesen 0:51a6e38a4d4a 123 t.reset();
Jitse_Giesen 0:51a6e38a4d4a 124 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 125 led_B=0;
Jitse_Giesen 0:51a6e38a4d4a 126 led_R=1;
Jitse_Giesen 0:51a6e38a4d4a 127 while(true){
Jitse_Giesen 0:51a6e38a4d4a 128 button.fall(ledtY);
Jitse_Giesen 0:51a6e38a4d4a 129 t.start();
Jitse_Giesen 0:51a6e38a4d4a 130 huidigetijdY=t.read();
Jitse_Giesen 0:51a6e38a4d4a 131 if (huidigetijdY>2){
Jitse_Giesen 0:51a6e38a4d4a 132 led_B=1;
Jitse_Giesen 0:51a6e38a4d4a 133 Yin_new = Yin;
Jitse_Giesen 0:51a6e38a4d4a 134 Yin = 0;
Jitse_Giesen 0:51a6e38a4d4a 135
Jitse_Giesen 0:51a6e38a4d4a 136 Move_done = false;
Jitse_Giesen 0:51a6e38a4d4a 137 return Yin_new;
Jitse_Giesen 0:51a6e38a4d4a 138
Jitse_Giesen 0:51a6e38a4d4a 139 }
Jitse_Giesen 0:51a6e38a4d4a 140 }
Jitse_Giesen 0:51a6e38a4d4a 141 }
Jitse_Giesen 0:51a6e38a4d4a 142 return 0; // ga door naar het volgende programma
Jitse_Giesen 0:51a6e38a4d4a 143 }
Jitse_Giesen 0:51a6e38a4d4a 144
Jitse_Giesen 0:51a6e38a4d4a 145
Jitse_Giesen 0:51a6e38a4d4a 146
Jitse_Giesen 0:51a6e38a4d4a 147
Jitse_Giesen 0:51a6e38a4d4a 148 /*---------------------------------------------------------------------------------
Jitse_Giesen 0:51a6e38a4d4a 149 // Initial input value for X
Jitse_Giesen 0:51a6e38a4d4a 150 int Xin=0; //<- Hoe zit het met deze als we de robot daadwerkelijk gebruiken
Jitse_Giesen 0:51a6e38a4d4a 151 double huidigetijdX;
Jitse_Giesen 0:51a6e38a4d4a 152
Jitse_Giesen 0:51a6e38a4d4a 153 // Feedback system for counting values of X
Jitse_Giesen 0:51a6e38a4d4a 154 void ledtX(){
Jitse_Giesen 0:51a6e38a4d4a 155 t.reset();
Jitse_Giesen 0:51a6e38a4d4a 156 Xin++;
Jitse_Giesen 0:51a6e38a4d4a 157 pc.printf("Xin is %i\n",Xin);
Jitse_Giesen 0:51a6e38a4d4a 158 led_G=0;
Jitse_Giesen 0:51a6e38a4d4a 159 led_R=1;
Jitse_Giesen 0:51a6e38a4d4a 160 wait(0.2);
Jitse_Giesen 0:51a6e38a4d4a 161 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 162 led_R=0;
Jitse_Giesen 0:51a6e38a4d4a 163 wait(0.5);
Jitse_Giesen 0:51a6e38a4d4a 164 }
Jitse_Giesen 0:51a6e38a4d4a 165
Jitse_Giesen 0:51a6e38a4d4a 166
Jitse_Giesen 0:51a6e38a4d4a 167 // Couting system for values of X
Jitse_Giesen 0:51a6e38a4d4a 168 int tellerX(){
Jitse_Giesen 0:51a6e38a4d4a 169 t.reset();
Jitse_Giesen 0:51a6e38a4d4a 170 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 171 led_B=1;
Jitse_Giesen 0:51a6e38a4d4a 172 led_R=0;
Jitse_Giesen 0:51a6e38a4d4a 173 while(true){
Jitse_Giesen 0:51a6e38a4d4a 174 button.fall(ledtX); //This has to be replaced by EMG
Jitse_Giesen 0:51a6e38a4d4a 175 t.start();
Jitse_Giesen 0:51a6e38a4d4a 176 huidigetijdX=t.read();
Jitse_Giesen 0:51a6e38a4d4a 177 if (huidigetijdX>2){
Jitse_Giesen 0:51a6e38a4d4a 178 led_R=1; //Go to the next program (couting values for Y)
Jitse_Giesen 0:51a6e38a4d4a 179 return 0;
Jitse_Giesen 0:51a6e38a4d4a 180 }
Jitse_Giesen 0:51a6e38a4d4a 181 }
Jitse_Giesen 0:51a6e38a4d4a 182 }
Jitse_Giesen 0:51a6e38a4d4a 183
Jitse_Giesen 0:51a6e38a4d4a 184 // Initial values needed for Y (see comments at X function)
Jitse_Giesen 0:51a6e38a4d4a 185 int Yin=0;
Jitse_Giesen 0:51a6e38a4d4a 186 double huidigetijdY;
Jitse_Giesen 0:51a6e38a4d4a 187
Jitse_Giesen 0:51a6e38a4d4a 188 //Feedback system for couting values of Y
Jitse_Giesen 0:51a6e38a4d4a 189 void ledtY(){
Jitse_Giesen 0:51a6e38a4d4a 190 t.reset();
Jitse_Giesen 0:51a6e38a4d4a 191 Yin++;
Jitse_Giesen 0:51a6e38a4d4a 192 pc.printf("Yin is %i\n",Yin);
Jitse_Giesen 0:51a6e38a4d4a 193 led_G=0;
Jitse_Giesen 0:51a6e38a4d4a 194 led_B=1;
Jitse_Giesen 0:51a6e38a4d4a 195 wait(0.2);
Jitse_Giesen 0:51a6e38a4d4a 196 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 197 led_B=0;
Jitse_Giesen 0:51a6e38a4d4a 198 wait(0.5);
Jitse_Giesen 0:51a6e38a4d4a 199 }
Jitse_Giesen 0:51a6e38a4d4a 200
Jitse_Giesen 0:51a6e38a4d4a 201 // Couting system for values of Y
Jitse_Giesen 0:51a6e38a4d4a 202 int tellerY(){
Jitse_Giesen 0:51a6e38a4d4a 203 t.reset();
Jitse_Giesen 0:51a6e38a4d4a 204 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 205 led_B=0;
Jitse_Giesen 0:51a6e38a4d4a 206 led_R=1;
Jitse_Giesen 0:51a6e38a4d4a 207 while(true){
Jitse_Giesen 0:51a6e38a4d4a 208 button.fall(ledtY); //See comments at X
Jitse_Giesen 0:51a6e38a4d4a 209
Jitse_Giesen 0:51a6e38a4d4a 210 t.start();
Jitse_Giesen 0:51a6e38a4d4a 211 huidigetijdY=t.read();
Jitse_Giesen 0:51a6e38a4d4a 212 if (huidigetijdY>2){
Jitse_Giesen 0:51a6e38a4d4a 213 led_B=1;
Jitse_Giesen 0:51a6e38a4d4a 214
Jitse_Giesen 0:51a6e38a4d4a 215 //button.fall(0); // Wat is deze?
Jitse_Giesen 0:51a6e38a4d4a 216 return 0; // ga door naar het volgende programma
Jitse_Giesen 0:51a6e38a4d4a 217 }
Jitse_Giesen 0:51a6e38a4d4a 218 }
Jitse_Giesen 0:51a6e38a4d4a 219 }
Jitse_Giesen 0:51a6e38a4d4a 220 */
Jitse_Giesen 0:51a6e38a4d4a 221
Jitse_Giesen 0:51a6e38a4d4a 222 // Declaring all variables needed for calculating rope lengths,
Jitse_Giesen 0:51a6e38a4d4a 223 double Pox = 0;
Jitse_Giesen 0:51a6e38a4d4a 224 double Poy = 0;
Jitse_Giesen 0:51a6e38a4d4a 225 double Pbx = 0;
Jitse_Giesen 0:51a6e38a4d4a 226 double Pby = 887;
Jitse_Giesen 0:51a6e38a4d4a 227 double Prx = 768;
Jitse_Giesen 0:51a6e38a4d4a 228 double Pry = 443;
Jitse_Giesen 0:51a6e38a4d4a 229 double Pex=121;
Jitse_Giesen 0:51a6e38a4d4a 230 double Pey=308;
Jitse_Giesen 0:51a6e38a4d4a 231 double diamtrklosje=20;
Jitse_Giesen 0:51a6e38a4d4a 232 double pi=3.14159265359;
Jitse_Giesen 0:51a6e38a4d4a 233 double omtrekklosje=diamtrklosje*pi;
Jitse_Giesen 0:51a6e38a4d4a 234 double Lou;
Jitse_Giesen 0:51a6e38a4d4a 235 double Lbu;
Jitse_Giesen 0:51a6e38a4d4a 236 double Lru;
Jitse_Giesen 0:51a6e38a4d4a 237 double dLod;
Jitse_Giesen 0:51a6e38a4d4a 238 double dLbd;
Jitse_Giesen 0:51a6e38a4d4a 239 double dLrd;
Jitse_Giesen 0:51a6e38a4d4a 240
Jitse_Giesen 0:51a6e38a4d4a 241 // Declaring variables needed for calculating motor counts
Jitse_Giesen 0:51a6e38a4d4a 242 double roto;
Jitse_Giesen 0:51a6e38a4d4a 243 double rotb;
Jitse_Giesen 0:51a6e38a4d4a 244 double rotr;
Jitse_Giesen 0:51a6e38a4d4a 245 double rotzo;
Jitse_Giesen 0:51a6e38a4d4a 246 double rotzb;
Jitse_Giesen 0:51a6e38a4d4a 247 double rotzr;
Jitse_Giesen 0:51a6e38a4d4a 248 double counto;
Jitse_Giesen 0:51a6e38a4d4a 249 double countb;
Jitse_Giesen 0:51a6e38a4d4a 250 double countr;
Jitse_Giesen 0:51a6e38a4d4a 251 double countzo;
Jitse_Giesen 0:51a6e38a4d4a 252 double countzb;
Jitse_Giesen 0:51a6e38a4d4a 253 double countzr;
Jitse_Giesen 0:51a6e38a4d4a 254
Jitse_Giesen 0:51a6e38a4d4a 255 double hcounto;
Jitse_Giesen 0:51a6e38a4d4a 256 double dcounto;
Jitse_Giesen 0:51a6e38a4d4a 257 double hcountb;
Jitse_Giesen 0:51a6e38a4d4a 258 double dcountb;
Jitse_Giesen 0:51a6e38a4d4a 259 double hcountr;
Jitse_Giesen 0:51a6e38a4d4a 260 double dcountr;
Jitse_Giesen 0:51a6e38a4d4a 261
Jitse_Giesen 0:51a6e38a4d4a 262 // Declaring variables neeeded for calculating motor movements to get to a certain point <- klopt dit?
Jitse_Giesen 0:51a6e38a4d4a 263 double Psx;
Jitse_Giesen 0:51a6e38a4d4a 264 double Psy;
Jitse_Giesen 0:51a6e38a4d4a 265 double Vex;
Jitse_Giesen 0:51a6e38a4d4a 266 double Vey;
Jitse_Giesen 0:51a6e38a4d4a 267 double Kz=0.7; // nadersnelheid instellen
Jitse_Giesen 0:51a6e38a4d4a 268 double modVe;
Jitse_Giesen 0:51a6e38a4d4a 269 double Vmax=20;
Jitse_Giesen 0:51a6e38a4d4a 270 double Pstx;
Jitse_Giesen 0:51a6e38a4d4a 271 double Psty;
Jitse_Giesen 0:51a6e38a4d4a 272 double T=0.02;//seconds
Jitse_Giesen 0:51a6e38a4d4a 273
Jitse_Giesen 0:51a6e38a4d4a 274
Jitse_Giesen 0:51a6e38a4d4a 275
Jitse_Giesen 0:51a6e38a4d4a 276 //Deel om motor(en) aan te sturen--------------------------------------------
Jitse_Giesen 0:51a6e38a4d4a 277
Jitse_Giesen 0:51a6e38a4d4a 278 /*double Accelaration(){
Jitse_Giesen 0:51a6e38a4d4a 279 if (countso < statedsverander)
Jitse_Giesen 0:51a6e38a4d4a 280 { speedfactor = 0.01;
Jitse_Giesen 0:51a6e38a4d4a 281 speedfactor = speedfactor + 0.01;
Jitse_Giesen 0:51a6e38a4d4a 282 }
Jitse_Giesen 0:51a6e38a4d4a 283 }*/
Jitse_Giesen 0:51a6e38a4d4a 284
Jitse_Giesen 0:51a6e38a4d4a 285
Jitse_Giesen 0:51a6e38a4d4a 286 void calcdelta1() {
Jitse_Giesen 0:51a6e38a4d4a 287 delta1 = (dcounto - Encoder1.getPulses());
Jitse_Giesen 0:51a6e38a4d4a 288 }
Jitse_Giesen 0:51a6e38a4d4a 289
Jitse_Giesen 0:51a6e38a4d4a 290 void calcdelta2() {
Jitse_Giesen 0:51a6e38a4d4a 291 delta2 = (dcountb - Encoder2.getPulses()); // <------- de reden dat de delta negatief is (jitse)
Jitse_Giesen 0:51a6e38a4d4a 292 }
Jitse_Giesen 0:51a6e38a4d4a 293
Jitse_Giesen 0:51a6e38a4d4a 294 double referenceVelocity1;
Jitse_Giesen 0:51a6e38a4d4a 295 double motorValue1;
Jitse_Giesen 0:51a6e38a4d4a 296
Jitse_Giesen 0:51a6e38a4d4a 297 double referenceVelocity2;
Jitse_Giesen 0:51a6e38a4d4a 298 double motorValue2;
Jitse_Giesen 0:51a6e38a4d4a 299
Jitse_Giesen 0:51a6e38a4d4a 300 Ticker controlmotor1; // één ticker van maken?
Jitse_Giesen 0:51a6e38a4d4a 301 Ticker calculatedelta1;
Jitse_Giesen 0:51a6e38a4d4a 302 Ticker printdata1; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
Jitse_Giesen 0:51a6e38a4d4a 303
Jitse_Giesen 0:51a6e38a4d4a 304 Ticker controlmotor2; // één ticker van maken?
Jitse_Giesen 0:51a6e38a4d4a 305 Ticker calculatedelta2;
Jitse_Giesen 0:51a6e38a4d4a 306 Ticker printdata2; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
Jitse_Giesen 0:51a6e38a4d4a 307
Jitse_Giesen 0:51a6e38a4d4a 308 double GetReferenceVelocity1()
Jitse_Giesen 0:51a6e38a4d4a 309 {
Jitse_Giesen 0:51a6e38a4d4a 310 // Returns reference velocity in rad/s. Positive value means clockwise rotation.
Jitse_Giesen 0:51a6e38a4d4a 311 double maxVelocity1=Vex*25+Vey*25; // max 8.4 in rad/s of course!
Jitse_Giesen 0:51a6e38a4d4a 312 referenceVelocity1 = (-1)*speedfactor1 * maxVelocity1;
Jitse_Giesen 0:51a6e38a4d4a 313
Jitse_Giesen 0:51a6e38a4d4a 314 if (Encoder1.getPulses() < (dcounto+10))
Jitse_Giesen 0:51a6e38a4d4a 315 { speedfactor1 = 0.01;
Jitse_Giesen 0:51a6e38a4d4a 316 if (Encoder1.getPulses() > (dcounto-10))
Jitse_Giesen 0:51a6e38a4d4a 317 { printf("kleiner111111111");
Jitse_Giesen 0:51a6e38a4d4a 318 speedfactor1=0;
Jitse_Giesen 0:51a6e38a4d4a 319 }
Jitse_Giesen 0:51a6e38a4d4a 320 }
Jitse_Giesen 0:51a6e38a4d4a 321 else if (Encoder1.getPulses() > (dcounto-10))
Jitse_Giesen 0:51a6e38a4d4a 322 { speedfactor1 = -0.01;
Jitse_Giesen 0:51a6e38a4d4a 323 if (Encoder1.getPulses() < (dcounto+10))
Jitse_Giesen 0:51a6e38a4d4a 324 { printf("groter");
Jitse_Giesen 0:51a6e38a4d4a 325 speedfactor1=0;
Jitse_Giesen 0:51a6e38a4d4a 326 }
Jitse_Giesen 0:51a6e38a4d4a 327 }
Jitse_Giesen 0:51a6e38a4d4a 328 else
Jitse_Giesen 0:51a6e38a4d4a 329 { speedfactor1 = 0;
Jitse_Giesen 0:51a6e38a4d4a 330 pc.printf("speedfactor nul;");
Jitse_Giesen 0:51a6e38a4d4a 331 }
Jitse_Giesen 0:51a6e38a4d4a 332
Jitse_Giesen 0:51a6e38a4d4a 333 return referenceVelocity1;
Jitse_Giesen 0:51a6e38a4d4a 334 }
Jitse_Giesen 0:51a6e38a4d4a 335
Jitse_Giesen 0:51a6e38a4d4a 336 double GetReferenceVelocity2()
Jitse_Giesen 0:51a6e38a4d4a 337 {
Jitse_Giesen 0:51a6e38a4d4a 338 // Returns reference velocity in rad/s. Positive value means clockwise rotation.
Jitse_Giesen 0:51a6e38a4d4a 339 double maxVelocity2=Vex*25+Vey*25; // max 8.4 in rad/s of course!
Jitse_Giesen 0:51a6e38a4d4a 340 referenceVelocity2 = (-1)*speedfactor2 * maxVelocity2;
Jitse_Giesen 0:51a6e38a4d4a 341
Jitse_Giesen 0:51a6e38a4d4a 342 if (Encoder2.getPulses() < (dcountb+10))
Jitse_Giesen 0:51a6e38a4d4a 343 { speedfactor2 = -0.01;
Jitse_Giesen 0:51a6e38a4d4a 344 if (Encoder2.getPulses() > (dcountb-10))
Jitse_Giesen 0:51a6e38a4d4a 345 { //printf("kleiner22222222222");
Jitse_Giesen 0:51a6e38a4d4a 346 speedfactor2=0;
Jitse_Giesen 0:51a6e38a4d4a 347 }
Jitse_Giesen 0:51a6e38a4d4a 348 }
Jitse_Giesen 0:51a6e38a4d4a 349 else if (Encoder2.getPulses() > (dcountb-10))
Jitse_Giesen 0:51a6e38a4d4a 350 { speedfactor2 = 0.01;
Jitse_Giesen 0:51a6e38a4d4a 351 if (Encoder2.getPulses() < (dcountb+10))
Jitse_Giesen 0:51a6e38a4d4a 352 { //printf("groter");
Jitse_Giesen 0:51a6e38a4d4a 353 speedfactor2=0;
Jitse_Giesen 0:51a6e38a4d4a 354 }
Jitse_Giesen 0:51a6e38a4d4a 355 }
Jitse_Giesen 0:51a6e38a4d4a 356 else
Jitse_Giesen 0:51a6e38a4d4a 357 { speedfactor2 = 0;
Jitse_Giesen 0:51a6e38a4d4a 358 //pc.printf("speedfactor nul;");
Jitse_Giesen 0:51a6e38a4d4a 359 }
Jitse_Giesen 0:51a6e38a4d4a 360
Jitse_Giesen 0:51a6e38a4d4a 361 return referenceVelocity2;
Jitse_Giesen 0:51a6e38a4d4a 362 }
Jitse_Giesen 0:51a6e38a4d4a 363
Jitse_Giesen 0:51a6e38a4d4a 364 void SetMotor1(double motorValue1)
Jitse_Giesen 0:51a6e38a4d4a 365 {
Jitse_Giesen 0:51a6e38a4d4a 366 // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes
Jitse_Giesen 0:51a6e38a4d4a 367 // motor rotating clockwise. motorValues outside range are truncated to within range
Jitse_Giesen 0:51a6e38a4d4a 368 if (motorValue1 >=0) motor1DirectionPin=1;
Jitse_Giesen 0:51a6e38a4d4a 369 else motor1DirectionPin=0;
Jitse_Giesen 0:51a6e38a4d4a 370 if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
Jitse_Giesen 0:51a6e38a4d4a 371 else motor1MagnitudePin = fabs(motorValue1);
Jitse_Giesen 0:51a6e38a4d4a 372
Jitse_Giesen 0:51a6e38a4d4a 373 }
Jitse_Giesen 0:51a6e38a4d4a 374
Jitse_Giesen 0:51a6e38a4d4a 375 void SetMotor2(double motorValue2)
Jitse_Giesen 0:51a6e38a4d4a 376 {
Jitse_Giesen 0:51a6e38a4d4a 377 // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes
Jitse_Giesen 0:51a6e38a4d4a 378 // motor rotating clockwise. motorValues outside range are truncated to within range
Jitse_Giesen 0:51a6e38a4d4a 379 if (motorValue2 >=0) motor2DirectionPin=1;
Jitse_Giesen 0:51a6e38a4d4a 380 else motor2DirectionPin=0;
Jitse_Giesen 0:51a6e38a4d4a 381 if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
Jitse_Giesen 0:51a6e38a4d4a 382 else motor2MagnitudePin = fabs(motorValue2);
Jitse_Giesen 0:51a6e38a4d4a 383
Jitse_Giesen 0:51a6e38a4d4a 384 }
Jitse_Giesen 0:51a6e38a4d4a 385
Jitse_Giesen 0:51a6e38a4d4a 386 double FeedForwardControl1(double referenceVelocity1)
Jitse_Giesen 0:51a6e38a4d4a 387 {
Jitse_Giesen 0:51a6e38a4d4a 388 // very simple linear feed-forward control
Jitse_Giesen 0:51a6e38a4d4a 389 const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
Jitse_Giesen 0:51a6e38a4d4a 390 double motorValue1 = referenceVelocity1 / MotorGain;
Jitse_Giesen 0:51a6e38a4d4a 391 return motorValue1;
Jitse_Giesen 0:51a6e38a4d4a 392 }
Jitse_Giesen 0:51a6e38a4d4a 393
Jitse_Giesen 0:51a6e38a4d4a 394 double FeedForwardControl2(double referenceVelocity2)
Jitse_Giesen 0:51a6e38a4d4a 395 {
Jitse_Giesen 0:51a6e38a4d4a 396 // very simple linear feed-forward control
Jitse_Giesen 0:51a6e38a4d4a 397 const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
Jitse_Giesen 0:51a6e38a4d4a 398 double motorValue2 = referenceVelocity2 / MotorGain;
Jitse_Giesen 0:51a6e38a4d4a 399 return motorValue2;
Jitse_Giesen 0:51a6e38a4d4a 400 }
Jitse_Giesen 0:51a6e38a4d4a 401
Jitse_Giesen 0:51a6e38a4d4a 402 void MeasureAndControl1()
Jitse_Giesen 0:51a6e38a4d4a 403 {
Jitse_Giesen 0:51a6e38a4d4a 404 // This function measures the potmeter position, extracts a reference velocity from it,
Jitse_Giesen 0:51a6e38a4d4a 405 // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
Jitse_Giesen 0:51a6e38a4d4a 406 double referenceVelocity1 = GetReferenceVelocity1();
Jitse_Giesen 0:51a6e38a4d4a 407 double motorValue1 = FeedForwardControl1(referenceVelocity1);
Jitse_Giesen 0:51a6e38a4d4a 408 SetMotor1(motorValue1);
Jitse_Giesen 0:51a6e38a4d4a 409 }
Jitse_Giesen 0:51a6e38a4d4a 410
Jitse_Giesen 0:51a6e38a4d4a 411 void MeasureAndControl2()
Jitse_Giesen 0:51a6e38a4d4a 412 {
Jitse_Giesen 0:51a6e38a4d4a 413 // This function measures the potmeter position, extracts a reference velocity from it,
Jitse_Giesen 0:51a6e38a4d4a 414 // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
Jitse_Giesen 0:51a6e38a4d4a 415 double referenceVelocity2 = GetReferenceVelocity2();
Jitse_Giesen 0:51a6e38a4d4a 416 double motorValue2 = FeedForwardControl2(referenceVelocity2);
Jitse_Giesen 0:51a6e38a4d4a 417 SetMotor2(motorValue2);
Jitse_Giesen 0:51a6e38a4d4a 418 }
Jitse_Giesen 0:51a6e38a4d4a 419
Jitse_Giesen 0:51a6e38a4d4a 420 void readdata1()
Jitse_Giesen 0:51a6e38a4d4a 421 { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
Jitse_Giesen 0:51a6e38a4d4a 422 //pc.printf("Pulses_M1 = %i \r\n",Encoder1.getPulses());
Jitse_Giesen 0:51a6e38a4d4a 423 //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
Jitse_Giesen 0:51a6e38a4d4a 424 //pc.printf("Delta_M1 = %i \r\n",delta1);
Jitse_Giesen 0:51a6e38a4d4a 425 }
Jitse_Giesen 0:51a6e38a4d4a 426
Jitse_Giesen 0:51a6e38a4d4a 427 void readdata2()
Jitse_Giesen 0:51a6e38a4d4a 428 { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
Jitse_Giesen 0:51a6e38a4d4a 429 //pc.printf("Pulses_M2 = %i \r\n",Encoder2.getPulses());
Jitse_Giesen 0:51a6e38a4d4a 430 //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
Jitse_Giesen 0:51a6e38a4d4a 431 //pc.printf("Delta_M2 = %i \r\n",delta2);
Jitse_Giesen 0:51a6e38a4d4a 432 }
Jitse_Giesen 0:51a6e38a4d4a 433
Jitse_Giesen 0:51a6e38a4d4a 434 // einde deel motor------------------------------------------------------------------------------------
Jitse_Giesen 0:51a6e38a4d4a 435
Jitse_Giesen 0:51a6e38a4d4a 436 Ticker loop;
Jitse_Giesen 0:51a6e38a4d4a 437
Jitse_Giesen 0:51a6e38a4d4a 438 /*Calculates ropelengths that are needed to get to new positions, based on the
Jitse_Giesen 0:51a6e38a4d4a 439 set coordinates and the position of the poles */
Jitse_Giesen 0:51a6e38a4d4a 440 double touwlengtes(){
Jitse_Giesen 0:51a6e38a4d4a 441 Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));
Jitse_Giesen 0:51a6e38a4d4a 442 Lbu=sqrt(pow((Pstx-Pbx),2)+pow((Psty-Pby),2));
Jitse_Giesen 0:51a6e38a4d4a 443 Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
Jitse_Giesen 0:51a6e38a4d4a 444 return 0;
Jitse_Giesen 0:51a6e38a4d4a 445 }
Jitse_Giesen 0:51a6e38a4d4a 446
Jitse_Giesen 0:51a6e38a4d4a 447 /* Calculates rotations (and associated counts) of the motor to get to the
Jitse_Giesen 0:51a6e38a4d4a 448 desired new position*/
Jitse_Giesen 0:51a6e38a4d4a 449 double turns(){
Jitse_Giesen 0:51a6e38a4d4a 450
Jitse_Giesen 0:51a6e38a4d4a 451 roto=Lou/omtrekklosje;
Jitse_Giesen 0:51a6e38a4d4a 452 rotb=Lbu/omtrekklosje;
Jitse_Giesen 0:51a6e38a4d4a 453 rotr=Lru/omtrekklosje;
Jitse_Giesen 0:51a6e38a4d4a 454 counto=roto*4200;
Jitse_Giesen 0:51a6e38a4d4a 455 dcounto=counto-hcounto;
Jitse_Giesen 0:51a6e38a4d4a 456 pc.printf("dcounto = %f \n\r",dcounto);
Jitse_Giesen 0:51a6e38a4d4a 457 countb=rotb*4200;
Jitse_Giesen 0:51a6e38a4d4a 458 dcountb=countb-hcountb;
Jitse_Giesen 0:51a6e38a4d4a 459 pc.printf("dcountb = %f \n\r",dcountb);
Jitse_Giesen 0:51a6e38a4d4a 460 countr=rotr*4200;
Jitse_Giesen 0:51a6e38a4d4a 461 dcountr=countr-hcountr;
Jitse_Giesen 0:51a6e38a4d4a 462
Jitse_Giesen 0:51a6e38a4d4a 463 return 0;
Jitse_Giesen 0:51a6e38a4d4a 464 }
Jitse_Giesen 0:51a6e38a4d4a 465
Jitse_Giesen 0:51a6e38a4d4a 466 // Waar komen Pstx en Psty vandaan en waar staan ze voor? En is dit maar voor een paal?
Jitse_Giesen 0:51a6e38a4d4a 467 double Pst(){
Jitse_Giesen 0:51a6e38a4d4a 468 Pstx=Pex+Vex*T;
Jitse_Giesen 0:51a6e38a4d4a 469 Psty=Pey+Vey*T;
Jitse_Giesen 0:51a6e38a4d4a 470 touwlengtes();
Jitse_Giesen 0:51a6e38a4d4a 471 Pex=Pstx;
Jitse_Giesen 0:51a6e38a4d4a 472 Pey=Psty;
Jitse_Giesen 0:51a6e38a4d4a 473 pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);
Jitse_Giesen 0:51a6e38a4d4a 474 //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru);
Jitse_Giesen 0:51a6e38a4d4a 475 turns();
Jitse_Giesen 0:51a6e38a4d4a 476 //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr);
Jitse_Giesen 0:51a6e38a4d4a 477 pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);
Jitse_Giesen 0:51a6e38a4d4a 478 /*float R;
Jitse_Giesen 0:51a6e38a4d4a 479 R=Vex/Vey; // met dit stukje kan je zien dat de verhouding tussen Vex en Vey constant is en de end efector dus een rechte lijn maakt
Jitse_Giesen 0:51a6e38a4d4a 480 pc.printf("\n\r R=%f",R);*/
Jitse_Giesen 0:51a6e38a4d4a 481 return 0;
Jitse_Giesen 0:51a6e38a4d4a 482 }
Jitse_Giesen 0:51a6e38a4d4a 483
Jitse_Giesen 0:51a6e38a4d4a 484 //Calculating desired end position based on the EMG input <- Waarom maar voor een paal?
Jitse_Giesen 0:51a6e38a4d4a 485 double Ps(){
Jitse_Giesen 0:51a6e38a4d4a 486 if (Move_done==true);
Jitse_Giesen 0:51a6e38a4d4a 487 Psx=(Xin_new)*30+121;
Jitse_Giesen 0:51a6e38a4d4a 488 Psy=(Yin_new)*30+308;
Jitse_Giesen 0:51a6e38a4d4a 489 pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
Jitse_Giesen 0:51a6e38a4d4a 490 hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje);
Jitse_Giesen 0:51a6e38a4d4a 491 hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje);
Jitse_Giesen 0:51a6e38a4d4a 492 hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje);
Jitse_Giesen 0:51a6e38a4d4a 493 return 0;
Jitse_Giesen 0:51a6e38a4d4a 494 }
Jitse_Giesen 0:51a6e38a4d4a 495
Jitse_Giesen 0:51a6e38a4d4a 496 // Rekent dit de snelheid uit waarmee de motoren bewegen?
Jitse_Giesen 0:51a6e38a4d4a 497 void Ve(){
Jitse_Giesen 0:51a6e38a4d4a 498 Vex=Kz*(Psx-Pex);
Jitse_Giesen 0:51a6e38a4d4a 499 Vey=Kz*(Psy-Pey);
Jitse_Giesen 0:51a6e38a4d4a 500 modVe=sqrt(pow(Vex,2)+pow(Vey,2));
Jitse_Giesen 0:51a6e38a4d4a 501 if(modVe>Vmax){
Jitse_Giesen 0:51a6e38a4d4a 502 Vex=(Vex/modVe)*Vmax;
Jitse_Giesen 0:51a6e38a4d4a 503 Vey=(Vey/modVe)*Vmax;
Jitse_Giesen 0:51a6e38a4d4a 504 }
Jitse_Giesen 0:51a6e38a4d4a 505 Pst();
Jitse_Giesen 0:51a6e38a4d4a 506 //pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
Jitse_Giesen 0:51a6e38a4d4a 507 if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
Jitse_Giesen 0:51a6e38a4d4a 508 Move_done=true;
Jitse_Giesen 0:51a6e38a4d4a 509 loop.detach();
Jitse_Giesen 0:51a6e38a4d4a 510 }
Jitse_Giesen 0:51a6e38a4d4a 511 }
Jitse_Giesen 0:51a6e38a4d4a 512
Jitse_Giesen 0:51a6e38a4d4a 513 // Calculating the desired position, so that the motors can go here
Jitse_Giesen 0:51a6e38a4d4a 514 int calculator(){
Jitse_Giesen 0:51a6e38a4d4a 515 Ps();
Jitse_Giesen 0:51a6e38a4d4a 516 loop.attach(&Ve,0.02);
Jitse_Giesen 0:51a6e38a4d4a 517 return 0;
Jitse_Giesen 0:51a6e38a4d4a 518 }
Jitse_Giesen 0:51a6e38a4d4a 519
Jitse_Giesen 0:51a6e38a4d4a 520 // Function which makes it possible to lower the end-effector to pick up a piece
Jitse_Giesen 0:51a6e38a4d4a 521 void zakker(){
Jitse_Giesen 0:51a6e38a4d4a 522 while(1){
Jitse_Giesen 0:51a6e38a4d4a 523 wait(1);
Jitse_Giesen 0:51a6e38a4d4a 524 if(Move_done==true){ //misschien moet je hier als voorwaarden een delta is 1 zetten // hierdoor wacht dit programma totdat de beweging klaar is
Jitse_Giesen 0:51a6e38a4d4a 525 dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken
Jitse_Giesen 0:51a6e38a4d4a 526 dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
Jitse_Giesen 0:51a6e38a4d4a 527 dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
Jitse_Giesen 0:51a6e38a4d4a 528 rotzo=dLod/omtrekklosje;
Jitse_Giesen 0:51a6e38a4d4a 529 rotzb=dLbd/omtrekklosje;
Jitse_Giesen 0:51a6e38a4d4a 530 rotzr=dLrd/omtrekklosje;
Jitse_Giesen 0:51a6e38a4d4a 531 countzo=rotzo*4200;
Jitse_Giesen 0:51a6e38a4d4a 532 countzb=rotzb*4200;
Jitse_Giesen 0:51a6e38a4d4a 533 countzr=rotzr*4200;
Jitse_Giesen 0:51a6e38a4d4a 534
Jitse_Giesen 0:51a6e38a4d4a 535 //pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat
Jitse_Giesen 0:51a6e38a4d4a 536 }
Jitse_Giesen 0:51a6e38a4d4a 537 }
Jitse_Giesen 0:51a6e38a4d4a 538 }
Jitse_Giesen 0:51a6e38a4d4a 539
Jitse_Giesen 0:51a6e38a4d4a 540 int main()
Jitse_Giesen 0:51a6e38a4d4a 541 {
Jitse_Giesen 0:51a6e38a4d4a 542 pc.baud(115200);
Jitse_Giesen 0:51a6e38a4d4a 543 //getbqChain();
Jitse_Giesen 0:51a6e38a4d4a 544 while(true){
Jitse_Giesen 0:51a6e38a4d4a 545 // sample_timer.attach(&EMG_sample, 0.002);
Jitse_Giesen 0:51a6e38a4d4a 546 //wait(2.5f);
Jitse_Giesen 0:51a6e38a4d4a 547
Jitse_Giesen 0:51a6e38a4d4a 548 tellerX();
Jitse_Giesen 0:51a6e38a4d4a 549 tellerY();
Jitse_Giesen 0:51a6e38a4d4a 550 calculator();
Jitse_Giesen 0:51a6e38a4d4a 551 controlmotor1.attach(&MeasureAndControl1, 0.01);
Jitse_Giesen 0:51a6e38a4d4a 552 calculatedelta1.attach(&calcdelta1, 0.01);
Jitse_Giesen 0:51a6e38a4d4a 553 printdata1.attach(&readdata1, 0.5);
Jitse_Giesen 0:51a6e38a4d4a 554 // controlmotor2.attach(&MeasureAndControl2, 0.01);
Jitse_Giesen 0:51a6e38a4d4a 555 //calculatedelta2.attach(&calcdelta2, 0.01);
Jitse_Giesen 0:51a6e38a4d4a 556 //printdata2.attach(&readdata2, 0.5);
Jitse_Giesen 0:51a6e38a4d4a 557 //zakker();
Jitse_Giesen 0:51a6e38a4d4a 558 wait(5.0f);
Jitse_Giesen 0:51a6e38a4d4a 559 }
Jitse_Giesen 0:51a6e38a4d4a 560 return 0;
Jitse_Giesen 0:51a6e38a4d4a 561 }