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: mbed QEI biquadFilter
test_main.cpp@22:cda3e5ad89c0, 2016-11-03 (annotated)
- Committer:
- Jankoekenpan
- Date:
- Thu Nov 03 16:48:58 2016 +0000
- Revision:
- 22:cda3e5ad89c0
- Parent:
- 21:2aed81380bc3
- Child:
- 23:dba0233303b6
add kill button
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jankoekenpan | 15:5d2bce2abd41 | 1 | #include "controller.h" |
Jankoekenpan | 15:5d2bce2abd41 | 2 | #include "BiQuad.h" |
Jankoekenpan | 1:7d218e9d2111 | 3 | |
Jankoekenpan | 1:7d218e9d2111 | 4 | // ====== Hardware stuff ====== |
Jankoekenpan | 1:7d218e9d2111 | 5 | |
Jankoekenpan | 15:5d2bce2abd41 | 6 | /* The robot controller */ |
Jankoekenpan | 15:5d2bce2abd41 | 7 | RobotController robotController; |
Jankoekenpan | 1:7d218e9d2111 | 8 | |
Jankoekenpan | 15:5d2bce2abd41 | 9 | /* The EMG inputs */ |
Jankoekenpan | 5:23c850380b86 | 10 | AnalogIn emg1(A0); |
Jankoekenpan | 5:23c850380b86 | 11 | AnalogIn emg2(A1); |
Jankoekenpan | 1:7d218e9d2111 | 12 | |
Jankoekenpan | 15:5d2bce2abd41 | 13 | /* Used in calibration */ |
Jankoekenpan | 15:5d2bce2abd41 | 14 | DigitalIn calibrating(SW2); |
Jankoekenpan | 15:5d2bce2abd41 | 15 | /* Used to start calibrating */ |
Jankoekenpan | 15:5d2bce2abd41 | 16 | InterruptIn calibrateButton(SW3); |
Jankoekenpan | 15:5d2bce2abd41 | 17 | |
Jankoekenpan | 15:5d2bce2abd41 | 18 | /* LEDs |
Jankoekenpan | 15:5d2bce2abd41 | 19 | RED FLICKERING --> Ready to calibrate (press button SW3 to start) |
Jankoekenpan | 15:5d2bce2abd41 | 20 | GREEN FlICKERING --> Calibration success |
Jankoekenpan | 15:5d2bce2abd41 | 21 | BLUE --> Busy calibrating |
Jankoekenpan | 15:5d2bce2abd41 | 22 | */ |
Jankoekenpan | 15:5d2bce2abd41 | 23 | DigitalOut led_red(LED_RED); |
Jankoekenpan | 15:5d2bce2abd41 | 24 | DigitalOut led_green(LED_GREEN); |
Jankoekenpan | 15:5d2bce2abd41 | 25 | DigitalOut led_blue(LED_BLUE); |
Jankoekenpan | 15:5d2bce2abd41 | 26 | |
Jankoekenpan | 22:cda3e5ad89c0 | 27 | /* ABORT! ABORT! */ |
Jankoekenpan | 22:cda3e5ad89c0 | 28 | InterruptIn killButton(D3); |
Jankoekenpan | 22:cda3e5ad89c0 | 29 | |
Jankoekenpan | 15:5d2bce2abd41 | 30 | /*For debuggin purposes*/ |
Jankoekenpan | 15:5d2bce2abd41 | 31 | //Serial pc(USBTX, USBRX); |
Jankoekenpan | 8:874fe459b10a | 32 | |
Jankoekenpan | 8:874fe459b10a | 33 | //====== Constants ===== |
Jankoekenpan | 1:7d218e9d2111 | 34 | |
Jankoekenpan | 11:57f0ab4d0e99 | 35 | enum RobotCommand{NOTHING, UP, DOWN, FORWARD, BACKWARD}; |
Jankoekenpan | 9:3193094ba3b2 | 36 | enum ProgramState{CALIBRATING, UPDOWN, FORBACK}; |
Jankoekenpan | 1:7d218e9d2111 | 37 | |
Jankoekenpan | 8:874fe459b10a | 38 | const float sampleFrequency = 500; |
Jankoekenpan | 8:874fe459b10a | 39 | const float sampleTime = 1.0f/sampleFrequency; |
Jankoekenpan | 8:874fe459b10a | 40 | |
Jankoekenpan | 8:874fe459b10a | 41 | |
Jankoekenpan | 1:7d218e9d2111 | 42 | //====== Program Variables ====== |
Jankoekenpan | 1:7d218e9d2111 | 43 | |
Jankoekenpan | 15:5d2bce2abd41 | 44 | volatile ProgramState progState; |
Jankoekenpan | 15:5d2bce2abd41 | 45 | volatile RobotCommand robotCommand; |
Jankoekenpan | 15:5d2bce2abd41 | 46 | |
Jankoekenpan | 15:5d2bce2abd41 | 47 | /*The 'main' ticker which samples our emg signals at the control state*/ |
Jankoekenpan | 15:5d2bce2abd41 | 48 | Ticker ticker; |
Jankoekenpan | 15:5d2bce2abd41 | 49 | /*The ticker used for calibration*/ |
Jankoekenpan | 15:5d2bce2abd41 | 50 | Ticker sampler; |
Jankoekenpan | 15:5d2bce2abd41 | 51 | |
Jankoekenpan | 15:5d2bce2abd41 | 52 | const float sample_frequency = 500.0f; //Hz |
Jankoekenpan | 15:5d2bce2abd41 | 53 | const float Ts = 1.0f / sample_frequency; |
Jankoekenpan | 15:5d2bce2abd41 | 54 | volatile int count = 0; //how many signals have passed. resets at 50. |
Jankoekenpan | 15:5d2bce2abd41 | 55 | |
Jankoekenpan | 15:5d2bce2abd41 | 56 | /*Function used to send data to the motor*/ |
Jankoekenpan | 15:5d2bce2abd41 | 57 | void (*motorFunc)(bool, bool); |
Jankoekenpan | 15:5d2bce2abd41 | 58 | |
Jankoekenpan | 15:5d2bce2abd41 | 59 | |
Jankoekenpan | 15:5d2bce2abd41 | 60 | |
Jankoekenpan | 15:5d2bce2abd41 | 61 | /* EMG BiQuadChain 1 */ |
Jankoekenpan | 15:5d2bce2abd41 | 62 | BiQuadChain bqc1; |
Jankoekenpan | 15:5d2bce2abd41 | 63 | //Notch iir filter. |
Jankoekenpan | 15:5d2bce2abd41 | 64 | //Notch: 50 +- 2 Hz |
Jankoekenpan | 15:5d2bce2abd41 | 65 | BiQuad bq1(9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 ); |
Jankoekenpan | 15:5d2bce2abd41 | 66 | |
Jankoekenpan | 15:5d2bce2abd41 | 67 | |
Jankoekenpan | 15:5d2bce2abd41 | 68 | /* EMG BiQuadChain 2 */ |
Jankoekenpan | 15:5d2bce2abd41 | 69 | BiQuadChain bqc2; |
Jankoekenpan | 15:5d2bce2abd41 | 70 | //Notch iir filter. |
Jankoekenpan | 15:5d2bce2abd41 | 71 | //Notch: 50 +- 2 Hz |
Jankoekenpan | 15:5d2bce2abd41 | 72 | BiQuad bq2( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 ); |
Jankoekenpan | 15:5d2bce2abd41 | 73 | |
Jankoekenpan | 15:5d2bce2abd41 | 74 | |
Jankoekenpan | 15:5d2bce2abd41 | 75 | // Arrays used in the calibrationi phase |
Jankoekenpan | 15:5d2bce2abd41 | 76 | // Values in these arrays contain samples that are already notched and rectified. |
Jankoekenpan | 15:5d2bce2abd41 | 77 | const int calibrateNumEmgCache = 100; |
Jankoekenpan | 15:5d2bce2abd41 | 78 | float calibrateEmgCache1[calibrateNumEmgCache]; //sorted from new to old; |
Jankoekenpan | 15:5d2bce2abd41 | 79 | float calibrateEmgCache2[calibrateNumEmgCache]; //sorted from new to old; |
Jankoekenpan | 15:5d2bce2abd41 | 80 | |
Jankoekenpan | 15:5d2bce2abd41 | 81 | // Arrays used to calculate the moving average |
Jankoekenpan | 15:5d2bce2abd41 | 82 | // Values in these arrays contain samples that are already notched and rectified. |
Jankoekenpan | 15:5d2bce2abd41 | 83 | const int numEmgCache = 50; |
Jankoekenpan | 15:5d2bce2abd41 | 84 | float emgCache1[numEmgCache]; //sorted from new to old; |
Jankoekenpan | 15:5d2bce2abd41 | 85 | float emgCache2[numEmgCache]; //sorted from new to old; |
Jankoekenpan | 15:5d2bce2abd41 | 86 | |
Jankoekenpan | 15:5d2bce2abd41 | 87 | |
Jankoekenpan | 15:5d2bce2abd41 | 88 | // Thresholds for the decisioin. by default 0.2, |
Jankoekenpan | 15:5d2bce2abd41 | 89 | // The values are changed during calibration. |
Jankoekenpan | 15:5d2bce2abd41 | 90 | volatile float threshold1 = 0.2; |
Jankoekenpan | 15:5d2bce2abd41 | 91 | volatile float threshold2 = 0.2; |
Jankoekenpan | 15:5d2bce2abd41 | 92 | |
Jankoekenpan | 15:5d2bce2abd41 | 93 | // The last 50 signals that have been dititalised. |
Jankoekenpan | 15:5d2bce2abd41 | 94 | // Only contains ones and zeros. |
Jankoekenpan | 15:5d2bce2abd41 | 95 | int decided1[numEmgCache]; |
Jankoekenpan | 15:5d2bce2abd41 | 96 | int decided2[numEmgCache]; |
Jankoekenpan | 1:7d218e9d2111 | 97 | |
Jankoekenpan | 8:874fe459b10a | 98 | |
Jankoekenpan | 1:7d218e9d2111 | 99 | //====== Functions ====== |
Jankoekenpan | 1:7d218e9d2111 | 100 | |
Jankoekenpan | 15:5d2bce2abd41 | 101 | // Helper Functions |
Jankoekenpan | 1:7d218e9d2111 | 102 | |
Jankoekenpan | 15:5d2bce2abd41 | 103 | void resetLeds() { |
Jankoekenpan | 15:5d2bce2abd41 | 104 | led_red = true; |
Jankoekenpan | 15:5d2bce2abd41 | 105 | led_green = true; |
Jankoekenpan | 15:5d2bce2abd41 | 106 | led_blue = true; |
Jankoekenpan | 15:5d2bce2abd41 | 107 | } |
Jankoekenpan | 15:5d2bce2abd41 | 108 | |
Jankoekenpan | 15:5d2bce2abd41 | 109 | // Rotates the array one position, replacing the first value with the new value |
Jankoekenpan | 15:5d2bce2abd41 | 110 | void addFirst(float newValue, float array[], int size) { |
Jankoekenpan | 15:5d2bce2abd41 | 111 | for (int i = size - 2; i >= 0; i--) { |
Jankoekenpan | 15:5d2bce2abd41 | 112 | array[i+1] = array[i]; |
Jankoekenpan | 15:5d2bce2abd41 | 113 | } |
Jankoekenpan | 15:5d2bce2abd41 | 114 | array[0] = newValue; |
Jankoekenpan | 15:5d2bce2abd41 | 115 | } |
Jankoekenpan | 15:5d2bce2abd41 | 116 | |
Jankoekenpan | 15:5d2bce2abd41 | 117 | // Rotates the array one position, replacing the first value with the new value |
Jankoekenpan | 15:5d2bce2abd41 | 118 | void addFirst(int newValue, int array[], int size) { |
Jankoekenpan | 15:5d2bce2abd41 | 119 | for (int i = size - 2; i >= 0; i--) { |
Jankoekenpan | 15:5d2bce2abd41 | 120 | array[i+1] = array[i]; |
Jankoekenpan | 15:5d2bce2abd41 | 121 | } |
Jankoekenpan | 15:5d2bce2abd41 | 122 | array[0] = newValue; |
Jankoekenpan | 15:5d2bce2abd41 | 123 | } |
Jankoekenpan | 15:5d2bce2abd41 | 124 | |
Jankoekenpan | 15:5d2bce2abd41 | 125 | |
Jankoekenpan | 15:5d2bce2abd41 | 126 | float sum(float array[], int size) { |
Jankoekenpan | 15:5d2bce2abd41 | 127 | float sum = 0; |
Jankoekenpan | 15:5d2bce2abd41 | 128 | for (int i = 0; i < size; i++) { |
Jankoekenpan | 15:5d2bce2abd41 | 129 | sum += array[i]; |
Jankoekenpan | 15:5d2bce2abd41 | 130 | } |
Jankoekenpan | 15:5d2bce2abd41 | 131 | return sum; |
Jankoekenpan | 15:5d2bce2abd41 | 132 | } |
Jankoekenpan | 15:5d2bce2abd41 | 133 | |
Jankoekenpan | 15:5d2bce2abd41 | 134 | float mean(float array[], int size) { |
Jankoekenpan | 15:5d2bce2abd41 | 135 | return sum(array, size) / size; |
Jankoekenpan | 15:5d2bce2abd41 | 136 | } |
Jankoekenpan | 15:5d2bce2abd41 | 137 | |
Jankoekenpan | 15:5d2bce2abd41 | 138 | // 'Digitize' an analog value by comparing to a threshold |
Jankoekenpan | 15:5d2bce2abd41 | 139 | int decide(float value, float threshold) { |
Jankoekenpan | 15:5d2bce2abd41 | 140 | return value < threshold ? 0 : 1; |
Jankoekenpan | 15:5d2bce2abd41 | 141 | } |
Jankoekenpan | 15:5d2bce2abd41 | 142 | |
Jankoekenpan | 15:5d2bce2abd41 | 143 | // Functions used for filtering |
Jankoekenpan | 15:5d2bce2abd41 | 144 | |
Jankoekenpan | 15:5d2bce2abd41 | 145 | //shifts the array by adding the new emg value up front. |
Jankoekenpan | 15:5d2bce2abd41 | 146 | //returns the new calculated average |
Jankoekenpan | 15:5d2bce2abd41 | 147 | float movingAverage(float newValue, float array[], int size) { |
Jankoekenpan | 15:5d2bce2abd41 | 148 | float sum = 0; |
Jankoekenpan | 15:5d2bce2abd41 | 149 | for (int i = size - 2; i >= 0; i--) { |
Jankoekenpan | 15:5d2bce2abd41 | 150 | array[i+1] = array[i]; |
Jankoekenpan | 15:5d2bce2abd41 | 151 | sum += array[i]; |
Jankoekenpan | 15:5d2bce2abd41 | 152 | } |
Jankoekenpan | 15:5d2bce2abd41 | 153 | array[0] = newValue; |
Jankoekenpan | 15:5d2bce2abd41 | 154 | sum += newValue; |
Jankoekenpan | 15:5d2bce2abd41 | 155 | return sum / size; |
Jankoekenpan | 15:5d2bce2abd41 | 156 | } |
Jankoekenpan | 15:5d2bce2abd41 | 157 | |
Jankoekenpan | 15:5d2bce2abd41 | 158 | |
Jankoekenpan | 15:5d2bce2abd41 | 159 | float rectifier(float value) { |
Jankoekenpan | 15:5d2bce2abd41 | 160 | return fabs(value - 0.5f)*2.0f; |
Jankoekenpan | 1:7d218e9d2111 | 161 | } |
Jankoekenpan | 1:7d218e9d2111 | 162 | |
Jankoekenpan | 15:5d2bce2abd41 | 163 | void sendToMotor(void (*func)(bool, bool), bool arg1, bool arg2) { |
Jankoekenpan | 15:5d2bce2abd41 | 164 | func(arg1, arg2); |
Jankoekenpan | 15:5d2bce2abd41 | 165 | } |
Jankoekenpan | 15:5d2bce2abd41 | 166 | |
Jankoekenpan | 15:5d2bce2abd41 | 167 | |
Jankoekenpan | 15:5d2bce2abd41 | 168 | // ====== Functions used for calibrations ===== |
Jankoekenpan | 15:5d2bce2abd41 | 169 | |
Jankoekenpan | 15:5d2bce2abd41 | 170 | void sample() { |
Jankoekenpan | 15:5d2bce2abd41 | 171 | float emgOne = emg1.read(); |
Jankoekenpan | 15:5d2bce2abd41 | 172 | float notch1 = bqc1.step( emgOne ); |
Jankoekenpan | 15:5d2bce2abd41 | 173 | |
Jankoekenpan | 15:5d2bce2abd41 | 174 | float emgTwo = emg2.read(); |
Jankoekenpan | 15:5d2bce2abd41 | 175 | float notch2 = bqc2.step( emgTwo ); |
Jankoekenpan | 15:5d2bce2abd41 | 176 | |
Jankoekenpan | 15:5d2bce2abd41 | 177 | float rect1 = rectifier(notch1); |
Jankoekenpan | 15:5d2bce2abd41 | 178 | float rect2 = rectifier(notch2); |
Jankoekenpan | 15:5d2bce2abd41 | 179 | |
Jankoekenpan | 15:5d2bce2abd41 | 180 | float filtered1 = movingAverage( rect1, calibrateEmgCache1, calibrateNumEmgCache); |
Jankoekenpan | 15:5d2bce2abd41 | 181 | float filtered2 = movingAverage( rect2, calibrateEmgCache2, calibrateNumEmgCache); |
Jankoekenpan | 15:5d2bce2abd41 | 182 | } |
Jankoekenpan | 15:5d2bce2abd41 | 183 | |
Jankoekenpan | 15:5d2bce2abd41 | 184 | void calibrate() { |
Jankoekenpan | 15:5d2bce2abd41 | 185 | while(calibrating) { |
Jankoekenpan | 15:5d2bce2abd41 | 186 | led_red = false; |
Jankoekenpan | 15:5d2bce2abd41 | 187 | wait(0.5); |
Jankoekenpan | 15:5d2bce2abd41 | 188 | led_red = true; |
Jankoekenpan | 15:5d2bce2abd41 | 189 | wait(0.5); |
Jankoekenpan | 15:5d2bce2abd41 | 190 | } |
Jankoekenpan | 15:5d2bce2abd41 | 191 | |
Jankoekenpan | 15:5d2bce2abd41 | 192 | // Button pressed for rest measurement |
Jankoekenpan | 15:5d2bce2abd41 | 193 | led_red = true; |
Jankoekenpan | 15:5d2bce2abd41 | 194 | sampler.attach(&sample, Ts); |
Jankoekenpan | 15:5d2bce2abd41 | 195 | led_blue = false; |
Jankoekenpan | 15:5d2bce2abd41 | 196 | wait(10); |
Jankoekenpan | 15:5d2bce2abd41 | 197 | // 10 seconds sampled |
Jankoekenpan | 15:5d2bce2abd41 | 198 | led_blue = true; |
Jankoekenpan | 15:5d2bce2abd41 | 199 | sampler.detach(); |
Jankoekenpan | 15:5d2bce2abd41 | 200 | float restAvg1 = mean(calibrateEmgCache1, calibrateNumEmgCache); |
Jankoekenpan | 15:5d2bce2abd41 | 201 | float restAvg2 = mean(calibrateEmgCache2, calibrateNumEmgCache); |
Jankoekenpan | 15:5d2bce2abd41 | 202 | |
Jankoekenpan | 15:5d2bce2abd41 | 203 | int i =0; |
Jankoekenpan | 15:5d2bce2abd41 | 204 | while(i<3) { |
Jankoekenpan | 15:5d2bce2abd41 | 205 | led_green = false; |
Jankoekenpan | 15:5d2bce2abd41 | 206 | wait(0.5); |
Jankoekenpan | 15:5d2bce2abd41 | 207 | led_green = true; |
Jankoekenpan | 15:5d2bce2abd41 | 208 | wait(0.5); |
Jankoekenpan | 15:5d2bce2abd41 | 209 | i++; |
Jankoekenpan | 15:5d2bce2abd41 | 210 | } |
Jankoekenpan | 15:5d2bce2abd41 | 211 | led_green = true; |
Jankoekenpan | 15:5d2bce2abd41 | 212 | |
Jankoekenpan | 15:5d2bce2abd41 | 213 | while(calibrating) { |
Jankoekenpan | 15:5d2bce2abd41 | 214 | led_red = false; |
Jankoekenpan | 15:5d2bce2abd41 | 215 | wait(0.5); |
Jankoekenpan | 15:5d2bce2abd41 | 216 | led_red = true; |
Jankoekenpan | 15:5d2bce2abd41 | 217 | wait(0.5); |
Jankoekenpan | 15:5d2bce2abd41 | 218 | } |
Jankoekenpan | 15:5d2bce2abd41 | 219 | // Button pressed for contracted measurement |
Jankoekenpan | 15:5d2bce2abd41 | 220 | led_red = true; |
Jankoekenpan | 15:5d2bce2abd41 | 221 | sampler.attach(&sample, Ts); |
Jankoekenpan | 15:5d2bce2abd41 | 222 | led_blue = false; |
Jankoekenpan | 15:5d2bce2abd41 | 223 | wait(10); |
Jankoekenpan | 15:5d2bce2abd41 | 224 | |
Jankoekenpan | 15:5d2bce2abd41 | 225 | // 10 seconds sampled |
Jankoekenpan | 15:5d2bce2abd41 | 226 | led_blue = true; |
Jankoekenpan | 15:5d2bce2abd41 | 227 | sampler.detach(); |
Jankoekenpan | 15:5d2bce2abd41 | 228 | |
Jankoekenpan | 15:5d2bce2abd41 | 229 | i =0; |
Jankoekenpan | 15:5d2bce2abd41 | 230 | while(i<3) { |
Jankoekenpan | 15:5d2bce2abd41 | 231 | led_green = false; |
Jankoekenpan | 15:5d2bce2abd41 | 232 | wait(0.5); |
Jankoekenpan | 15:5d2bce2abd41 | 233 | led_green = true; |
Jankoekenpan | 15:5d2bce2abd41 | 234 | wait(0.5); |
Jankoekenpan | 15:5d2bce2abd41 | 235 | i++; |
Jankoekenpan | 15:5d2bce2abd41 | 236 | } |
Jankoekenpan | 15:5d2bce2abd41 | 237 | |
Jankoekenpan | 15:5d2bce2abd41 | 238 | float contAvg1 = mean(calibrateEmgCache1, calibrateNumEmgCache); |
Jankoekenpan | 15:5d2bce2abd41 | 239 | float contAvg2 = mean(calibrateEmgCache2, calibrateNumEmgCache); |
Jankoekenpan | 15:5d2bce2abd41 | 240 | |
Jankoekenpan | 15:5d2bce2abd41 | 241 | threshold1 = (contAvg1 + restAvg1)/2; |
Jankoekenpan | 15:5d2bce2abd41 | 242 | threshold2 = (contAvg2 + restAvg2)/2; |
Jankoekenpan | 15:5d2bce2abd41 | 243 | //pc.printf("threshold1: %f\tthreshold2:%f\n\r", threshold1, threshold2); |
Jankoekenpan | 9:3193094ba3b2 | 244 | |
Jankoekenpan | 9:3193094ba3b2 | 245 | } |
Jankoekenpan | 9:3193094ba3b2 | 246 | |
Jankoekenpan | 15:5d2bce2abd41 | 247 | // ===== The main functions called by our main ticker ====== |
Jankoekenpan | 15:5d2bce2abd41 | 248 | |
Jankoekenpan | 15:5d2bce2abd41 | 249 | void processEMG() { |
Jankoekenpan | 15:5d2bce2abd41 | 250 | float emgOne = emg1.read(); |
Jankoekenpan | 15:5d2bce2abd41 | 251 | float emgTwo = emg2.read(); |
Jankoekenpan | 15:5d2bce2abd41 | 252 | float notch1 = bqc1.step( emgOne ); |
Jankoekenpan | 15:5d2bce2abd41 | 253 | float notch2 = bqc2.step( emgTwo ); |
Jankoekenpan | 15:5d2bce2abd41 | 254 | |
Jankoekenpan | 15:5d2bce2abd41 | 255 | float rect1 = rectifier(notch1); |
Jankoekenpan | 15:5d2bce2abd41 | 256 | float rect2 = rectifier(notch2); |
Jankoekenpan | 15:5d2bce2abd41 | 257 | |
Jankoekenpan | 15:5d2bce2abd41 | 258 | float filtered1 = movingAverage( rect1, emgCache1, numEmgCache); |
Jankoekenpan | 15:5d2bce2abd41 | 259 | float filtered2 = movingAverage( rect2, emgCache2, numEmgCache); |
Jankoekenpan | 1:7d218e9d2111 | 260 | |
Jankoekenpan | 15:5d2bce2abd41 | 261 | int decide1 = decide(mean(emgCache1, numEmgCache ), threshold1); |
Jankoekenpan | 15:5d2bce2abd41 | 262 | int decide2 = decide(mean(emgCache2, numEmgCache ), threshold2); |
Jankoekenpan | 15:5d2bce2abd41 | 263 | addFirst(decide1, decided1, numEmgCache); |
Jankoekenpan | 15:5d2bce2abd41 | 264 | addFirst(decide2, decided2, numEmgCache); |
Jankoekenpan | 15:5d2bce2abd41 | 265 | |
Jankoekenpan | 15:5d2bce2abd41 | 266 | if (count >= 49) { |
Jankoekenpan | 15:5d2bce2abd41 | 267 | int counter1=0; |
Jankoekenpan | 15:5d2bce2abd41 | 268 | int counter2=0; |
Jankoekenpan | 15:5d2bce2abd41 | 269 | for(int i = 0; i < numEmgCache; ++i){ |
Jankoekenpan | 15:5d2bce2abd41 | 270 | if(decided1[i] == 0) |
Jankoekenpan | 15:5d2bce2abd41 | 271 | ++counter1; |
Jankoekenpan | 15:5d2bce2abd41 | 272 | if(decided2[i] == 0) |
Jankoekenpan | 15:5d2bce2abd41 | 273 | ++counter2; |
Jankoekenpan | 15:5d2bce2abd41 | 274 | } |
Jankoekenpan | 15:5d2bce2abd41 | 275 | int avgDecide1 = counter1 > std::ceil(numEmgCache/2.0) ? 0: 1; |
Jankoekenpan | 15:5d2bce2abd41 | 276 | int avgDecide2 = counter2 > std::ceil(numEmgCache/2.0) ? 0: 1; |
Jankoekenpan | 15:5d2bce2abd41 | 277 | sendToMotor(motorFunc,avgDecide1, avgDecide2); |
Jankoekenpan | 15:5d2bce2abd41 | 278 | |
Jankoekenpan | 15:5d2bce2abd41 | 279 | count =0; |
Jankoekenpan | 15:5d2bce2abd41 | 280 | } else { |
Jankoekenpan | 15:5d2bce2abd41 | 281 | count++; |
Jankoekenpan | 15:5d2bce2abd41 | 282 | } |
Jankoekenpan | 15:5d2bce2abd41 | 283 | } |
Jankoekenpan | 15:5d2bce2abd41 | 284 | |
Jankoekenpan | 20:66513b3c09b7 | 285 | void getXandY(float &x, float &y) { |
Jankoekenpan | 20:66513b3c09b7 | 286 | float lower = robotController.getLowerArmLength(); |
Jankoekenpan | 20:66513b3c09b7 | 287 | float upper = robotController.getUpperArmLength(); |
Jankoekenpan | 20:66513b3c09b7 | 288 | getRollerPositionForArmLengths(upper, lower, x, y); |
Jankoekenpan | 20:66513b3c09b7 | 289 | } |
Jankoekenpan | 20:66513b3c09b7 | 290 | |
Jankoekenpan | 20:66513b3c09b7 | 291 | |
Jankoekenpan | 15:5d2bce2abd41 | 292 | /* executes the robot command */ |
Jankoekenpan | 15:5d2bce2abd41 | 293 | void processCommand(RobotCommand cmd) { |
Jankoekenpan | 15:5d2bce2abd41 | 294 | if (cmd == robotCommand) return; |
Jankoekenpan | 15:5d2bce2abd41 | 295 | |
Jankoekenpan | 15:5d2bce2abd41 | 296 | robotCommand = cmd; |
Jankoekenpan | 15:5d2bce2abd41 | 297 | |
Jankoekenpan | 15:5d2bce2abd41 | 298 | switch (robotCommand) { |
Jankoekenpan | 20:66513b3c09b7 | 299 | float x; |
Jankoekenpan | 20:66513b3c09b7 | 300 | float y; |
Jankoekenpan | 15:5d2bce2abd41 | 301 | case UP: |
Jankoekenpan | 16:66326e4a40b1 | 302 | robotController.paintUp(); |
Jankoekenpan | 15:5d2bce2abd41 | 303 | break; |
Jankoekenpan | 15:5d2bce2abd41 | 304 | case DOWN: |
Jankoekenpan | 16:66326e4a40b1 | 305 | robotController.paintDown(); |
Jankoekenpan | 15:5d2bce2abd41 | 306 | break; |
Jankoekenpan | 15:5d2bce2abd41 | 307 | case FORWARD: |
Jankoekenpan | 20:66513b3c09b7 | 308 | getXandY(x, y); |
Jankoekenpan | 20:66513b3c09b7 | 309 | if (x < 40) |
Jankoekenpan | 20:66513b3c09b7 | 310 | robotController.moveTo(x + 5.0f, y); |
Jankoekenpan | 15:5d2bce2abd41 | 311 | break; |
Jankoekenpan | 15:5d2bce2abd41 | 312 | case BACKWARD: |
Jankoekenpan | 20:66513b3c09b7 | 313 | getXandY(x, y); |
Jankoekenpan | 20:66513b3c09b7 | 314 | if (x > 5) |
Jankoekenpan | 20:66513b3c09b7 | 315 | robotController.moveTo(x - 5.0f, y); |
Jankoekenpan | 15:5d2bce2abd41 | 316 | break; |
Jankoekenpan | 15:5d2bce2abd41 | 317 | case NOTHING: |
Jankoekenpan | 15:5d2bce2abd41 | 318 | |
Jankoekenpan | 15:5d2bce2abd41 | 319 | break; |
Jankoekenpan | 15:5d2bce2abd41 | 320 | } |
Jankoekenpan | 15:5d2bce2abd41 | 321 | } |
Jankoekenpan | 15:5d2bce2abd41 | 322 | |
Jankoekenpan | 15:5d2bce2abd41 | 323 | //some little utils used by the function below |
Jankoekenpan | 15:5d2bce2abd41 | 324 | Timeout switchBlocker; |
Jankoekenpan | 15:5d2bce2abd41 | 325 | volatile bool justSwitched; |
Jankoekenpan | 15:5d2bce2abd41 | 326 | void unblockSwitch() { |
Jankoekenpan | 15:5d2bce2abd41 | 327 | justSwitched = false; |
Jankoekenpan | 15:5d2bce2abd41 | 328 | } |
Jankoekenpan | 15:5d2bce2abd41 | 329 | |
Jankoekenpan | 15:5d2bce2abd41 | 330 | //tries to switch the state. |
Jankoekenpan | 15:5d2bce2abd41 | 331 | //returns true if it was successfull |
Jankoekenpan | 15:5d2bce2abd41 | 332 | //or false if we couldn't switch. |
Jankoekenpan | 15:5d2bce2abd41 | 333 | bool switchState() { |
Jankoekenpan | 15:5d2bce2abd41 | 334 | if (justSwitched) return false; |
Jankoekenpan | 15:5d2bce2abd41 | 335 | justSwitched = true; |
Jankoekenpan | 15:5d2bce2abd41 | 336 | switch(progState) { |
Jankoekenpan | 15:5d2bce2abd41 | 337 | case UPDOWN: |
Jankoekenpan | 15:5d2bce2abd41 | 338 | progState = FORBACK; |
Jankoekenpan | 15:5d2bce2abd41 | 339 | break; |
Jankoekenpan | 15:5d2bce2abd41 | 340 | case FORBACK: |
Jankoekenpan | 15:5d2bce2abd41 | 341 | progState = UPDOWN; |
Jankoekenpan | 15:5d2bce2abd41 | 342 | break; |
Jankoekenpan | 15:5d2bce2abd41 | 343 | } |
Jankoekenpan | 15:5d2bce2abd41 | 344 | //we can only switch once per 2 seconds |
Jankoekenpan | 15:5d2bce2abd41 | 345 | switchBlocker.attach(&unblockSwitch, 2.0f); |
Jankoekenpan | 15:5d2bce2abd41 | 346 | return true; |
Jankoekenpan | 15:5d2bce2abd41 | 347 | } |
Jankoekenpan | 15:5d2bce2abd41 | 348 | |
Jankoekenpan | 15:5d2bce2abd41 | 349 | /* Translates our two digital signals to robot commands */ |
Jankoekenpan | 15:5d2bce2abd41 | 350 | void onSignal(bool emg1, bool emg2) { |
Jankoekenpan | 15:5d2bce2abd41 | 351 | RobotCommand command = NOTHING; |
Jankoekenpan | 15:5d2bce2abd41 | 352 | if (emg1 && emg2) { |
Jankoekenpan | 15:5d2bce2abd41 | 353 | switchState(); |
Jankoekenpan | 15:5d2bce2abd41 | 354 | processCommand(command); |
Jankoekenpan | 15:5d2bce2abd41 | 355 | return; |
Jankoekenpan | 15:5d2bce2abd41 | 356 | } |
Jankoekenpan | 15:5d2bce2abd41 | 357 | switch(progState) { |
Jankoekenpan | 15:5d2bce2abd41 | 358 | case UPDOWN: |
Jankoekenpan | 15:5d2bce2abd41 | 359 | if (emg1) { |
Jankoekenpan | 15:5d2bce2abd41 | 360 | command = UP; |
Jankoekenpan | 15:5d2bce2abd41 | 361 | } else if (emg2) { |
Jankoekenpan | 15:5d2bce2abd41 | 362 | command = DOWN; |
Jankoekenpan | 15:5d2bce2abd41 | 363 | } |
Jankoekenpan | 15:5d2bce2abd41 | 364 | break; |
Jankoekenpan | 15:5d2bce2abd41 | 365 | case FORBACK: |
Jankoekenpan | 15:5d2bce2abd41 | 366 | if (emg1) { |
Jankoekenpan | 15:5d2bce2abd41 | 367 | command = FORWARD; |
Jankoekenpan | 15:5d2bce2abd41 | 368 | } else if (emg2) { |
Jankoekenpan | 15:5d2bce2abd41 | 369 | command = BACKWARD; |
Jankoekenpan | 15:5d2bce2abd41 | 370 | } |
Jankoekenpan | 15:5d2bce2abd41 | 371 | break; |
Jankoekenpan | 15:5d2bce2abd41 | 372 | } |
Jankoekenpan | 15:5d2bce2abd41 | 373 | |
Jankoekenpan | 15:5d2bce2abd41 | 374 | //execute the command |
Jankoekenpan | 15:5d2bce2abd41 | 375 | processCommand(command); |
Jankoekenpan | 15:5d2bce2abd41 | 376 | } |
Jankoekenpan | 15:5d2bce2abd41 | 377 | |
Jankoekenpan | 15:5d2bce2abd41 | 378 | void consumeBools(bool x, bool y) { |
Jankoekenpan | 15:5d2bce2abd41 | 379 | //pc.printf("%d\t%d\r\n", x, y); |
Jankoekenpan | 15:5d2bce2abd41 | 380 | onSignal(x, y); |
Jankoekenpan | 15:5d2bce2abd41 | 381 | } |
Jankoekenpan | 15:5d2bce2abd41 | 382 | |
Jankoekenpan | 15:5d2bce2abd41 | 383 | |
Jankoekenpan | 15:5d2bce2abd41 | 384 | // ====== The entry point of our programme ====== |
Jankoekenpan | 15:5d2bce2abd41 | 385 | |
Jankoekenpan | 22:cda3e5ad89c0 | 386 | |
Jankoekenpan | 21:2aed81380bc3 | 387 | int main() //TODO this will become the actual main! |
Jankoekenpan | 15:5d2bce2abd41 | 388 | { |
Jankoekenpan | 15:5d2bce2abd41 | 389 | //pc.baud(115200); |
Jankoekenpan | 22:cda3e5ad89c0 | 390 | killButton.fall(robotController.getRobot(), &Robot::kill); |
Jankoekenpan | 15:5d2bce2abd41 | 391 | |
Jankoekenpan | 15:5d2bce2abd41 | 392 | // initial state |
Jankoekenpan | 15:5d2bce2abd41 | 393 | resetLeds(); |
Jankoekenpan | 15:5d2bce2abd41 | 394 | progState = CALIBRATING; |
Jankoekenpan | 15:5d2bce2abd41 | 395 | robotCommand = NOTHING; |
Jankoekenpan | 15:5d2bce2abd41 | 396 | |
Jankoekenpan | 15:5d2bce2abd41 | 397 | // initialize notch filters |
Jankoekenpan | 15:5d2bce2abd41 | 398 | bqc1.add( &bq1 ); |
Jankoekenpan | 15:5d2bce2abd41 | 399 | bqc2.add( &bq2 ); |
Jankoekenpan | 15:5d2bce2abd41 | 400 | |
Jankoekenpan | 15:5d2bce2abd41 | 401 | // Attach cablitrate function to the button to be able to calibrate again |
Jankoekenpan | 15:5d2bce2abd41 | 402 | // If the user desires so |
Jankoekenpan | 15:5d2bce2abd41 | 403 | calibrateButton.fall(&calibrate); |
Jankoekenpan | 15:5d2bce2abd41 | 404 | |
Jankoekenpan | 15:5d2bce2abd41 | 405 | // The function that takes our ditised signals and controls the robot |
Jankoekenpan | 15:5d2bce2abd41 | 406 | motorFunc = &consumeBools; |
Jankoekenpan | 15:5d2bce2abd41 | 407 | |
Jankoekenpan | 15:5d2bce2abd41 | 408 | |
Jankoekenpan | 15:5d2bce2abd41 | 409 | // call the calibrating function once at the start |
Jankoekenpan | 15:5d2bce2abd41 | 410 | // this function blocks until the calibration phase is over |
Jankoekenpan | 15:5d2bce2abd41 | 411 | calibrate(); |
Jankoekenpan | 15:5d2bce2abd41 | 412 | |
Jankoekenpan | 15:5d2bce2abd41 | 413 | // After calibration the program state is UPDOWN |
Jankoekenpan | 15:5d2bce2abd41 | 414 | progState = UPDOWN; |
Jankoekenpan | 15:5d2bce2abd41 | 415 | |
Jankoekenpan | 15:5d2bce2abd41 | 416 | |
Jankoekenpan | 15:5d2bce2abd41 | 417 | // 500 HZ Ticker |
Jankoekenpan | 15:5d2bce2abd41 | 418 | ticker.attach(&processEMG, Ts); |
Jankoekenpan | 15:5d2bce2abd41 | 419 | |
Jankoekenpan | 15:5d2bce2abd41 | 420 | while (true); |
Jankoekenpan | 21:2aed81380bc3 | 421 | } |