Timo de Vries / Mbed 2 deprecated Wearealltogheternietgoed

niet goed

Dependencies:   HIDScope MODSERIAL PID QEI biquadFilter mbed

Fork of Wearealltogheter by Timo de Vries

main.cpp@24:bdd74b91abbb, 2016-10-27 (annotated)

Committer:
Frostworks
Date:
Thu Oct 27 08:12:25 2016 +0000
Revision:
24:bdd74b91abbb
Parent:
23:fdde3e4b9e69 
niet goed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
Frostworks 21:2b55d53e11f6 3 #include "MODSERIAL.h"
Frostworks 23:fdde3e4b9e69 4 #include "QEI.h"
Frostworks 24:bdd74b91abbb 5 #include "PID.h"
Frostworks 24:bdd74b91abbb 6 #include "BiQuad.h"
Frostworks 23:fdde3e4b9e69 7
Frostworks 23:fdde3e4b9e69 8 DigitalOut led_g(LED_GREEN);
Frostworks 23:fdde3e4b9e69 9 DigitalOut led_b(LED_BLUE);
Frostworks 23:fdde3e4b9e69 10 DigitalOut led_r(LED_RED);
Frostworks 24:bdd74b91abbb 11 /*
Frostworks 24:bdd74b91abbb 12 #define RATEM2 0.001f // transmission during translation
Frostworks 24:bdd74b91abbb 13 #define RATEM3 1 // transmission during rotation
Frostworks 24:bdd74b91abbb 14 #define Kc 0.30
Frostworks 24:bdd74b91abbb 15 #define Ti 0.00
Frostworks 24:bdd74b91abbb 16 #define Td 0.00
Frostworks 24:bdd74b91abbb 17
Frostworks 24:bdd74b91abbb 18 PID M3_Rotation_Controller(KC, Ti, Td, RATEM3);
Frostworks 24:bdd74b91abbb 19 PID M2_Translation_Controller(KC, Ti, Td, RATEM2);
Frostworks 24:bdd74b91abbb 20 */
Frostworks 23:fdde3e4b9e69 21
Frostworks 23:fdde3e4b9e69 22 DigitalOut M1_Rotate(D2); // voltage only base rotation
Frostworks 23:fdde3e4b9e69 23 PwmOut M1_Speed(D3); // voltage only base rotation
Frostworks 23:fdde3e4b9e69 24
Frostworks 23:fdde3e4b9e69 25 MODSERIAL pc(USBTX, USBRX);
Frostworks 23:fdde3e4b9e69 26
Frostworks 23:fdde3e4b9e69 27 //QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING)
Frostworks 23:fdde3e4b9e69 28 QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING);
Frostworks 23:fdde3e4b9e69 29 QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING);
Frostworks 23:fdde3e4b9e69 30
Frostworks 23:fdde3e4b9e69 31 DigitalOut M2_Rotate(D4); // encoder side pot 2 translation
Frostworks 23:fdde3e4b9e69 32 PwmOut M2_Speed(D5); // encoder side pot 2 translation
Frostworks 23:fdde3e4b9e69 33
Frostworks 23:fdde3e4b9e69 34 DigitalOut M3_Rotate(D7); // encoder side pot 1 spatel rotation
Frostworks 23:fdde3e4b9e69 35 PwmOut M3_Speed(D6); // encoder side pot 1 spatel rotation
Frostworks 23:fdde3e4b9e69 36
Frostworks 23:fdde3e4b9e69 37 DigitalIn links(SW3);
Frostworks 23:fdde3e4b9e69 38 DigitalIn rechts(SW2);
Frostworks 23:fdde3e4b9e69 39
Frostworks 23:fdde3e4b9e69 40 AnalogIn pot1(A4); // pot 1 motor 1
Frostworks 23:fdde3e4b9e69 41 AnalogIn pot2(A3); // pot 2 motor 3
vsluiter 0:32bb76391d89 42
vsluiter 4:8b298dfada81 43 //Define objects
tomlankhorst 19:2bf824669684 44 AnalogIn emg0( A0 );
tomlankhorst 19:2bf824669684 45 AnalogIn emg1( A1 );
Frostworks 23:fdde3e4b9e69 46 DigitalIn buttonCalibrate(D9);
Frostworks 23:fdde3e4b9e69 47
Frostworks 23:fdde3e4b9e69 48 bool draairechts;
Frostworks 23:fdde3e4b9e69 49 bool draailinks;
Frostworks 23:fdde3e4b9e69 50 bool turn = 0;
Frostworks 23:fdde3e4b9e69 51 float waiter = 0.1;
Frostworks 24:bdd74b91abbb 52 float afstand = -200;
Frostworks 23:fdde3e4b9e69 53 float translation = 0;
Frostworks 23:fdde3e4b9e69 54 float degrees3 = 0;
Frostworks 23:fdde3e4b9e69 55
Frostworks 23:fdde3e4b9e69 56 float Puls_degree = (8400/360);
Frostworks 23:fdde3e4b9e69 57 float wheel1 = 16;
Frostworks 23:fdde3e4b9e69 58 float wheel2 = 31;
Frostworks 23:fdde3e4b9e69 59 float wheel3 = 41;
Frostworks 23:fdde3e4b9e69 60 float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
Frostworks 23:fdde3e4b9e69 61 float pi = 3.14159265359;
Frostworks 21:2b55d53e11f6 62
Frostworks 21:2b55d53e11f6 63 volatile float x;
Frostworks 21:2b55d53e11f6 64 volatile float x_prev =0;
Frostworks 21:2b55d53e11f6 65 volatile float b; // filtered 'output' of ReadAnalogInAndFilter
Frostworks 21:2b55d53e11f6 66
Frostworks 22:ad85b8acf8b5 67 bool calibrate = false;
Frostworks 22:ad85b8acf8b5 68 double threshold_Left = 0;
Frostworks 22:ad85b8acf8b5 69 double threshold_Right= 0;
tomlankhorst 14:f83354387756 70 Ticker sample_timer;
Frostworks 23:fdde3e4b9e69 71 Ticker sample_timer2;
tomlankhorst 19:2bf824669684 72 HIDScope scope( 2 );
tomlankhorst 18:21d8e7a81cf5 73 DigitalOut led(LED1);
Frostworks 21:2b55d53e11f6 74 const double a1 = -1.6475;
Frostworks 21:2b55d53e11f6 75 const double a2 = 0.7009;
Frostworks 21:2b55d53e11f6 76 const double b0 = 0.8371;
Frostworks 21:2b55d53e11f6 77 const double b1 = -1.6742;
Frostworks 21:2b55d53e11f6 78 const double b2 = 0.8371;
Frostworks 21:2b55d53e11f6 79 const double c1 = -1.9645;
Frostworks 21:2b55d53e11f6 80 const double c2 = 0.9651;
Frostworks 21:2b55d53e11f6 81 const double d0 = 0.0001551;
Frostworks 21:2b55d53e11f6 82 const double d1 = 0.0003103;
Frostworks 21:2b55d53e11f6 83 const double d2 = 0.0001551;
Frostworks 21:2b55d53e11f6 84 double v1_high = 0;
Frostworks 21:2b55d53e11f6 85 double v2_high = 0;
Frostworks 21:2b55d53e11f6 86 double v1_low = 0;
Frostworks 21:2b55d53e11f6 87 double v2_low = 0;
Frostworks 22:ad85b8acf8b5 88 double highpassFilterLeft = 0;
Frostworks 22:ad85b8acf8b5 89 double lowpassFilterLeft = 0;
Frostworks 22:ad85b8acf8b5 90 double highpassFilterRight = 0;
Frostworks 22:ad85b8acf8b5 91 double lowpassFilterRight = 0;
vsluiter 2:e314bb3b2d99 92
Frostworks 24:bdd74b91abbb 93 /*/ copied from slides
Frostworks 24:bdd74b91abbb 94 const double Ts = 0.01;
Frostworks 24:bdd74b91abbb 95 const double Kp = 1.0, Ki = 0.5, Kd = 0.1;
Frostworks 24:bdd74b91abbb 96 const double N = 25; // N = 1/Tf
Frostworks 24:bdd74b91abbb 97
Frostworks 24:bdd74b91abbb 98 BiQuad pidf;
Frostworks 24:bdd74b91abbb 99 // AnalogIn reference( A0 );
Frostworks 24:bdd74b91abbb 100 Ticker controllerTicker;
Frostworks 24:bdd74b91abbb 101
Frostworks 24:bdd74b91abbb 102 void controller()
Frostworks 24:bdd74b91abbb 103 {
Frostworks 24:bdd74b91abbb 104 double ctrlOutputTranslation = pidf.step(GetTranslationM2());
Frostworks 24:bdd74b91abbb 105 double ctrlOutputRotation = pidf.step( GetRotationM3());
Frostworks 24:bdd74b91abbb 106 //double ctrlOutput = pidf.step( reference.read() );
Frostworks 24:bdd74b91abbb 107 }
Frostworks 24:bdd74b91abbb 108 end copy*/
Frostworks 24:bdd74b91abbb 109
Frostworks 21:2b55d53e11f6 110 double biquad1(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
Frostworks 21:2b55d53e11f6 111 const double b1, const double b2)
Frostworks 21:2b55d53e11f6 112 {
Frostworks 21:2b55d53e11f6 113 double v = u - a1*v1 - a2*v2;
Frostworks 21:2b55d53e11f6 114 double y = b0*v + b1*v1 + b2*v2;
Frostworks 21:2b55d53e11f6 115 v2 = v1;
Frostworks 21:2b55d53e11f6 116 v1 = v;
Frostworks 21:2b55d53e11f6 117 return y;
Frostworks 21:2b55d53e11f6 118 }
Frostworks 24:bdd74b91abbb 119
tomlankhorst 14:f83354387756 120 /** Sample function
tomlankhorst 14:f83354387756 121 * this function samples the emg and sends it to HIDScope
tomlankhorst 14:f83354387756 122 **/
Frostworks 21:2b55d53e11f6 123
Frostworks 22:ad85b8acf8b5 124 void filterSampleLeft()
Frostworks 21:2b55d53e11f6 125 {
Frostworks 22:ad85b8acf8b5 126 highpassFilterLeft = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
Frostworks 22:ad85b8acf8b5 127 lowpassFilterLeft = biquad1(highpassFilterLeft, v1_low, v2_low, c1, c2, d0, d1, d2);
Frostworks 22:ad85b8acf8b5 128 scope.set(0, lowpassFilterLeft );
Frostworks 21:2b55d53e11f6 129 scope.send();
Frostworks 22:ad85b8acf8b5 130 //pc.printf("%f \n \r ", lowpassFilter);
Frostworks 21:2b55d53e11f6 131 }
Frostworks 22:ad85b8acf8b5 132 void filterSampleRight()
Frostworks 22:ad85b8acf8b5 133 {
Frostworks 22:ad85b8acf8b5 134 highpassFilterRight = fabs(biquad1(emg1.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
Frostworks 22:ad85b8acf8b5 135 lowpassFilterRight = biquad1(highpassFilterRight, v1_low, v2_low, c1, c2, d0, d1, d2);
Frostworks 22:ad85b8acf8b5 136 scope.set(1, lowpassFilterRight );
Frostworks 22:ad85b8acf8b5 137 scope.send();
Frostworks 22:ad85b8acf8b5 138 //pc.printf("%f \n \r ", lowpassFilter);
Frostworks 22:ad85b8acf8b5 139 }
Frostworks 24:bdd74b91abbb 140
tomlankhorst 14:f83354387756 141 void sample()
vsluiter 2:e314bb3b2d99 142 {
Frostworks 24:bdd74b91abbb 143 // Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
tomlankhorst 19:2bf824669684 144 scope.set(0, emg0.read() );
tomlankhorst 19:2bf824669684 145 scope.set(1, emg1.read() );
Frostworks 21:2b55d53e11f6 146 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
tomlankhorst 19:2bf824669684 147 * Ensure that enough channels are available (HIDScope scope( 2 ))
tomlankhorst 20:97059009a491 148 * Finally, send all channels to the PC at once */
Frostworks 21:2b55d53e11f6 149
Frostworks 21:2b55d53e11f6 150 x = emg0; // Capture data scope.set(0, x); // store data in first element of scope memory
Frostworks 21:2b55d53e11f6 151 b = (x_prev + x)/2.0; // averaging filter
Frostworks 21:2b55d53e11f6 152 x_prev = x; // Prepare for next round
Frostworks 21:2b55d53e11f6 153
vsluiter 11:ce72ec658a95 154 scope.send();
Frostworks 24:bdd74b91abbb 155 // To indicate that the function is working, the LED is toggled
tomlankhorst 18:21d8e7a81cf5 156 led = !led;
Frostworks 21:2b55d53e11f6 157 pc.printf("%f, %f \n \r ", x, b);
vsluiter 2:e314bb3b2d99 158 }
Frostworks 24:bdd74b91abbb 159
Frostworks 23:fdde3e4b9e69 160 void GetDirections()
Frostworks 23:fdde3e4b9e69 161 {
Frostworks 23:fdde3e4b9e69 162 pc.baud(115200);
Frostworks 23:fdde3e4b9e69 163 if ((rechts == 0) && (links == 0) && (turn == 0)) {
Frostworks 23:fdde3e4b9e69 164 draailinks = 0;
Frostworks 23:fdde3e4b9e69 165 draairechts = 0;
Frostworks 23:fdde3e4b9e69 166 turn = 1;
Frostworks 23:fdde3e4b9e69 167 pc.printf("begin de actie \n \r ");
Frostworks 23:fdde3e4b9e69 168 wait(waiter);
vsluiter 0:32bb76391d89 169
Frostworks 23:fdde3e4b9e69 170 } else if ((rechts == 0) && (links == 0) && (turn == 1)) {
Frostworks 23:fdde3e4b9e69 171 draailinks = 0;
Frostworks 23:fdde3e4b9e69 172 draairechts = 0;
Frostworks 23:fdde3e4b9e69 173 turn = 0;
Frostworks 23:fdde3e4b9e69 174 pc.printf("breek de actie af \n \r ");
Frostworks 23:fdde3e4b9e69 175 wait(waiter);
Frostworks 23:fdde3e4b9e69 176 } else if ((rechts == 1) && (links == 1)&& (turn == 0)) {
Frostworks 23:fdde3e4b9e69 177
Frostworks 23:fdde3e4b9e69 178 } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
Frostworks 23:fdde3e4b9e69 179 /* if the right button is pressed and the motor isn't rotating to the left,
Frostworks 23:fdde3e4b9e69 180 then start rotating to the right etc*/
Frostworks 23:fdde3e4b9e69 181 draairechts = !draairechts;
Frostworks 23:fdde3e4b9e69 182 pc.printf("draai naar rechts \n \r ");
Frostworks 23:fdde3e4b9e69 183 wait(waiter);
Frostworks 23:fdde3e4b9e69 184 } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) {
Frostworks 23:fdde3e4b9e69 185 draailinks = 0;
Frostworks 23:fdde3e4b9e69 186 draairechts = !draairechts;
Frostworks 23:fdde3e4b9e69 187 pc.printf("draai naar rechts na links \n \r ");
Frostworks 23:fdde3e4b9e69 188 wait(waiter);
Frostworks 23:fdde3e4b9e69 189 } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) {
Frostworks 23:fdde3e4b9e69 190 draailinks = !draailinks;
Frostworks 23:fdde3e4b9e69 191 pc.printf("draai naar links \n \r ");
Frostworks 23:fdde3e4b9e69 192 wait(waiter);
Frostworks 23:fdde3e4b9e69 193 } else if ((links == 1) && (draairechts == 1) && (turn == 0)) {
Frostworks 23:fdde3e4b9e69 194 draairechts = 0;
Frostworks 23:fdde3e4b9e69 195 draailinks = !draailinks;
Frostworks 23:fdde3e4b9e69 196 pc.printf("draai naar links na rechts \n \r ");
Frostworks 23:fdde3e4b9e69 197 wait(waiter);
Frostworks 23:fdde3e4b9e69 198 }
Frostworks 23:fdde3e4b9e69 199 wait(2*waiter);
Frostworks 23:fdde3e4b9e69 200 }
Frostworks 23:fdde3e4b9e69 201
Frostworks 23:fdde3e4b9e69 202 float GetPositionM2()
Frostworks 23:fdde3e4b9e69 203 {
Frostworks 23:fdde3e4b9e69 204 float pulses2 = motor2.getPulses();
Frostworks 23:fdde3e4b9e69 205 float degrees2 = (pulses2/Puls_degree);
Frostworks 23:fdde3e4b9e69 206 float radians2 = (degrees2/360)*2*pi;
Frostworks 23:fdde3e4b9e69 207 float translation = ((radians2/overbrenging)*32.25);
Frostworks 23:fdde3e4b9e69 208
Frostworks 23:fdde3e4b9e69 209 return translation;
Frostworks 23:fdde3e4b9e69 210 }
Frostworks 23:fdde3e4b9e69 211 float GetRotationM3()
Frostworks 23:fdde3e4b9e69 212 {
Frostworks 23:fdde3e4b9e69 213 float pulses3 = motor3.getPulses();
Frostworks 23:fdde3e4b9e69 214 float degrees3 = (pulses3/Puls_degree);
Frostworks 23:fdde3e4b9e69 215 float radians3 = (degrees3/360)*2*pi;
Frostworks 23:fdde3e4b9e69 216
Frostworks 23:fdde3e4b9e69 217 return degrees3;
Frostworks 23:fdde3e4b9e69 218 }
Frostworks 23:fdde3e4b9e69 219 void GoBack()
Frostworks 23:fdde3e4b9e69 220 {
Frostworks 23:fdde3e4b9e69 221 while (GetPositionM2() > 0) {
Frostworks 23:fdde3e4b9e69 222 M3_Speed = 0;
Frostworks 23:fdde3e4b9e69 223 M2_Speed = 1;
Frostworks 23:fdde3e4b9e69 224 M2_Rotate = 0;
Frostworks 23:fdde3e4b9e69 225 pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
Frostworks 23:fdde3e4b9e69 226 led_r = 0;
Frostworks 23:fdde3e4b9e69 227 }
Frostworks 23:fdde3e4b9e69 228 M2_Speed = 0;
Frostworks 23:fdde3e4b9e69 229
Frostworks 23:fdde3e4b9e69 230
Frostworks 23:fdde3e4b9e69 231 while (GetRotationM3() > 0) {
Frostworks 23:fdde3e4b9e69 232 M3_Rotate = 1;
Frostworks 23:fdde3e4b9e69 233 M3_Speed = 0.2;
Frostworks 23:fdde3e4b9e69 234 led_r = 1;
Frostworks 23:fdde3e4b9e69 235 led_b = 0;
Frostworks 23:fdde3e4b9e69 236 pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
Frostworks 23:fdde3e4b9e69 237
Frostworks 23:fdde3e4b9e69 238 }
Frostworks 23:fdde3e4b9e69 239 M3_Speed = 0;
Frostworks 23:fdde3e4b9e69 240
Frostworks 23:fdde3e4b9e69 241 turn = 0;
Frostworks 24:bdd74b91abbb 242 }
Frostworks 23:fdde3e4b9e69 243
Frostworks 23:fdde3e4b9e69 244 void Burgerflip()
Frostworks 23:fdde3e4b9e69 245 {
Frostworks 23:fdde3e4b9e69 246 if (GetPositionM2() > afstand) {
Frostworks 23:fdde3e4b9e69 247 M3_Speed = 0.2;
Frostworks 24:bdd74b91abbb 248 M3_Rotate = 1;
Frostworks 23:fdde3e4b9e69 249 M2_Speed = 0;
Frostworks 23:fdde3e4b9e69 250 } else if (GetPositionM2() < afstand) {
Frostworks 23:fdde3e4b9e69 251 M2_Speed = 1;
Frostworks 23:fdde3e4b9e69 252 M2_Rotate = 1;
Frostworks 23:fdde3e4b9e69 253
Frostworks 23:fdde3e4b9e69 254 }
Frostworks 23:fdde3e4b9e69 255 if (GetRotationM3() > 150) {
Frostworks 23:fdde3e4b9e69 256 GoBack();
Frostworks 23:fdde3e4b9e69 257 }
Frostworks 23:fdde3e4b9e69 258 }
vsluiter 0:32bb76391d89 259 int main()
Frostworks 21:2b55d53e11f6 260 {
Frostworks 24:bdd74b91abbb 261 //Leds
Frostworks 23:fdde3e4b9e69 262 led_g = 1;
Frostworks 23:fdde3e4b9e69 263 led_b = 1;
Frostworks 23:fdde3e4b9e69 264 led_r = 1;
Frostworks 23:fdde3e4b9e69 265
Frostworks 24:bdd74b91abbb 266 /*
Frostworks 24:bdd74b91abbb 267 //PID
Frostworks 24:bdd74b91abbb 268 pidf.PIDF( Kp, Ki, Kd, N, Ts );
Frostworks 24:bdd74b91abbb 269 controllerTicker.attach( &controller, Ts );
Frostworks 24:bdd74b91abbb 270 */
tomlankhorst 14:f83354387756 271 /**Attach the 'sample' function to the timer 'sample_timer'.
tomlankhorst 19:2bf824669684 272 * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
vsluiter 4:8b298dfada81 273 */
Frostworks 22:ad85b8acf8b5 274 //sample_timer.attach(&sample, 0.001953125);
Frostworks 23:fdde3e4b9e69 275 sample_timer2.attach(&filterSampleLeft, 0.001953125); //512 Hz
Frostworks 22:ad85b8acf8b5 276 sample_timer.attach(&filterSampleRight, 0.001953125);
Frostworks 21:2b55d53e11f6 277 pc.baud(115200);
Frostworks 22:ad85b8acf8b5 278 pc.printf("please push the button to calibrate \n \r");
Frostworks 22:ad85b8acf8b5 279 while (1) {
Frostworks 22:ad85b8acf8b5 280 if (buttonCalibrate == 0) {
Frostworks 22:ad85b8acf8b5 281 calibrate = true;
Frostworks 22:ad85b8acf8b5 282 threshold_Left = lowpassFilterLeft*0.7;
Frostworks 22:ad85b8acf8b5 283 threshold_Right = lowpassFilterRight*0.7;
Frostworks 24:bdd74b91abbb 284
Frostworks 21:2b55d53e11f6 285 }
Frostworks 22:ad85b8acf8b5 286 if (calibrate == true) {
Frostworks 22:ad85b8acf8b5 287 pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right);
Frostworks 23:fdde3e4b9e69 288 GetDirections();
Frostworks 23:fdde3e4b9e69 289 if (draairechts == true) {
Frostworks 23:fdde3e4b9e69 290 M1_Speed = 0.2;
Frostworks 23:fdde3e4b9e69 291 M1_Rotate = 0;
Frostworks 23:fdde3e4b9e69 292 } else if (draailinks == true) {
Frostworks 23:fdde3e4b9e69 293 M1_Speed = 0.2;
Frostworks 23:fdde3e4b9e69 294 M1_Rotate = 1;
Frostworks 23:fdde3e4b9e69 295 } else if (turn == 1) {
Frostworks 23:fdde3e4b9e69 296 /*M2_Speed = 0.5;
Frostworks 23:fdde3e4b9e69 297 M2_Rotate = 1;
Frostworks 23:fdde3e4b9e69 298 M3_Speed = 0.5;
Frostworks 23:fdde3e4b9e69 299 M3_Rotate = 1;*/
Frostworks 23:fdde3e4b9e69 300 Burgerflip();
Frostworks 23:fdde3e4b9e69 301 } else if (turn == 0) {
Frostworks 23:fdde3e4b9e69 302 M2_Speed = 0;
Frostworks 23:fdde3e4b9e69 303 M3_Speed = 0;
Frostworks 23:fdde3e4b9e69 304 }
Frostworks 23:fdde3e4b9e69 305 if ((draailinks == false) && (draairechts == false)) {
Frostworks 23:fdde3e4b9e69 306 M1_Speed = 0;
Frostworks 23:fdde3e4b9e69 307 }
Frostworks 23:fdde3e4b9e69 308 pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
Frostworks 23:fdde3e4b9e69 309 /* pulses = 8400 */
Frostworks 22:ad85b8acf8b5 310 /*empty loop, sample() is executed periodically*/
Frostworks 22:ad85b8acf8b5 311 }
Frostworks 22:ad85b8acf8b5 312 }
vsluiter 0:32bb76391d89 313 }