Project Paint / Mbed 2 deprecated arm_control

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@29:1c5254b27851, 2016-11-04 (annotated)

Committer:
Jankoekenpan
Date:
Fri Nov 04 13:26:28 2016 +0000
Revision:
29:1c5254b27851
Parent:
27:91dc5174333a 
documented the processEMG function

Who changed what in which revision?

UserRevisionLine numberNew 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 23:dba0233303b6 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 29:1c5254b27851 54
Jankoekenpan 29:1c5254b27851 55 //Counts how many signals have passed. Resets at 50. See processEMG.
Jankoekenpan 29:1c5254b27851 56 volatile int count = 0;
Jankoekenpan 15:5d2bce2abd41 57
Jankoekenpan 15:5d2bce2abd41 58 /*Function used to send data to the motor*/
Jankoekenpan 15:5d2bce2abd41 59 void (*motorFunc)(bool, bool);
Jankoekenpan 15:5d2bce2abd41 60
Jankoekenpan 15:5d2bce2abd41 61
Jankoekenpan 15:5d2bce2abd41 62
Jankoekenpan 15:5d2bce2abd41 63 /* EMG BiQuadChain 1 */
Jankoekenpan 15:5d2bce2abd41 64 BiQuadChain bqc1;
Jankoekenpan 15:5d2bce2abd41 65 //Notch iir filter.
Jankoekenpan 15:5d2bce2abd41 66 //Notch: 50 +- 2 Hz
Jankoekenpan 15:5d2bce2abd41 67 BiQuad bq1(9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
Jankoekenpan 15:5d2bce2abd41 68
Jankoekenpan 15:5d2bce2abd41 69
Jankoekenpan 15:5d2bce2abd41 70 /* EMG BiQuadChain 2 */
Jankoekenpan 15:5d2bce2abd41 71 BiQuadChain bqc2;
Jankoekenpan 15:5d2bce2abd41 72 //Notch iir filter.
Jankoekenpan 15:5d2bce2abd41 73 //Notch: 50 +- 2 Hz
Jankoekenpan 15:5d2bce2abd41 74 BiQuad bq2( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
Jankoekenpan 15:5d2bce2abd41 75
Jankoekenpan 15:5d2bce2abd41 76
Jankoekenpan 15:5d2bce2abd41 77 // Arrays used in the calibrationi phase
Jankoekenpan 15:5d2bce2abd41 78 // Values in these arrays contain samples that are already notched and rectified.
Jankoekenpan 15:5d2bce2abd41 79 const int calibrateNumEmgCache = 100;
Jankoekenpan 15:5d2bce2abd41 80 float calibrateEmgCache1[calibrateNumEmgCache]; //sorted from new to old;
Jankoekenpan 15:5d2bce2abd41 81 float calibrateEmgCache2[calibrateNumEmgCache]; //sorted from new to old;
Jankoekenpan 15:5d2bce2abd41 82
Jankoekenpan 15:5d2bce2abd41 83 // Arrays used to calculate the moving average
Jankoekenpan 15:5d2bce2abd41 84 // Values in these arrays contain samples that are already notched and rectified.
Jankoekenpan 15:5d2bce2abd41 85 const int numEmgCache = 50;
Jankoekenpan 15:5d2bce2abd41 86 float emgCache1[numEmgCache]; //sorted from new to old;
Jankoekenpan 15:5d2bce2abd41 87 float emgCache2[numEmgCache]; //sorted from new to old;
Jankoekenpan 15:5d2bce2abd41 88
Jankoekenpan 15:5d2bce2abd41 89
Jankoekenpan 15:5d2bce2abd41 90 // Thresholds for the decisioin. by default 0.2,
Jankoekenpan 15:5d2bce2abd41 91 // The values are changed during calibration.
Jankoekenpan 15:5d2bce2abd41 92 volatile float threshold1 = 0.2;
Jankoekenpan 15:5d2bce2abd41 93 volatile float threshold2 = 0.2;
Jankoekenpan 15:5d2bce2abd41 94
Jankoekenpan 15:5d2bce2abd41 95 // The last 50 signals that have been dititalised.
Jankoekenpan 15:5d2bce2abd41 96 // Only contains ones and zeros.
Jankoekenpan 15:5d2bce2abd41 97 int decided1[numEmgCache];
Jankoekenpan 15:5d2bce2abd41 98 int decided2[numEmgCache];
Jankoekenpan 1:7d218e9d2111 99
Jankoekenpan 8:874fe459b10a 100
Jankoekenpan 1:7d218e9d2111 101 //====== Functions ======
Jankoekenpan 1:7d218e9d2111 102
Jankoekenpan 15:5d2bce2abd41 103 // Helper Functions
Jankoekenpan 1:7d218e9d2111 104
Jankoekenpan 15:5d2bce2abd41 105 void resetLeds() {
Jankoekenpan 15:5d2bce2abd41 106 led_red = true;
Jankoekenpan 15:5d2bce2abd41 107 led_green = true;
Jankoekenpan 15:5d2bce2abd41 108 led_blue = true;
Jankoekenpan 15:5d2bce2abd41 109 }
Jankoekenpan 15:5d2bce2abd41 110
Jankoekenpan 15:5d2bce2abd41 111 // Rotates the array one position, replacing the first value with the new value
Jankoekenpan 15:5d2bce2abd41 112 void addFirst(float newValue, float array[], int size) {
Jankoekenpan 15:5d2bce2abd41 113 for (int i = size - 2; i >= 0; i--) {
Jankoekenpan 15:5d2bce2abd41 114 array[i+1] = array[i];
Jankoekenpan 15:5d2bce2abd41 115 }
Jankoekenpan 15:5d2bce2abd41 116 array[0] = newValue;
Jankoekenpan 15:5d2bce2abd41 117 }
Jankoekenpan 15:5d2bce2abd41 118
Jankoekenpan 15:5d2bce2abd41 119 // Rotates the array one position, replacing the first value with the new value
Jankoekenpan 15:5d2bce2abd41 120 void addFirst(int newValue, int array[], int size) {
Jankoekenpan 15:5d2bce2abd41 121 for (int i = size - 2; i >= 0; i--) {
Jankoekenpan 15:5d2bce2abd41 122 array[i+1] = array[i];
Jankoekenpan 15:5d2bce2abd41 123 }
Jankoekenpan 15:5d2bce2abd41 124 array[0] = newValue;
Jankoekenpan 15:5d2bce2abd41 125 }
Jankoekenpan 15:5d2bce2abd41 126
Jankoekenpan 15:5d2bce2abd41 127
Jankoekenpan 15:5d2bce2abd41 128 float sum(float array[], int size) {
Jankoekenpan 15:5d2bce2abd41 129 float sum = 0;
Jankoekenpan 15:5d2bce2abd41 130 for (int i = 0; i < size; i++) {
Jankoekenpan 15:5d2bce2abd41 131 sum += array[i];
Jankoekenpan 15:5d2bce2abd41 132 }
Jankoekenpan 15:5d2bce2abd41 133 return sum;
Jankoekenpan 15:5d2bce2abd41 134 }
Jankoekenpan 15:5d2bce2abd41 135
Jankoekenpan 15:5d2bce2abd41 136 float mean(float array[], int size) {
Jankoekenpan 15:5d2bce2abd41 137 return sum(array, size) / size;
Jankoekenpan 15:5d2bce2abd41 138 }
Jankoekenpan 15:5d2bce2abd41 139
Jankoekenpan 15:5d2bce2abd41 140 // 'Digitize' an analog value by comparing to a threshold
Jankoekenpan 15:5d2bce2abd41 141 int decide(float value, float threshold) {
Jankoekenpan 15:5d2bce2abd41 142 return value < threshold ? 0 : 1;
Jankoekenpan 15:5d2bce2abd41 143 }
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 29:1c5254b27851 248
Jankoekenpan 29:1c5254b27851 249 /**
Jankoekenpan 29:1c5254b27851 250 * processEMG function
Jankoekenpan 29:1c5254b27851 251 * This function is called by our ticker.
Jankoekenpan 29:1c5254b27851 252 * This function measures emg and applies the filters.
Jankoekenpan 29:1c5254b27851 253 * out of 50 emg values, a 1 or 0 is chosen depending on which occurs the most
Jankoekenpan 29:1c5254b27851 254 * then that value is used in the motor func.
Jankoekenpan 29:1c5254b27851 255 */
Jankoekenpan 15:5d2bce2abd41 256 void processEMG() {
Jankoekenpan 29:1c5254b27851 257 //read emg
Jankoekenpan 15:5d2bce2abd41 258 float emgOne = emg1.read();
Jankoekenpan 15:5d2bce2abd41 259 float emgTwo = emg2.read();
Jankoekenpan 29:1c5254b27851 260
Jankoekenpan 29:1c5254b27851 261 //apply notch filter
Jankoekenpan 15:5d2bce2abd41 262 float notch1 = bqc1.step( emgOne );
Jankoekenpan 15:5d2bce2abd41 263 float notch2 = bqc2.step( emgTwo );
Jankoekenpan 29:1c5254b27851 264
Jankoekenpan 29:1c5254b27851 265 //then apply rectifier
Jankoekenpan 15:5d2bce2abd41 266 float rect1 = rectifier(notch1);
Jankoekenpan 15:5d2bce2abd41 267 float rect2 = rectifier(notch2);
Jankoekenpan 15:5d2bce2abd41 268
Jankoekenpan 29:1c5254b27851 269 //then apply moving average
Jankoekenpan 15:5d2bce2abd41 270 float filtered1 = movingAverage( rect1, emgCache1, numEmgCache);
Jankoekenpan 15:5d2bce2abd41 271 float filtered2 = movingAverage( rect2, emgCache2, numEmgCache);
Jankoekenpan 1:7d218e9d2111 272
Jankoekenpan 29:1c5254b27851 273 //decide on wether the signal was strong enough (1) or too weak (0)
Jankoekenpan 15:5d2bce2abd41 274 int decide1 = decide(mean(emgCache1, numEmgCache ), threshold1);
Jankoekenpan 15:5d2bce2abd41 275 int decide2 = decide(mean(emgCache2, numEmgCache ), threshold2);
Jankoekenpan 29:1c5254b27851 276
Jankoekenpan 29:1c5254b27851 277 //add boolean value in front of the boolean arrays
Jankoekenpan 15:5d2bce2abd41 278 addFirst(decide1, decided1, numEmgCache);
Jankoekenpan 15:5d2bce2abd41 279 addFirst(decide2, decided2, numEmgCache);
Jankoekenpan 15:5d2bce2abd41 280
Jankoekenpan 29:1c5254b27851 281 //after 50 calls of this function, control the motor.
Jankoekenpan 15:5d2bce2abd41 282 if (count >= 49) {
Jankoekenpan 15:5d2bce2abd41 283 int counter1=0;
Jankoekenpan 15:5d2bce2abd41 284 int counter2=0;
Jankoekenpan 15:5d2bce2abd41 285 for(int i = 0; i < numEmgCache; ++i){
Jankoekenpan 15:5d2bce2abd41 286 if(decided1[i] == 0)
Jankoekenpan 15:5d2bce2abd41 287 ++counter1;
Jankoekenpan 15:5d2bce2abd41 288 if(decided2[i] == 0)
Jankoekenpan 15:5d2bce2abd41 289 ++counter2;
Jankoekenpan 15:5d2bce2abd41 290 }
Jankoekenpan 29:1c5254b27851 291 // avgDecide1 = true if the decided1 array contains mostly ones, otherwise false.
Jankoekenpan 15:5d2bce2abd41 292 int avgDecide1 = counter1 > std::ceil(numEmgCache/2.0) ? 0: 1;
Jankoekenpan 29:1c5254b27851 293 // avgDecide2 = true if the decided2 array contains mostly ones, otherwise false.
Jankoekenpan 15:5d2bce2abd41 294 int avgDecide2 = counter2 > std::ceil(numEmgCache/2.0) ? 0: 1;
Jankoekenpan 29:1c5254b27851 295
Jankoekenpan 29:1c5254b27851 296 // Use these values to contorl the motor.
Jankoekenpan 15:5d2bce2abd41 297 sendToMotor(motorFunc,avgDecide1, avgDecide2);
Jankoekenpan 15:5d2bce2abd41 298
Jankoekenpan 29:1c5254b27851 299 //reset function call count to 0
Jankoekenpan 15:5d2bce2abd41 300 count =0;
Jankoekenpan 15:5d2bce2abd41 301 } else {
Jankoekenpan 15:5d2bce2abd41 302 count++;
Jankoekenpan 15:5d2bce2abd41 303 }
Jankoekenpan 15:5d2bce2abd41 304 }
Jankoekenpan 15:5d2bce2abd41 305
Jankoekenpan 20:66513b3c09b7 306 void getXandY(float &x, float &y) {
Jankoekenpan 20:66513b3c09b7 307 float lower = robotController.getLowerArmLength();
Jankoekenpan 20:66513b3c09b7 308 float upper = robotController.getUpperArmLength();
Jankoekenpan 20:66513b3c09b7 309 getRollerPositionForArmLengths(upper, lower, x, y);
Jankoekenpan 20:66513b3c09b7 310 }
Jankoekenpan 20:66513b3c09b7 311
Jankoekenpan 23:dba0233303b6 312 //wheter the user can still execute a command
Jankoekenpan 23:dba0233303b6 313 Timeout commandDelayer;
Jankoekenpan 23:dba0233303b6 314 volatile bool canCommand = true;
Jankoekenpan 23:dba0233303b6 315 void enableCommand() {
Jankoekenpan 23:dba0233303b6 316 canCommand = true;
Jankoekenpan 23:dba0233303b6 317 }
Jankoekenpan 23:dba0233303b6 318
Jankoekenpan 23:dba0233303b6 319 Timeout commandSequencer;
Jankoekenpan 23:dba0233303b6 320 void doPaintUp() {
Jankoekenpan 23:dba0233303b6 321 robotController.paintUp();
Jankoekenpan 23:dba0233303b6 322 }
Jankoekenpan 23:dba0233303b6 323 void doPaintDown() {
Jankoekenpan 23:dba0233303b6 324 robotController.paintDown();
Jankoekenpan 23:dba0233303b6 325 }
Jankoekenpan 20:66513b3c09b7 326
Jankoekenpan 27:91dc5174333a 327 float xmin = 25;
Jankoekenpan 27:91dc5174333a 328 float xmax = 40;
Jankoekenpan 27:91dc5174333a 329 float ymin = 20;
Jankoekenpan 27:91dc5174333a 330 float ymax = 50;
Jankoekenpan 27:91dc5174333a 331
Jankoekenpan 15:5d2bce2abd41 332 /* executes the robot command */
Jankoekenpan 15:5d2bce2abd41 333 void processCommand(RobotCommand cmd) {
Jankoekenpan 23:dba0233303b6 334 if (!canCommand || cmd == robotCommand) return;
Jankoekenpan 15:5d2bce2abd41 335
Jankoekenpan 15:5d2bce2abd41 336 robotCommand = cmd;
Jankoekenpan 15:5d2bce2abd41 337
Jankoekenpan 23:dba0233303b6 338 //user can only switch commands once every 2 seconds
Jankoekenpan 23:dba0233303b6 339 canCommand = false;
Jankoekenpan 23:dba0233303b6 340 commandDelayer.attach(&enableCommand, 2.0f);
Jankoekenpan 23:dba0233303b6 341
Jankoekenpan 15:5d2bce2abd41 342 switch (robotCommand) {
Jankoekenpan 20:66513b3c09b7 343 float x;
Jankoekenpan 20:66513b3c09b7 344 float y;
Jankoekenpan 15:5d2bce2abd41 345 case UP:
Jankoekenpan 23:dba0233303b6 346 robotController.goToBottom();
Jankoekenpan 23:dba0233303b6 347 commandSequencer.attach(&doPaintUp, 3.0f);
Jankoekenpan 15:5d2bce2abd41 348 break;
Jankoekenpan 15:5d2bce2abd41 349 case DOWN:
Jankoekenpan 23:dba0233303b6 350 robotController.goToTop();
Jankoekenpan 23:dba0233303b6 351 commandSequencer.attach(&doPaintDown, 3.0f);
Jankoekenpan 15:5d2bce2abd41 352 break;
Jankoekenpan 15:5d2bce2abd41 353 case FORWARD:
Jankoekenpan 20:66513b3c09b7 354 getXandY(x, y);
Jankoekenpan 27:91dc5174333a 355 robotController.moveTo(xmax, h + 0.5*d);
Jankoekenpan 15:5d2bce2abd41 356 break;
Jankoekenpan 15:5d2bce2abd41 357 case BACKWARD:
Jankoekenpan 20:66513b3c09b7 358 getXandY(x, y);
Jankoekenpan 27:91dc5174333a 359 robotController.moveTo(xmin, h + 0.5*d);
Jankoekenpan 15:5d2bce2abd41 360 break;
Jankoekenpan 15:5d2bce2abd41 361 case NOTHING:
Jankoekenpan 15:5d2bce2abd41 362
Jankoekenpan 15:5d2bce2abd41 363 break;
Jankoekenpan 15:5d2bce2abd41 364 }
Jankoekenpan 15:5d2bce2abd41 365 }
Jankoekenpan 15:5d2bce2abd41 366
Jankoekenpan 15:5d2bce2abd41 367 //some little utils used by the function below
Jankoekenpan 15:5d2bce2abd41 368 Timeout switchBlocker;
Jankoekenpan 15:5d2bce2abd41 369 volatile bool justSwitched;
Jankoekenpan 15:5d2bce2abd41 370 void unblockSwitch() {
Jankoekenpan 15:5d2bce2abd41 371 justSwitched = false;
Jankoekenpan 15:5d2bce2abd41 372 }
Jankoekenpan 15:5d2bce2abd41 373
Jankoekenpan 15:5d2bce2abd41 374 //tries to switch the state.
Jankoekenpan 15:5d2bce2abd41 375 //returns true if it was successfull
Jankoekenpan 15:5d2bce2abd41 376 //or false if we couldn't switch.
Jankoekenpan 15:5d2bce2abd41 377 bool switchState() {
Jankoekenpan 15:5d2bce2abd41 378 if (justSwitched) return false;
Jankoekenpan 15:5d2bce2abd41 379 justSwitched = true;
Jankoekenpan 15:5d2bce2abd41 380 switch(progState) {
Jankoekenpan 15:5d2bce2abd41 381 case UPDOWN:
Jankoekenpan 15:5d2bce2abd41 382 progState = FORBACK;
Jankoekenpan 15:5d2bce2abd41 383 break;
Jankoekenpan 15:5d2bce2abd41 384 case FORBACK:
Jankoekenpan 15:5d2bce2abd41 385 progState = UPDOWN;
Jankoekenpan 27:91dc5174333a 386 break;
Jankoekenpan 15:5d2bce2abd41 387 }
Jankoekenpan 27:91dc5174333a 388 pc.printf("\r\n\n\n1 and 1, switch state to %s.\r\n\n\n", progState == UPDOWN ? "UPDOWN" : "FORBACK");
Jankoekenpan 15:5d2bce2abd41 389 //we can only switch once per 2 seconds
Jankoekenpan 15:5d2bce2abd41 390 switchBlocker.attach(&unblockSwitch, 2.0f);
Jankoekenpan 15:5d2bce2abd41 391 return true;
Jankoekenpan 15:5d2bce2abd41 392 }
Jankoekenpan 15:5d2bce2abd41 393
Jankoekenpan 15:5d2bce2abd41 394 /* Translates our two digital signals to robot commands */
Jankoekenpan 15:5d2bce2abd41 395 void onSignal(bool emg1, bool emg2) {
Jankoekenpan 15:5d2bce2abd41 396 RobotCommand command = NOTHING;
Jankoekenpan 15:5d2bce2abd41 397 if (emg1 && emg2) {
Jankoekenpan 15:5d2bce2abd41 398 switchState();
Jankoekenpan 15:5d2bce2abd41 399 processCommand(command);
Jankoekenpan 15:5d2bce2abd41 400 return;
Jankoekenpan 15:5d2bce2abd41 401 }
Jankoekenpan 15:5d2bce2abd41 402 switch(progState) {
Jankoekenpan 15:5d2bce2abd41 403 case UPDOWN:
Jankoekenpan 15:5d2bce2abd41 404 if (emg1) {
Jankoekenpan 15:5d2bce2abd41 405 command = UP;
Jankoekenpan 27:91dc5174333a 406 pc.printf("1 and 0, command = UP\r\n");
Jankoekenpan 15:5d2bce2abd41 407 } else if (emg2) {
Jankoekenpan 15:5d2bce2abd41 408 command = DOWN;
Jankoekenpan 27:91dc5174333a 409 pc.printf("0 and 1, command = DOWN\r\n");
Jankoekenpan 15:5d2bce2abd41 410 }
Jankoekenpan 15:5d2bce2abd41 411 break;
Jankoekenpan 15:5d2bce2abd41 412 case FORBACK:
Jankoekenpan 15:5d2bce2abd41 413 if (emg1) {
Jankoekenpan 15:5d2bce2abd41 414 command = FORWARD;
Jankoekenpan 27:91dc5174333a 415 pc.printf("1 and 0, command = FORWARD\r\n");
Jankoekenpan 15:5d2bce2abd41 416 } else if (emg2) {
Jankoekenpan 15:5d2bce2abd41 417 command = BACKWARD;
Jankoekenpan 27:91dc5174333a 418 pc.printf("0 and 1, command = BACKWARD\r\n");
Jankoekenpan 15:5d2bce2abd41 419 }
Jankoekenpan 15:5d2bce2abd41 420 break;
Jankoekenpan 15:5d2bce2abd41 421 }
Jankoekenpan 15:5d2bce2abd41 422
Jankoekenpan 27:91dc5174333a 423 if (!emg1 && !emg2) {
Jankoekenpan 27:91dc5174333a 424 pc.printf("0 and 0\r\n");
Jankoekenpan 27:91dc5174333a 425 }
Jankoekenpan 27:91dc5174333a 426
Jankoekenpan 15:5d2bce2abd41 427 //execute the command
Jankoekenpan 15:5d2bce2abd41 428 processCommand(command);
Jankoekenpan 15:5d2bce2abd41 429 }
Jankoekenpan 15:5d2bce2abd41 430
Jankoekenpan 15:5d2bce2abd41 431 void consumeBools(bool x, bool y) {
Jankoekenpan 15:5d2bce2abd41 432 onSignal(x, y);
Jankoekenpan 15:5d2bce2abd41 433 }
Jankoekenpan 15:5d2bce2abd41 434
Jankoekenpan 15:5d2bce2abd41 435
Jankoekenpan 15:5d2bce2abd41 436 // ====== The entry point of our programme ======
Jankoekenpan 15:5d2bce2abd41 437
Jankoekenpan 22:cda3e5ad89c0 438
Jankoekenpan 21:2aed81380bc3 439 int main() //TODO this will become the actual main!
Jankoekenpan 15:5d2bce2abd41 440 {
Jankoekenpan 23:dba0233303b6 441 pc.baud(115200);
Jankoekenpan 22:cda3e5ad89c0 442 killButton.fall(robotController.getRobot(), &Robot::kill);
Jankoekenpan 15:5d2bce2abd41 443
Jankoekenpan 15:5d2bce2abd41 444 // initial state
Jankoekenpan 15:5d2bce2abd41 445 resetLeds();
Jankoekenpan 15:5d2bce2abd41 446 progState = CALIBRATING;
Jankoekenpan 15:5d2bce2abd41 447 robotCommand = NOTHING;
Jankoekenpan 15:5d2bce2abd41 448
Jankoekenpan 15:5d2bce2abd41 449 // initialize notch filters
Jankoekenpan 15:5d2bce2abd41 450 bqc1.add( &bq1 );
Jankoekenpan 15:5d2bce2abd41 451 bqc2.add( &bq2 );
Jankoekenpan 15:5d2bce2abd41 452
Jankoekenpan 15:5d2bce2abd41 453 // Attach cablitrate function to the button to be able to calibrate again
Jankoekenpan 15:5d2bce2abd41 454 // If the user desires so
Jankoekenpan 15:5d2bce2abd41 455 calibrateButton.fall(&calibrate);
Jankoekenpan 15:5d2bce2abd41 456
Jankoekenpan 15:5d2bce2abd41 457 // The function that takes our ditised signals and controls the robot
Jankoekenpan 15:5d2bce2abd41 458 motorFunc = &consumeBools;
Jankoekenpan 15:5d2bce2abd41 459
Jankoekenpan 15:5d2bce2abd41 460
Jankoekenpan 15:5d2bce2abd41 461 // call the calibrating function once at the start
Jankoekenpan 15:5d2bce2abd41 462 // this function blocks until the calibration phase is over
Jankoekenpan 15:5d2bce2abd41 463 calibrate();
Jankoekenpan 15:5d2bce2abd41 464
Jankoekenpan 15:5d2bce2abd41 465 // After calibration the program state is UPDOWN
Jankoekenpan 15:5d2bce2abd41 466 progState = UPDOWN;
Jankoekenpan 15:5d2bce2abd41 467
Jankoekenpan 15:5d2bce2abd41 468
Jankoekenpan 15:5d2bce2abd41 469 // 500 HZ Ticker
Jankoekenpan 15:5d2bce2abd41 470 ticker.attach(&processEMG, Ts);
Jankoekenpan 15:5d2bce2abd41 471
Jankoekenpan 15:5d2bce2abd41 472 while (true);
Jankoekenpan 21:2aed81380bc3 473 }