Alexander Rao / Mbed 2 deprecated object_counter

Counts points that hit a LIDAR sensor, and includes threshold sensing so a point doesn't get counted twice, much in the same spirit as debouncing.

Dependencies:   mbed mbed-rtos 4DGL-uLCD-SE X_NUCLEO_53L0A1

The Object Counter counts the number of objects (in our case, ping pong balls) that pass by the sensor. It features a high water mark that ensures only one ball is counted at any given time, and that a ball will only be counted once.

Partner Program

This is intended to be used with Ping Pong Robot, but can be used by itself if you wish

Installation

We use the following components:

All of these can be purchased from SparkFun with the exception of the VL53L0X sensor, wich is from AdaFruit.

To connect these, use the following pin outs:

Class D Amplifier & Speaker

MBEDClass D Audio AmpSpeaker
GndIN-
GndPWR-
5VPWR+
p25IN+
p20S
OUT-Speaker-
OUT+Speaker+

uLCD Screen

MBEDuLCD HeaderuLCD
GndGndGnd
+5V+5V+5V
p13TXRX
p14RXTX
p15RESRES

VL35L0X

MBEDVL35L0X
GndGnd
+3.3VVIN
p26SHDN
p27SDA
p28SCL

Usage

This is intended to be used to track the number of objects that have been collected. Because of this, the software features a "High Water Mark" mentality - after an object has gotten close enough, a flag is tripped - this flag only resets after the distance increases back past the high water mark. In this way, we ensure we count each object once - much in the same spirit as debouncing.

The VL53L0X is a low cost chip that measures distance via "Time of Flight" - basically how long it takes light to travel to the object and back. Because it uses a single laser, it can be quite precise. This makes it ideal for our purposes - tracking a single object as it falls.

Heads Up!

The largest issue is data collection time - while the chip does collect data quickly, if your object is moving at a high speed, it can sometimes miss the object entirely because it didn't "see" the object in the few fractions of a second that the object was close enough!

If you require more precision, there are plenty of higher quality (and higher cost) LIDAR systems - AdaFruit sells many of these.

As a workaround, ensure your object is not moving quickly - this can be accomplished by setting the LIDAR's side on the floor, so its line of sight is directly above the ground. When the object hits the ground, even if it bounces, it should be on the ground enough time for the LIDAR to pick it up. Slightly tilting the ground will make sure your object will roll away after falling.

As with everything else, experience is the best teacher - you might need to tweak the threshold in the code to get optimal results.

Demonstration

The circuit looks like this: /media/uploads/alexhrao/img_9521.jpeg

main.cpp@6:b720519bdfe8, 2019-04-18 (annotated)

Committer:
alexhrao
Date:
Thu Apr 18 21:33:13 2019 +0000
Revision:
6:b720519bdfe8
Parent:
5:b566bcb20d94 
Add capability for 2 players

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alexhrao 0:fc7ce4e596ff 1 #include "mbed.h"
alexhrao 0:fc7ce4e596ff 2 #include "rtos.h"
alexhrao 0:fc7ce4e596ff 3 #include <stdio.h>
alexhrao 0:fc7ce4e596ff 4 #include "uLCD_4DGL.h"
alexhrao 0:fc7ce4e596ff 5 #include "XNucleo53L0A1.h"
alexhrao 1:e82997c13013 6 #include <time.h>
alexhrao 0:fc7ce4e596ff 7
alexhrao 0:fc7ce4e596ff 8 #define VL53L0_I2C_SDA p28
alexhrao 0:fc7ce4e596ff 9 #define VL53L0_I2C_SCL p27
alexhrao 6:b720519bdfe8 10 #define INIT_TIME 90
alexhrao 6:b720519bdfe8 11 #define THRESHOLD 100
alexhrao 0:fc7ce4e596ff 12
alexhrao 2:54e1a301de72 13 // We need this for being able to reset the MBED (similar to watch dogs)
alexhrao 6:b720519bdfe8 14 extern "C" void mbed_reset();
alexhrao 0:fc7ce4e596ff 15
alexhrao 2:54e1a301de72 16 // Game Data
alexhrao 1:e82997c13013 17 volatile int num_points = 0;
alexhrao 6:b720519bdfe8 18 volatile int time_left = INIT_TIME;
alexhrao 0:fc7ce4e596ff 19 volatile bool is_point = false;
alexhrao 6:b720519bdfe8 20 volatile int player_1 = 0;
alexhrao 6:b720519bdfe8 21 volatile int player_2 = 0;
alexhrao 6:b720519bdfe8 22
alexhrao 6:b720519bdfe8 23 int player_1_color = GREEN + RED;
alexhrao 6:b720519bdfe8 24 int player_2_color = RED + (.707 * GREEN);
alexhrao 6:b720519bdfe8 25
alexhrao 6:b720519bdfe8 26 volatile int player_color = GREEN;
alexhrao 6:b720519bdfe8 27
alexhrao 6:b720519bdfe8 28
alexhrao 0:fc7ce4e596ff 29
alexhrao 0:fc7ce4e596ff 30 // LIDAR
alexhrao 0:fc7ce4e596ff 31 static XNucleo53L0A1* lidar = NULL;
alexhrao 0:fc7ce4e596ff 32 uint32_t dist;
alexhrao 0:fc7ce4e596ff 33 DigitalOut shdn(p26);
alexhrao 0:fc7ce4e596ff 34
alexhrao 0:fc7ce4e596ff 35 // LCD
alexhrao 0:fc7ce4e596ff 36 Mutex lcd_lock;
alexhrao 2:54e1a301de72 37 // **NOTE**: do NOT use p9, p10, p11
alexhrao 1:e82997c13013 38 // There is a bug in LIDAR system and uLCD system - they can't BOTH BE ON
alexhrao 1:e82997c13013 39 // I2C! It just so happens, p9 and p10 are I2C, so the systems will get
alexhrao 2:54e1a301de72 40 // confused, and you'll get weird hangs with the uLCD screen!
alexhrao 1:e82997c13013 41 uLCD_4DGL uLCD(p13, p14, p15); // serial tx, serial rx, reset pin;
alexhrao 0:fc7ce4e596ff 42
alexhrao 0:fc7ce4e596ff 43 // Speaker
alexhrao 0:fc7ce4e596ff 44 PwmOut speaker(p25);
alexhrao 0:fc7ce4e596ff 45
alexhrao 0:fc7ce4e596ff 46 // Threads
alexhrao 0:fc7ce4e596ff 47 Thread sound_thread;
alexhrao 0:fc7ce4e596ff 48 Thread time_thread;
alexhrao 0:fc7ce4e596ff 49 Thread points_thread;
alexhrao 0:fc7ce4e596ff 50
alexhrao 0:fc7ce4e596ff 51 // Debugging
alexhrao 2:54e1a301de72 52 Serial pc(USBTX, USBRX);
alexhrao 0:fc7ce4e596ff 53 DigitalOut err_led(LED1);
alexhrao 1:e82997c13013 54
alexhrao 0:fc7ce4e596ff 55 void time(void) {
alexhrao 0:fc7ce4e596ff 56 while (1) {
alexhrao 0:fc7ce4e596ff 57 // every second, change LED and update screen
alexhrao 0:fc7ce4e596ff 58 if (time_left > 0) {
alexhrao 0:fc7ce4e596ff 59 lcd_lock.lock();
alexhrao 6:b720519bdfe8 60 uLCD.locate(4, 6);
alexhrao 0:fc7ce4e596ff 61 uLCD.text_height(2);
alexhrao 0:fc7ce4e596ff 62 uLCD.text_width(2);
alexhrao 1:e82997c13013 63 uLCD.color(RED);
alexhrao 6:b720519bdfe8 64 uLCD.printf("%03d", time_left);
alexhrao 0:fc7ce4e596ff 65 lcd_lock.unlock();
alexhrao 0:fc7ce4e596ff 66 Thread::wait(1000);
alexhrao 1:e82997c13013 67 time_left--;
alexhrao 0:fc7ce4e596ff 68 } else {
alexhrao 0:fc7ce4e596ff 69 break;
alexhrao 0:fc7ce4e596ff 70 }
alexhrao 0:fc7ce4e596ff 71 }
alexhrao 0:fc7ce4e596ff 72 lcd_lock.lock();
alexhrao 6:b720519bdfe8 73 uLCD.locate(4, 6);
alexhrao 0:fc7ce4e596ff 74 uLCD.text_height(2);
alexhrao 0:fc7ce4e596ff 75 uLCD.text_width(2);
alexhrao 1:e82997c13013 76 uLCD.color(RED);
alexhrao 6:b720519bdfe8 77 uLCD.printf("000");
alexhrao 0:fc7ce4e596ff 78 lcd_lock.unlock();
alexhrao 2:54e1a301de72 79 // We're done - thread will die
alexhrao 0:fc7ce4e596ff 80 }
alexhrao 0:fc7ce4e596ff 81 void sound(void) {
alexhrao 0:fc7ce4e596ff 82 int is_new = 1;
alexhrao 6:b720519bdfe8 83 speaker.period(1.0 / 500.0);
alexhrao 6:b720519bdfe8 84 speaker = 0.5;
alexhrao 6:b720519bdfe8 85 Thread::wait(250);
alexhrao 6:b720519bdfe8 86 speaker = 0.0;
alexhrao 6:b720519bdfe8 87 Thread::wait(100);
alexhrao 6:b720519bdfe8 88 speaker = 0.5;
alexhrao 6:b720519bdfe8 89 Thread::wait(250);
alexhrao 6:b720519bdfe8 90 speaker = 0.0;
alexhrao 0:fc7ce4e596ff 91 speaker.period(1.0 / 900.0);
alexhrao 0:fc7ce4e596ff 92 while (1) {
alexhrao 2:54e1a301de72 93 // if we have a point, sound
alexhrao 2:54e1a301de72 94 // If there's no more time left, die
alexhrao 2:54e1a301de72 95 if (time_left <= 0) {
alexhrao 2:54e1a301de72 96 break;
alexhrao 2:54e1a301de72 97 } else if (is_point && is_new == 1) {
alexhrao 0:fc7ce4e596ff 98 is_new = 0;
alexhrao 0:fc7ce4e596ff 99 // sound that
alexhrao 1:e82997c13013 100 speaker = 0.5;
alexhrao 1:e82997c13013 101 Thread::wait(100);
alexhrao 1:e82997c13013 102 speaker = 0;
alexhrao 0:fc7ce4e596ff 103 } else if (!is_point) {
alexhrao 2:54e1a301de72 104 // We've engaged, get ready for next
alexhrao 0:fc7ce4e596ff 105 is_new = 1;
alexhrao 0:fc7ce4e596ff 106 }
alexhrao 0:fc7ce4e596ff 107 Thread::wait(10);
alexhrao 0:fc7ce4e596ff 108 }
alexhrao 6:b720519bdfe8 109 speaker.period(1.0 / 100.0);
alexhrao 6:b720519bdfe8 110 speaker = 0.5;
alexhrao 6:b720519bdfe8 111 Thread::wait(2000);
alexhrao 6:b720519bdfe8 112 speaker = 0.0;
alexhrao 0:fc7ce4e596ff 113 }
alexhrao 0:fc7ce4e596ff 114
alexhrao 1:e82997c13013 115 void points(void) {
alexhrao 1:e82997c13013 116 int prev_points = -1;
alexhrao 5:b566bcb20d94 117 lcd_lock.lock();
alexhrao 6:b720519bdfe8 118 uLCD.circle(110, 10, 10, player_color);
alexhrao 5:b566bcb20d94 119 lcd_lock.unlock();
alexhrao 0:fc7ce4e596ff 120 while (1) {
alexhrao 2:54e1a301de72 121 if (time_left <= 0) {
alexhrao 1:e82997c13013 122 break;
alexhrao 2:54e1a301de72 123 } else if (num_points != prev_points) {
alexhrao 0:fc7ce4e596ff 124 lcd_lock.lock();
alexhrao 6:b720519bdfe8 125 uLCD.locate(4, 3);
alexhrao 0:fc7ce4e596ff 126 uLCD.text_width(2);
alexhrao 0:fc7ce4e596ff 127 uLCD.text_height(2);
alexhrao 6:b720519bdfe8 128 uLCD.color(player_color);
alexhrao 1:e82997c13013 129 uLCD.printf("%d", num_points);
alexhrao 0:fc7ce4e596ff 130 lcd_lock.unlock();
alexhrao 1:e82997c13013 131 prev_points = num_points;
alexhrao 0:fc7ce4e596ff 132 }
alexhrao 6:b720519bdfe8 133 if (is_point) {
alexhrao 3:efb572ef5f15 134 // changed from no to yes
alexhrao 3:efb572ef5f15 135 lcd_lock.lock();
alexhrao 6:b720519bdfe8 136 uLCD.filled_circle(110, 10, 9, player_color);
alexhrao 3:efb572ef5f15 137 lcd_lock.unlock();
alexhrao 6:b720519bdfe8 138 } else {
alexhrao 3:efb572ef5f15 139 lcd_lock.lock();
alexhrao 5:b566bcb20d94 140 uLCD.filled_circle(110, 10, 9, BLACK);
alexhrao 3:efb572ef5f15 141 lcd_lock.unlock();
alexhrao 3:efb572ef5f15 142 }
alexhrao 6:b720519bdfe8 143 Thread::wait(10);
alexhrao 0:fc7ce4e596ff 144 }
alexhrao 0:fc7ce4e596ff 145 }
alexhrao 0:fc7ce4e596ff 146
alexhrao 6:b720519bdfe8 147 void run_game() {
alexhrao 6:b720519bdfe8 148 // Initialize
alexhrao 6:b720519bdfe8 149 time_left = INIT_TIME;
alexhrao 6:b720519bdfe8 150 is_point = false;
alexhrao 6:b720519bdfe8 151
alexhrao 6:b720519bdfe8 152 uLCD.text_width(2);
alexhrao 6:b720519bdfe8 153 uLCD.text_height(2);
alexhrao 6:b720519bdfe8 154 uLCD.locate(1, 2);
alexhrao 6:b720519bdfe8 155 uLCD.color(player_color);
alexhrao 6:b720519bdfe8 156 uLCD.printf("POINTS");
alexhrao 6:b720519bdfe8 157
alexhrao 6:b720519bdfe8 158 uLCD.locate(2, 5);
alexhrao 6:b720519bdfe8 159 uLCD.color(RED);
alexhrao 6:b720519bdfe8 160 uLCD.printf("TIME");
alexhrao 6:b720519bdfe8 161
alexhrao 6:b720519bdfe8 162 // Start up threads
alexhrao 6:b720519bdfe8 163 time_thread.start(time);
alexhrao 6:b720519bdfe8 164 points_thread.start(points);
alexhrao 6:b720519bdfe8 165 sound_thread.start(sound);
alexhrao 6:b720519bdfe8 166
alexhrao 6:b720519bdfe8 167 // Continually measure distance
alexhrao 6:b720519bdfe8 168 Thread::wait(2000);
alexhrao 6:b720519bdfe8 169 while (1) {
alexhrao 6:b720519bdfe8 170 if (time_left == 0)
alexhrao 6:b720519bdfe8 171 break;
alexhrao 6:b720519bdfe8 172 int status = lidar->sensor_centre->get_distance(&dist);
alexhrao 6:b720519bdfe8 173 if (status == VL53L0X_ERROR_NONE) {
alexhrao 6:b720519bdfe8 174
alexhrao 6:b720519bdfe8 175 // Determine if we've hit the threshold (4 cases):
alexhrao 6:b720519bdfe8 176 // 1. NOT in the threshold, current distance is above
alexhrao 6:b720519bdfe8 177 // 2. NOT in the threshold, current distance is below
alexhrao 6:b720519bdfe8 178 // 3. IN the threshold, current distance is above
alexhrao 6:b720519bdfe8 179 // 4. IN the threshold, current distance is below
alexhrao 6:b720519bdfe8 180 //pc.printf("D=%ld mm\r\n", dist);
alexhrao 6:b720519bdfe8 181 if (!is_point && dist <= THRESHOLD) {
alexhrao 6:b720519bdfe8 182 // We don't need to check this every time - save our previous
alexhrao 6:b720519bdfe8 183 // loop time!
alexhrao 6:b720519bdfe8 184 if (time_left == 0)
alexhrao 6:b720519bdfe8 185 break;
alexhrao 6:b720519bdfe8 186 // increment our points
alexhrao 6:b720519bdfe8 187 ++num_points;
alexhrao 6:b720519bdfe8 188 is_point = true;
alexhrao 6:b720519bdfe8 189 } else if (is_point && dist > THRESHOLD) {
alexhrao 6:b720519bdfe8 190 is_point = false;
alexhrao 6:b720519bdfe8 191 }
alexhrao 6:b720519bdfe8 192 } else {
alexhrao 6:b720519bdfe8 193 is_point = false;
alexhrao 6:b720519bdfe8 194 }
alexhrao 6:b720519bdfe8 195 }
alexhrao 6:b720519bdfe8 196 // wait for time_left (should be handled above, but just in case?
alexhrao 6:b720519bdfe8 197 }
alexhrao 0:fc7ce4e596ff 198 int main() {
alexhrao 0:fc7ce4e596ff 199 // Tell the user we're spinning up
alexhrao 0:fc7ce4e596ff 200 uLCD.cls();
alexhrao 0:fc7ce4e596ff 201 uLCD.text_height(2);
alexhrao 0:fc7ce4e596ff 202 uLCD.text_width(2);
alexhrao 2:54e1a301de72 203 uLCD.locate(0, 0);
alexhrao 0:fc7ce4e596ff 204 uLCD.color(RED);
alexhrao 2:54e1a301de72 205 uLCD.printf("SPINNING\nUP");
alexhrao 2:54e1a301de72 206
alexhrao 0:fc7ce4e596ff 207 DevI2C* device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
alexhrao 0:fc7ce4e596ff 208 lidar = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
alexhrao 1:e82997c13013 209
alexhrao 0:fc7ce4e596ff 210 // Reset shdn (?):
alexhrao 0:fc7ce4e596ff 211 shdn = 0;
alexhrao 0:fc7ce4e596ff 212 wait(0.1);
alexhrao 0:fc7ce4e596ff 213 shdn = 1;
alexhrao 0:fc7ce4e596ff 214 wait(0.1);
alexhrao 0:fc7ce4e596ff 215
alexhrao 0:fc7ce4e596ff 216 int status = lidar->init_board();
alexhrao 0:fc7ce4e596ff 217 while (status) {
alexhrao 2:54e1a301de72 218 err_led = 1;
alexhrao 0:fc7ce4e596ff 219 status = lidar->init_board();
alexhrao 0:fc7ce4e596ff 220 wait(1);
alexhrao 0:fc7ce4e596ff 221 }
alexhrao 2:54e1a301de72 222 err_led = 0;
alexhrao 6:b720519bdfe8 223 // We have nothing left to do... Just wait
alexhrao 6:b720519bdfe8 224 // Wait for the LIDAR
alexhrao 6:b720519bdfe8 225 player_color = player_1_color;
alexhrao 1:e82997c13013 226 uLCD.cls();
alexhrao 1:e82997c13013 227 uLCD.locate(0, 0);
alexhrao 6:b720519bdfe8 228 uLCD.color(player_color);
alexhrao 1:e82997c13013 229 uLCD.text_height(2);
alexhrao 1:e82997c13013 230 uLCD.text_width(2);
alexhrao 6:b720519bdfe8 231 uLCD.printf("Ready\nPlayer\n1");
alexhrao 2:54e1a301de72 232
alexhrao 4:44b4158fc494 233 // Wait for the LIDAR
alexhrao 3:efb572ef5f15 234 while(true){
alexhrao 3:efb572ef5f15 235 if(lidar->sensor_centre->get_distance(&dist) == VL53L0X_ERROR_NONE){
alexhrao 6:b720519bdfe8 236 if(dist<THRESHOLD){
alexhrao 6:b720519bdfe8 237 break;
alexhrao 6:b720519bdfe8 238 }
alexhrao 6:b720519bdfe8 239 }
alexhrao 6:b720519bdfe8 240 }
alexhrao 6:b720519bdfe8 241 uLCD.cls();
alexhrao 6:b720519bdfe8 242 uLCD.locate(0, 0);
alexhrao 6:b720519bdfe8 243 uLCD.color(player_color);
alexhrao 6:b720519bdfe8 244 uLCD.text_width(2);
alexhrao 6:b720519bdfe8 245 uLCD.text_height(2);
alexhrao 6:b720519bdfe8 246 uLCD.printf("P1");
alexhrao 6:b720519bdfe8 247 run_game();
alexhrao 6:b720519bdfe8 248 player_1 = num_points;
alexhrao 6:b720519bdfe8 249 Thread::wait(5000);
alexhrao 6:b720519bdfe8 250 player_color = player_2_color;
alexhrao 6:b720519bdfe8 251 num_points = 0;
alexhrao 6:b720519bdfe8 252 lcd_lock.lock();
alexhrao 6:b720519bdfe8 253 uLCD.cls();
alexhrao 6:b720519bdfe8 254 uLCD.locate(0, 0);
alexhrao 6:b720519bdfe8 255 uLCD.color(player_color);
alexhrao 6:b720519bdfe8 256 uLCD.text_height(2);
alexhrao 6:b720519bdfe8 257 uLCD.text_width(2);
alexhrao 6:b720519bdfe8 258 uLCD.printf("Ready\nPlayer\n2");
alexhrao 6:b720519bdfe8 259 lcd_lock.unlock();
alexhrao 6:b720519bdfe8 260 // Wait for the LIDAR
alexhrao 6:b720519bdfe8 261 while(true){
alexhrao 6:b720519bdfe8 262 if(lidar->sensor_centre->get_distance(&dist) == VL53L0X_ERROR_NONE){
alexhrao 6:b720519bdfe8 263 if(dist<THRESHOLD){
alexhrao 3:efb572ef5f15 264 break;
alexhrao 3:efb572ef5f15 265 }
alexhrao 3:efb572ef5f15 266 }
alexhrao 3:efb572ef5f15 267 }
alexhrao 1:e82997c13013 268 uLCD.cls();
alexhrao 6:b720519bdfe8 269 uLCD.locate(0, 0);
alexhrao 6:b720519bdfe8 270 uLCD.color(player_color);
alexhrao 1:e82997c13013 271 uLCD.text_width(2);
alexhrao 1:e82997c13013 272 uLCD.text_height(2);
alexhrao 6:b720519bdfe8 273 uLCD.printf("P2");
alexhrao 6:b720519bdfe8 274 run_game();
alexhrao 6:b720519bdfe8 275 player_2 = num_points;
alexhrao 2:54e1a301de72 276
alexhrao 6:b720519bdfe8 277 lcd_lock.lock();
alexhrao 6:b720519bdfe8 278 // Lock foever
alexhrao 6:b720519bdfe8 279 uLCD.cls();
alexhrao 6:b720519bdfe8 280 uLCD.color(GREEN);
alexhrao 6:b720519bdfe8 281 uLCD.locate(0, 0);
alexhrao 6:b720519bdfe8 282 uLCD.text_width(3);
alexhrao 6:b720519bdfe8 283 uLCD.text_height(3);
alexhrao 6:b720519bdfe8 284 if (player_1 > player_2) {
alexhrao 6:b720519bdfe8 285 uLCD.text_string("Player", 0, 0, FONT_7X8, player_1_color);
alexhrao 6:b720519bdfe8 286 uLCD.text_string("1", 0, 1, FONT_7X8, player_1_color);
alexhrao 6:b720519bdfe8 287 uLCD.text_string("Wins!", 0, 2, FONT_7X8, player_1_color);
alexhrao 6:b720519bdfe8 288 } else if (player_2 > player_1) {
alexhrao 6:b720519bdfe8 289 uLCD.text_string("Player", 0, 0, FONT_7X8, player_2_color);
alexhrao 6:b720519bdfe8 290 uLCD.text_string("2", 0, 1, FONT_7X8, player_2_color);
alexhrao 6:b720519bdfe8 291 uLCD.text_string("Wins!", 0, 2, FONT_7X8, player_2_color);
alexhrao 6:b720519bdfe8 292 } else {
alexhrao 6:b720519bdfe8 293 uLCD.printf("Draw!");
alexhrao 6:b720519bdfe8 294 }
alexhrao 6:b720519bdfe8 295 uLCD.locate(0, 3);
alexhrao 6:b720519bdfe8 296 uLCD.color(player_1_color);
alexhrao 6:b720519bdfe8 297 uLCD.printf("P1: %d", player_1);
alexhrao 6:b720519bdfe8 298 uLCD.locate(0, 4);
alexhrao 6:b720519bdfe8 299 uLCD.color(player_2_color);
alexhrao 6:b720519bdfe8 300 uLCD.printf("P2: %d", player_2);
alexhrao 6:b720519bdfe8 301 lcd_lock.unlock();
alexhrao 6:b720519bdfe8 302
alexhrao 6:b720519bdfe8 303 Thread::wait(5000);
alexhrao 6:b720519bdfe8 304 while(true){
alexhrao 6:b720519bdfe8 305 if(lidar->sensor_centre->get_distance(&dist) == VL53L0X_ERROR_NONE){
alexhrao 6:b720519bdfe8 306 if(dist<THRESHOLD){
alexhrao 6:b720519bdfe8 307 break;
alexhrao 1:e82997c13013 308 }
alexhrao 0:fc7ce4e596ff 309 }
alexhrao 0:fc7ce4e596ff 310 }
alexhrao 6:b720519bdfe8 311 mbed_reset();
alexhrao 2:54e1a301de72 312 }