niet goed
Dependencies: HIDScope MODSERIAL PID QEI biquadFilter mbed
Fork of Wearealltogheter by
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?
User | Revision | Line number | New 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 | } |