Project Paint / Mbed 2 deprecated arm_control

Dependencies:   mbed QEI biquadFilter

Committer:
Jankoekenpan
Date:
Thu Nov 03 16:37:41 2016 +0000
Revision:
21:2aed81380bc3
Parent:
20:66513b3c09b7
Parent:
19:a82b55a15457
Child:
22:cda3e5ad89c0
merge

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