MBED_DEMOS / Mbed 2 deprecated df-2013-minihack-slingshot-complete

Dependencies:   ADXL345 USBDevice filter mbed

Fork of df-2013-minihack-slingshot-complete by Doug Anson

main.cpp@3:62b70ec1c2b1, 2013-11-01 (annotated)

Committer:
ansond
Date:
Fri Nov 01 18:12:21 2013 +0000
Revision:
3:62b70ec1c2b1
Parent:
2:563ce68ef360
Child:
5:e2cf3b3f2a27
updates

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ansond 0:57bf3eb84d7e 1 /* mbed USB Slingshot,
ansond 0:57bf3eb84d7e 2 *
ansond 0:57bf3eb84d7e 3 * Copyright (c) 2010-2011 mbed.org, MIT License
ansond 0:57bf3eb84d7e 4 *
ansond 3:62b70ec1c2b1 5 * smokrani, sford, danson, sgrove
ansond 0:57bf3eb84d7e 6 *
ansond 0:57bf3eb84d7e 7 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
ansond 0:57bf3eb84d7e 8 * and associated documentation files (the "Software"), to deal in the Software without
ansond 0:57bf3eb84d7e 9 * restriction, including without limitation the rights to use, copy, modify, merge, publish,
ansond 0:57bf3eb84d7e 10 * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
ansond 0:57bf3eb84d7e 11 * Software is furnished to do so, subject to the following conditions:
ansond 0:57bf3eb84d7e 12 *
ansond 0:57bf3eb84d7e 13 * The above copyright notice and this permission notice shall be included in all copies or
ansond 0:57bf3eb84d7e 14 * substantial portions of the Software.
ansond 0:57bf3eb84d7e 15 *
ansond 0:57bf3eb84d7e 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
ansond 0:57bf3eb84d7e 17 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
ansond 0:57bf3eb84d7e 18 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
ansond 0:57bf3eb84d7e 19 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
ansond 0:57bf3eb84d7e 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
ansond 0:57bf3eb84d7e 21 */
ansond 0:57bf3eb84d7e 22
ansond 0:57bf3eb84d7e 23 //
ansond 0:57bf3eb84d7e 24 // DreamForce 2013 Challenge:
ansond 0:57bf3eb84d7e 25 //
ansond 1:9d7278f20f77 26 // Goal: modify the code below to adjust the sling body angle (theta) to take into account
ansond 0:57bf3eb84d7e 27 // the relative angle between the sling body and the sling band.
ansond 0:57bf3eb84d7e 28 //
ansond 0:57bf3eb84d7e 29 //
ansond 0:57bf3eb84d7e 30 // Mini-hack challenge: Your mission, should you choose to accept it, is to complete the function
ansond 0:57bf3eb84d7e 31 // "potentiometer_value_to_degrees()" below (around line 129) to return a reasonable
ansond 0:57bf3eb84d7e 32 // estimate of the sling band angle relative to the sling body
ansond 0:57bf3eb84d7e 33 //
ansond 0:57bf3eb84d7e 34 //
ansond 0:57bf3eb84d7e 35
ansond 0:57bf3eb84d7e 36 // DreamForce2013 TUNABLES START
ansond 0:57bf3eb84d7e 37
ansond 0:57bf3eb84d7e 38 // maximum angular value of the sling band
ansond 0:57bf3eb84d7e 39 float max_sling_angle = 30.0;
ansond 0:57bf3eb84d7e 40
ansond 0:57bf3eb84d7e 41 // minimum angluar value of the sling band
ansond 0:57bf3eb84d7e 42 float min_sling_angle = -30.0;
ansond 0:57bf3eb84d7e 43
ansond 0:57bf3eb84d7e 44 // DreamForce2013 TUNABLES END
ansond 0:57bf3eb84d7e 45
ansond 2:563ce68ef360 46 // Sling Tunables Start - !!! be careful changing these !!!
ansond 0:57bf3eb84d7e 47
ansond 0:57bf3eb84d7e 48 // stretch start threshold
ansond 0:57bf3eb84d7e 49 float stretch_start_threshold = 0.4;
ansond 0:57bf3eb84d7e 50
ansond 0:57bf3eb84d7e 51 // fire threshold
ansond 0:57bf3eb84d7e 52 float fire_threshold = 0.15;
ansond 0:57bf3eb84d7e 53
ansond 0:57bf3eb84d7e 54 // fire timing threshold
ansond 0:57bf3eb84d7e 55 float fire_timing_threshold = 3.0;
ansond 0:57bf3eb84d7e 56
ansond 0:57bf3eb84d7e 57 // scaling for mouse movement
ansond 0:57bf3eb84d7e 58 int mouse_scale = 230;
ansond 0:57bf3eb84d7e 59
ansond 0:57bf3eb84d7e 60 // Sling Tunables End
ansond 0:57bf3eb84d7e 61
ansond 0:57bf3eb84d7e 62 // definition of PI
ansond 0:57bf3eb84d7e 63 #define M_PI 3.14159
ansond 0:57bf3eb84d7e 64
ansond 0:57bf3eb84d7e 65 // Includes
ansond 0:57bf3eb84d7e 66 #include "mbed.h"
ansond 0:57bf3eb84d7e 67 #include "USBMouse.h"
ansond 0:57bf3eb84d7e 68 #include "ADXL345.h"
ansond 0:57bf3eb84d7e 69
ansond 0:57bf3eb84d7e 70 // Physical interfaces
ansond 0:57bf3eb84d7e 71 USBMouse mouse;
ansond 0:57bf3eb84d7e 72 ADXL345 accelerometer(p5, p6, p7, p8);
ansond 0:57bf3eb84d7e 73 AnalogIn stretch_sensor(p15);
ansond 0:57bf3eb84d7e 74 BusOut leds(LED1, LED2, LED3, LED4);
ansond 0:57bf3eb84d7e 75
ansond 2:563ce68ef360 76 // Potentiometer
ansond 0:57bf3eb84d7e 77 AnalogIn pot_1(p19);
ansond 0:57bf3eb84d7e 78
ansond 0:57bf3eb84d7e 79 // keep track of mouse position
ansond 0:57bf3eb84d7e 80 int current_x = 0;
ansond 0:57bf3eb84d7e 81 int current_y = 0;
ansond 0:57bf3eb84d7e 82
ansond 0:57bf3eb84d7e 83 // Potentiometer filters
ansond 0:57bf3eb84d7e 84 #include "filter.h"
ansond 0:57bf3eb84d7e 85 medianFilter prefilter(13);
ansond 0:57bf3eb84d7e 86 medianFilter postfilter(7);
ansond 0:57bf3eb84d7e 87
ansond 0:57bf3eb84d7e 88 // return radians for a given degree
ansond 0:57bf3eb84d7e 89 float degrees_to_radians(float degrees) {
ansond 0:57bf3eb84d7e 90 float radians = ((M_PI*degrees)/180.0);
ansond 0:57bf3eb84d7e 91 return radians;
ansond 0:57bf3eb84d7e 92 }
ansond 0:57bf3eb84d7e 93
ansond 0:57bf3eb84d7e 94 // return degrees for a given radian
ansond 0:57bf3eb84d7e 95 float radians_to_degrees(float radians) {
ansond 0:57bf3eb84d7e 96 float degrees = ((180*radians)/M_PI);
ansond 0:57bf3eb84d7e 97 return degrees;
ansond 0:57bf3eb84d7e 98 }
ansond 0:57bf3eb84d7e 99
ansond 0:57bf3eb84d7e 100 // get_potentiometer_value() reads the potentiometer, filters its value and remaps it to [0, 100.0]
ansond 0:57bf3eb84d7e 101 float get_potentiometer_value(AnalogIn pot) {
ansond 0:57bf3eb84d7e 102 float f = pot;
ansond 2:563ce68ef360 103 f = prefilter.process(f); // pre-filter
ansond 2:563ce68ef360 104 f = (f * 100); // remap: [ 0, 100]
ansond 2:563ce68ef360 105 return postfilter.process(f); // post-filter after remap
ansond 0:57bf3eb84d7e 106 }
ansond 0:57bf3eb84d7e 107
ansond 0:57bf3eb84d7e 108 //
ansond 0:57bf3eb84d7e 109 // DreamForce 2013 Challenge:
ansond 0:57bf3eb84d7e 110 // potentiometer_value_to_degrees() takes the potentiometer value (val_pot) and
ansond 0:57bf3eb84d7e 111 // maps it to an angle between [min_sling_angle, max_sling_angle] as defined in the tunables section
ansond 0:57bf3eb84d7e 112 //
ansond 0:57bf3eb84d7e 113 // NOTE: This function is INCOMPLETE. To complete it you should:
ansond 0:57bf3eb84d7e 114 // 1). Uncomment the debug statement, run the program, and look at raw potentiometer values
ansond 0:57bf3eb84d7e 115 // 2). Determine the min and max potentiometer values you wish to scale to degrees
ansond 0:57bf3eb84d7e 116 // 3). Determine the 90 degree potentiometer value ("median_pot") that denotes the sling band at 90 to the sling body
ansond 0:57bf3eb84d7e 117 // 4). Fill in median_pot, min_pot, max_pot below
ansond 0:57bf3eb84d7e 118 // 5). Compile up and give it a try
ansond 0:57bf3eb84d7e 119 //
ansond 0:57bf3eb84d7e 120 float potentiometer_value_to_degrees(float val_pot) {
ansond 0:57bf3eb84d7e 121 float deg = 0.0;
ansond 0:57bf3eb84d7e 122 float accuracy = 0.1;
ansond 0:57bf3eb84d7e 123
ansond 0:57bf3eb84d7e 124 // DEBUG - may need this to calibrate pot values below
ansond 0:57bf3eb84d7e 125 //std::printf("Raw pot value=%.1f\r\n",val_pot);
ansond 0:57bf3eb84d7e 126
ansond 0:57bf3eb84d7e 127 // Potentiometer range: typically about [36.8, 80.6] with 56.0 being "median_pot"
ansond 0:57bf3eb84d7e 128 float median_pot = 56.0;
ansond 0:57bf3eb84d7e 129 float min_pot = 36.8;
ansond 0:57bf3eb84d7e 130 float max_pot = 80.6;
ansond 0:57bf3eb84d7e 131 float incr_pot = (max_pot*10) - (min_pot*10); // how many .1 increments we have in the interval [min, max]
ansond 0:57bf3eb84d7e 132
ansond 0:57bf3eb84d7e 133 // Mapped degree range: approx [min_sling_angle, max_sling_angle] degrees so convert to
ansond 0:57bf3eb84d7e 134 float min_deg = min_sling_angle;
ansond 0:57bf3eb84d7e 135 float max_deg = max_sling_angle;
ansond 0:57bf3eb84d7e 136 float incr_deg = (max_deg*10) - (min_deg*10); // how many .1 increments we have in the interval [min, max]
ansond 0:57bf3eb84d7e 137
ansond 0:57bf3eb84d7e 138 // see if we are centered or not
ansond 0:57bf3eb84d7e 139 float centered_pot = fabs(val_pot - median_pot);
ansond 0:57bf3eb84d7e 140
ansond 0:57bf3eb84d7e 141 // if we are off 90 degrees (i.e. sling body and sling band are not at 90 degrees) - calculate the relative angle
ansond 0:57bf3eb84d7e 142 if (centered_pot > accuracy) {
ansond 0:57bf3eb84d7e 143 // map to degree range
ansond 0:57bf3eb84d7e 144 float conversion = (incr_deg/incr_pot);
ansond 0:57bf3eb84d7e 145 deg = min_deg + (conversion*(val_pot - min_pot));
ansond 0:57bf3eb84d7e 146 }
ansond 0:57bf3eb84d7e 147
ansond 0:57bf3eb84d7e 148 // we have to flip the sign of the result
ansond 0:57bf3eb84d7e 149 deg = -deg;
ansond 0:57bf3eb84d7e 150
ansond 0:57bf3eb84d7e 151 // return the calculated degrees
ansond 0:57bf3eb84d7e 152 return deg;
ansond 0:57bf3eb84d7e 153 }
ansond 0:57bf3eb84d7e 154
ansond 0:57bf3eb84d7e 155 // adjust the final angle (theta) taking into account the relative angle between the sling body and the sling band.
ansond 0:57bf3eb84d7e 156 float adjust_for_sling_angle(float slingshot_body_angle) {
ansond 2:563ce68ef360 157 // get the sling angle through approximation with the potentiometer
ansond 2:563ce68ef360 158 float sling_angle_degrees = potentiometer_value_to_degrees(get_potentiometer_value(pot_1));
ansond 0:57bf3eb84d7e 159
ansond 0:57bf3eb84d7e 160 // the sling angle is in degrees - so lets convert the body angle to degrees as well
ansond 0:57bf3eb84d7e 161 float modified_angle_degrees = radians_to_degrees(slingshot_body_angle);
ansond 0:57bf3eb84d7e 162
ansond 0:57bf3eb84d7e 163 // we simply add the sling angle to adjust it
ansond 0:57bf3eb84d7e 164 modified_angle_degrees += sling_angle_degrees;
ansond 0:57bf3eb84d7e 165
ansond 0:57bf3eb84d7e 166 // make sure that we are always between 0 and 359 degrees
ansond 0:57bf3eb84d7e 167 while (modified_angle_degrees > 360.0) modified_angle_degrees = modified_angle_degrees - 360;
ansond 0:57bf3eb84d7e 168 while (modified_angle_degrees < 0.0) modified_angle_degrees = modified_angle_degrees + 360;
ansond 0:57bf3eb84d7e 169
ansond 0:57bf3eb84d7e 170 // convert the modified angle back to radians
ansond 0:57bf3eb84d7e 171 float modified_angle_radians = degrees_to_radians(modified_angle_degrees);
ansond 0:57bf3eb84d7e 172
ansond 0:57bf3eb84d7e 173 // DEBUG
ansond 0:57bf3eb84d7e 174 //std::printf("adjust_for_sling_angle: body_angle=%.1f sling_angle=%.1f modified_angle=%.1f\r\n",radians_to_degrees(slingshot_body_angle),sling_angle_degrees,modified_angle_degrees);
ansond 0:57bf3eb84d7e 175
ansond 0:57bf3eb84d7e 176 // return the modified angle
ansond 0:57bf3eb84d7e 177 return modified_angle_radians;
ansond 0:57bf3eb84d7e 178 }
ansond 0:57bf3eb84d7e 179
ansond 0:57bf3eb84d7e 180 // Return slingshot angle in radians, up > 0 > down
ansond 0:57bf3eb84d7e 181 float get_angle() {
ansond 0:57bf3eb84d7e 182 int readings[3];
ansond 0:57bf3eb84d7e 183 accelerometer.getOutput(readings);
ansond 0:57bf3eb84d7e 184 float x = (int16_t)readings[0];
ansond 0:57bf3eb84d7e 185 float z = (int16_t)readings[2];
ansond 0:57bf3eb84d7e 186 return atan(z / x);
ansond 0:57bf3eb84d7e 187 }
ansond 0:57bf3eb84d7e 188
ansond 0:57bf3eb84d7e 189 // Return normalised stretch value based on bounds of all readings seen
ansond 0:57bf3eb84d7e 190 float get_stretch() {
ansond 0:57bf3eb84d7e 191 static float min_strength = 0.7;
ansond 0:57bf3eb84d7e 192 static float max_strength = 0.7;
ansond 0:57bf3eb84d7e 193 float current_strength = stretch_sensor.read();
ansond 0:57bf3eb84d7e 194 if(current_strength > max_strength) { max_strength = current_strength; }
ansond 0:57bf3eb84d7e 195 if(current_strength < min_strength) { min_strength = current_strength; }
ansond 0:57bf3eb84d7e 196 float stretch = (current_strength - min_strength) / (max_strength - min_strength);
ansond 0:57bf3eb84d7e 197 return 1.0 - stretch;
ansond 0:57bf3eb84d7e 198 }
ansond 0:57bf3eb84d7e 199
ansond 0:57bf3eb84d7e 200 // move mouse to a location relative to the start point, stepping as needed
ansond 0:57bf3eb84d7e 201 void move_mouse(int x, int y) {
ansond 0:57bf3eb84d7e 202 const int STEP = 10;
ansond 0:57bf3eb84d7e 203
ansond 0:57bf3eb84d7e 204 int move_x = x - current_x;
ansond 0:57bf3eb84d7e 205 int move_y = y - current_y;
ansond 0:57bf3eb84d7e 206
ansond 0:57bf3eb84d7e 207 // Move the mouse, in steps of max step size to ensure it is picked up by OS
ansond 0:57bf3eb84d7e 208 while(move_x > STEP) { mouse.move(STEP, 0); move_x -= STEP; }
ansond 0:57bf3eb84d7e 209 while(move_x < -STEP) { mouse.move(-STEP, 0); move_x += STEP; }
ansond 0:57bf3eb84d7e 210 while(move_y > STEP) { mouse.move(0, STEP); move_y -= STEP; }
ansond 0:57bf3eb84d7e 211 while(move_y < -STEP) { mouse.move(0, -STEP); move_y += STEP; }
ansond 0:57bf3eb84d7e 212 mouse.move(move_x, move_y);
ansond 0:57bf3eb84d7e 213
ansond 0:57bf3eb84d7e 214 current_x = x;
ansond 0:57bf3eb84d7e 215 current_y = y;
ansond 0:57bf3eb84d7e 216 }
ansond 0:57bf3eb84d7e 217
ansond 0:57bf3eb84d7e 218 // reset the mouse position
ansond 0:57bf3eb84d7e 219 void reset_mouse() {
ansond 0:57bf3eb84d7e 220 current_x = 0;
ansond 0:57bf3eb84d7e 221 current_y = 0;
ansond 0:57bf3eb84d7e 222 mouse.move(0,0);
ansond 0:57bf3eb84d7e 223 }
ansond 0:57bf3eb84d7e 224
ansond 0:57bf3eb84d7e 225 template <class T>
ansond 0:57bf3eb84d7e 226 T filter(T* array, int len, T value) {
ansond 0:57bf3eb84d7e 227 T mean = 0.0;
ansond 0:57bf3eb84d7e 228 for(int i = 0; i<len - 1; i++) {
ansond 0:57bf3eb84d7e 229 mean += array[i + 1];
ansond 0:57bf3eb84d7e 230 array[i] = array[i + 1];
ansond 0:57bf3eb84d7e 231 }
ansond 0:57bf3eb84d7e 232 mean += value;
ansond 0:57bf3eb84d7e 233 array[len - 1] = value;
ansond 0:57bf3eb84d7e 234 return mean / (T)len;
ansond 0:57bf3eb84d7e 235 }
ansond 0:57bf3eb84d7e 236
ansond 0:57bf3eb84d7e 237 typedef enum {
ansond 0:57bf3eb84d7e 238 WAITING = 2,
ansond 0:57bf3eb84d7e 239 AIMING = 4,
ansond 0:57bf3eb84d7e 240 FIRING = 8
ansond 0:57bf3eb84d7e 241 } state_t;
ansond 0:57bf3eb84d7e 242
ansond 0:57bf3eb84d7e 243 int main() {
ansond 0:57bf3eb84d7e 244 bool loop_forever = true;
ansond 0:57bf3eb84d7e 245 leds = 1;
ansond 0:57bf3eb84d7e 246
ansond 0:57bf3eb84d7e 247 // init mouse tracking
ansond 0:57bf3eb84d7e 248 reset_mouse();
ansond 0:57bf3eb84d7e 249
ansond 0:57bf3eb84d7e 250 // setup accelerometer
ansond 0:57bf3eb84d7e 251 accelerometer.setPowerControl(0x00);
ansond 0:57bf3eb84d7e 252 accelerometer.setDataFormatControl(0x0B);
ansond 0:57bf3eb84d7e 253 accelerometer.setDataRate(ADXL345_3200HZ);
ansond 0:57bf3eb84d7e 254 accelerometer.setPowerControl(0x08);
ansond 0:57bf3eb84d7e 255
ansond 0:57bf3eb84d7e 256 state_t state = WAITING;
ansond 0:57bf3eb84d7e 257 Timer timer;
ansond 0:57bf3eb84d7e 258
ansond 0:57bf3eb84d7e 259 float angles[8] = {0};
ansond 0:57bf3eb84d7e 260 float stretches[8] = {0};
ansond 0:57bf3eb84d7e 261
ansond 0:57bf3eb84d7e 262 while(loop_forever) {
ansond 0:57bf3eb84d7e 263 // get the slingshot parameters
ansond 0:57bf3eb84d7e 264 float this_stretch = get_stretch();
ansond 0:57bf3eb84d7e 265 float this_angle = get_angle();
ansond 0:57bf3eb84d7e 266
ansond 0:57bf3eb84d7e 267 // apply some filtering
ansond 0:57bf3eb84d7e 268 float stretch = filter(stretches, 8, this_stretch);
ansond 0:57bf3eb84d7e 269 float angle = filter(angles, 8, this_angle);
ansond 0:57bf3eb84d7e 270
ansond 0:57bf3eb84d7e 271 // DreamForce 2013 Challenge: Adjust the angle to account for the relative angle between the sling and the slingshot body
ansond 0:57bf3eb84d7e 272 angle = adjust_for_sling_angle(angle);
ansond 0:57bf3eb84d7e 273
ansond 0:57bf3eb84d7e 274 leds = state;
ansond 0:57bf3eb84d7e 275
ansond 0:57bf3eb84d7e 276 // act based on the current state
ansond 0:57bf3eb84d7e 277 switch (state) {
ansond 0:57bf3eb84d7e 278 case WAITING:
ansond 0:57bf3eb84d7e 279 if(stretch > stretch_start_threshold) { // significant stretch, considered starting
ansond 0:57bf3eb84d7e 280 mouse.press(MOUSE_LEFT);
ansond 0:57bf3eb84d7e 281 state = AIMING;
ansond 0:57bf3eb84d7e 282 }
ansond 0:57bf3eb84d7e 283 break;
ansond 0:57bf3eb84d7e 284
ansond 0:57bf3eb84d7e 285 case AIMING:
ansond 0:57bf3eb84d7e 286 if(stretch - this_stretch > fire_threshold) { // rapid de-stretch, considered a fire
ansond 0:57bf3eb84d7e 287 mouse.release(MOUSE_LEFT);
ansond 0:57bf3eb84d7e 288 reset_mouse();
ansond 0:57bf3eb84d7e 289 timer.start();
ansond 0:57bf3eb84d7e 290 state = FIRING;
ansond 0:57bf3eb84d7e 291 }
ansond 0:57bf3eb84d7e 292 else if(stretch < stretch_start_threshold) { // de-stretch
ansond 0:57bf3eb84d7e 293 reset_mouse();
ansond 0:57bf3eb84d7e 294 timer.stop();
ansond 0:57bf3eb84d7e 295 timer.reset();
ansond 0:57bf3eb84d7e 296 state = WAITING;
ansond 0:57bf3eb84d7e 297 } else {
ansond 0:57bf3eb84d7e 298 int x = 0.0 - cos(angle) * stretch * mouse_scale;
ansond 0:57bf3eb84d7e 299 int y = sin(angle) * stretch * mouse_scale;
ansond 0:57bf3eb84d7e 300 move_mouse(x, y);
ansond 0:57bf3eb84d7e 301 }
ansond 0:57bf3eb84d7e 302 break;
ansond 0:57bf3eb84d7e 303
ansond 0:57bf3eb84d7e 304 case FIRING:
ansond 0:57bf3eb84d7e 305 if(timer > fire_timing_threshold) {
ansond 0:57bf3eb84d7e 306 timer.stop();
ansond 0:57bf3eb84d7e 307 timer.reset();
ansond 0:57bf3eb84d7e 308 reset_mouse();
ansond 0:57bf3eb84d7e 309 state = WAITING;
ansond 0:57bf3eb84d7e 310 }
ansond 0:57bf3eb84d7e 311 break;
ansond 0:57bf3eb84d7e 312 };
ansond 0:57bf3eb84d7e 313
ansond 0:57bf3eb84d7e 314 // wait for 100ms
ansond 0:57bf3eb84d7e 315 wait_ms(100);
ansond 0:57bf3eb84d7e 316 }
ansond 0:57bf3eb84d7e 317 }