People Counter / Mbed 2 deprecated person_counter

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 mbed-rtos 4DGL-uLCD-SE HC_SR04_Ultrasonic_Library

main.cpp@24:fd113f826c98, 2020-04-27 (annotated)

Committer:
mmarine3
Date:
Mon Apr 27 21:58:28 2020 +0000
Revision:
24:fd113f826c98
Parent:
23:f877b13f22b7
Child:
25:577a78b97309
Changed allZero Algorithm to check for 3 non_zero values. Also added zero check to movingAway and movingToward to further prevent noise

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mmarine3 2:1ec843c95914 1 #include "mbed.h"
rmalik8 4:7d02b3f3bee6 2 #include "rtos.h"
mmarine3 17:b8b68ff5e33a 3 #include "SDFileSystem.h"
mmarine3 2:1ec843c95914 4 #include "uLCD_4DGL.h"
rmalik8 4:7d02b3f3bee6 5 #include "ultrasonic.h"
mmarine3 17:b8b68ff5e33a 6 #include "wave_player.h"
rmalik8 12:bebfef8d229f 7 #include "SongPlayer.h"
rmalik8 4:7d02b3f3bee6 8
rmalik8 3:b5cdd40e99e9 9 //#include <stdio.h>
mmarine3 2:1ec843c95914 10 #include <string>
mmarine3 2:1ec843c95914 11
rmalik8 4:7d02b3f3bee6 12
mmarine3 9:f4f03767acc0 13 uLCD_4DGL uLCD(p13, p14, p12); // Initialize uLCD serial tx, serial rx, reset pin;
mmarine3 17:b8b68ff5e33a 14 SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
rmalik8 10:7a939a54515e 15 RawSerial blue(p9,p10); //Initialize Blutooth
rmalik8 12:bebfef8d229f 16 //Serial BT(p9,p10);
rmalik8 12:bebfef8d229f 17
rmalik8 12:bebfef8d229f 18 //waiting to figure out if sd card works
mmarine3 20:1dc516a6a941 19 AnalogOut speaker(p18); //Initialize speaker
mmarine3 20:1dc516a6a941 20 wave_player waver(&speaker); //Initialize Waveplayer
rmalik8 12:bebfef8d229f 21
rmalik8 10:7a939a54515e 22 Mutex serial_mutex;
mmarine3 8:c0a6a3363e43 23 Mutex dist;
mmarine3 2:1ec843c95914 24 DigitalOut MyLED(LED1);
mmarine3 2:1ec843c95914 25
rmalik8 10:7a939a54515e 26 volatile int musicChoice = 0;
rmalik8 10:7a939a54515e 27 //musicChoice can be values 0 to 3
rmalik8 11:f5d0c8bf1849 28 volatile int sdist = 0;
rmalik8 12:bebfef8d229f 29 volatile int counter = 0;
mmarine3 21:4fef6bf7582c 30 volatile int numTowards = 0;
mmarine3 21:4fef6bf7582c 31 volatile int numAway = 0;
rmalik8 12:bebfef8d229f 32
rmalik8 12:bebfef8d229f 33 float note1[2]= {1568.0, 0.0
rmalik8 12:bebfef8d229f 34 };
rmalik8 13:3ba367bc16bc 35 float note2[2]= {1396.9, 0.0
rmalik8 12:bebfef8d229f 36 };
rmalik8 13:3ba367bc16bc 37 float note3[2]= {1244.5, 0.0
rmalik8 12:bebfef8d229f 38 };
mmarine3 20:1dc516a6a941 39 float duration[2]= {0.5, 0.0
rmalik8 12:bebfef8d229f 40 };
rmalik8 15:827fbc2e07d6 41
rmalik8 15:827fbc2e07d6 42 volatile int values[5] = {0,0,0,0,0};
rmalik8 15:827fbc2e07d6 43 volatile int index = 0;
mmarine3 20:1dc516a6a941 44 volatile float volume = 1.0;
rmalik8 15:827fbc2e07d6 45 volatile bool allZero = true;
mmarine3 19:c78aafa613b8 46 volatile bool movingTowards = false;
mmarine3 19:c78aafa613b8 47 volatile bool movingAway = false;
rmalik8 15:827fbc2e07d6 48 int state = 0;
mmarine3 18:7b1d65b007ab 49 int non_zeros = 0;
rmalik8 12:bebfef8d229f 50
rmalik8 12:bebfef8d229f 51
mmarine3 9:f4f03767acc0 52 void sonar(int distance)
rmalik8 4:7d02b3f3bee6 53 {
rmalik8 4:7d02b3f3bee6 54 //put code here to execute when the sonar distance has changed
mmarine3 9:f4f03767acc0 55 dist.lock();
mmarine3 9:f4f03767acc0 56 sdist = distance;
rmalik8 15:827fbc2e07d6 57 values[index] = distance;
mmarine3 9:f4f03767acc0 58 dist.unlock();
rmalik8 15:827fbc2e07d6 59
rmalik8 16:5250c9fe0408 60 //fill array with 5 values
rmalik8 15:827fbc2e07d6 61 index++;
rmalik8 15:827fbc2e07d6 62 if (index > 4)
rmalik8 15:827fbc2e07d6 63 {
rmalik8 15:827fbc2e07d6 64 index = 0;
rmalik8 15:827fbc2e07d6 65 }
rmalik8 15:827fbc2e07d6 66
mmarine3 18:7b1d65b007ab 67 //check if more than one value is zero (noise cancellation)
rmalik8 15:827fbc2e07d6 68 allZero = true;
mmarine3 19:c78aafa613b8 69 movingTowards = false;//NEW CODE
mmarine3 19:c78aafa613b8 70 movingAway = false;// NEW CODE
mmarine3 18:7b1d65b007ab 71 for (int i = 0; i < 5; i++) //Walk the list
mmarine3 18:7b1d65b007ab 72 {
mmarine3 18:7b1d65b007ab 73 if (values[i] != 0) //If value is not zero
rmalik8 15:827fbc2e07d6 74 {
mmarine3 18:7b1d65b007ab 75 ++non_zeros; //Incriment non_zero counter
mmarine3 18:7b1d65b007ab 76 //allZero = false;
mmarine3 24:fd113f826c98 77 if(non_zeros > 2) //If there is more than one non zero
mmarine3 18:7b1d65b007ab 78 {
mmarine3 18:7b1d65b007ab 79 allZero = false; //Change the allZero flag to false
mmarine3 18:7b1d65b007ab 80 }
rmalik8 15:827fbc2e07d6 81 }
mmarine3 19:c78aafa613b8 82 //NEW CODE
mmarine3 24:fd113f826c98 83 if(values[i] < values[(i+1)%5] && values[(i+1)%5] < values[(i+2)%5] && values[i] !=0)
mmarine3 19:c78aafa613b8 84 {
mmarine3 23:f877b13f22b7 85 movingAway = true; //Person moving away from sensor
mmarine3 19:c78aafa613b8 86 }
mmarine3 24:fd113f826c98 87 if (values[i] > values[(i+1)%5] && values[(i+1)%5] > values[(i+2)%5] && values[(i+2)%5] !=0)
mmarine3 19:c78aafa613b8 88 {
mmarine3 23:f877b13f22b7 89 movingTowards = true; //Person moving to the sensor
mmarine3 19:c78aafa613b8 90 }
mmarine3 19:c78aafa613b8 91 //
rmalik8 15:827fbc2e07d6 92 }
mmarine3 18:7b1d65b007ab 93 non_zeros = 0;
mmarine3 18:7b1d65b007ab 94
rmalik8 12:bebfef8d229f 95 //printf("Sonar Distance:\r\n %d", sdist);
mmarine3 9:f4f03767acc0 96 }
rmalik8 4:7d02b3f3bee6 97
mmarine3 9:f4f03767acc0 98 ultrasonic mu(p30, p29, .1, 1, &sonar); //Set the trigger pin to p30 and the echo pin to p29
rmalik8 12:bebfef8d229f 99 //have updates every .1 seconds and a timeout after 1
rmalik8 12:bebfef8d229f 100 //second, and call sonar when the distance changes
mmarine3 9:f4f03767acc0 101
mmarine3 9:f4f03767acc0 102 void Sonar(void const* arguments)
mmarine3 9:f4f03767acc0 103 {
mmarine3 9:f4f03767acc0 104 mu.startUpdates();//start measuring the distance
rmalik8 12:bebfef8d229f 105 while(1) {
mmarine3 9:f4f03767acc0 106 //Do something else here
mmarine3 9:f4f03767acc0 107 mu.checkDistance(); //call checkDistance() as much as possible, as this is where
rmalik8 12:bebfef8d229f 108 //the class checks if dist needs to be called.
mmarine3 9:f4f03767acc0 109 Thread::wait(10);
rmalik8 4:7d02b3f3bee6 110 }
rmalik8 4:7d02b3f3bee6 111 }
mmarine3 2:1ec843c95914 112
mmarine3 8:c0a6a3363e43 113 //Thread to print the TOF and Sonar Values to the LCD
mmarine3 8:c0a6a3363e43 114 void LCD(void const *arguments)
mmarine3 8:c0a6a3363e43 115 {
mmarine3 8:c0a6a3363e43 116 Thread::wait(1000); //Wait for lidar and sonar setup
rmalik8 12:bebfef8d229f 117 serial_mutex.lock();
rmalik8 12:bebfef8d229f 118 uLCD.cls();
rmalik8 12:bebfef8d229f 119 uLCD.baudrate(BAUD_3000000);
mmarine3 21:4fef6bf7582c 120 uLCD.printf("Sonar Dist:\nTowards: \nAway: \nCounter: \nSong: \n");
rmalik8 12:bebfef8d229f 121 serial_mutex.unlock();
mmarine3 21:4fef6bf7582c 122 char str1[4];
mmarine3 21:4fef6bf7582c 123
rmalik8 12:bebfef8d229f 124 while(1) {
rmalik8 10:7a939a54515e 125 serial_mutex.lock();
mmarine3 21:4fef6bf7582c 126 //uLCD.cls();
mmarine3 8:c0a6a3363e43 127 dist.lock();
mmarine3 21:4fef6bf7582c 128 //uLCD.filled_rectangle(90,0,128,40,BLACK);
mmarine3 23:f877b13f22b7 129 sprintf(str1,"%d ",sdist);
mmarine3 21:4fef6bf7582c 130 uLCD.text_string(str1,12,0,FONT_5X7,GREEN);
mmarine3 21:4fef6bf7582c 131 sprintf(str1,"%d ",numTowards);
mmarine3 21:4fef6bf7582c 132 uLCD.text_string(str1,12,1,FONT_5X7,GREEN);
mmarine3 21:4fef6bf7582c 133 sprintf(str1,"%d ",numAway);
mmarine3 21:4fef6bf7582c 134 uLCD.text_string(str1,12,2,FONT_5X7,GREEN);
mmarine3 21:4fef6bf7582c 135 sprintf(str1,"%d ",counter);
mmarine3 21:4fef6bf7582c 136 uLCD.text_string(str1,12,3,FONT_5X7,GREEN);
mmarine3 21:4fef6bf7582c 137 sprintf(str1,"%d ",musicChoice);
mmarine3 21:4fef6bf7582c 138 uLCD.text_string(str1,12,4,FONT_5X7,GREEN);
mmarine3 21:4fef6bf7582c 139 //uLCD.printf("Sonar Dist:%d\nTowards: %d\nAway: %d\nCounter: %d \nSong: %d\n", sdist,numTowards,numAway, counter, musicChoice);
mmarine3 8:c0a6a3363e43 140 dist.unlock();
rmalik8 10:7a939a54515e 141 serial_mutex.unlock();
mmarine3 17:b8b68ff5e33a 142 Thread::wait(100); //Allow time to read value before reprint
rmalik8 12:bebfef8d229f 143
rmalik8 12:bebfef8d229f 144 }
rmalik8 12:bebfef8d229f 145 }
rmalik8 12:bebfef8d229f 146
rmalik8 12:bebfef8d229f 147 void sounds(void const *arguments)
rmalik8 12:bebfef8d229f 148 {
rmalik8 12:bebfef8d229f 149 int last_counter = 0;
mmarine3 17:b8b68ff5e33a 150 //FILE *wave_file;
rmalik8 12:bebfef8d229f 151 // setup instance of new SongPlayer class, mySpeaker using pin 26
rmalik8 12:bebfef8d229f 152 // the pin must be a PWM output pin
rmalik8 12:bebfef8d229f 153 SongPlayer mySpeaker(p26);
rmalik8 12:bebfef8d229f 154 // Start song and return once playing starts
rmalik8 12:bebfef8d229f 155 //mySpeaker.PlaySong(note1,duration);
rmalik8 12:bebfef8d229f 156 while(1) {
mmarine3 19:c78aafa613b8 157 if (counter != last_counter) {//CHANGE > to !=
rmalik8 12:bebfef8d229f 158 switch (musicChoice) {
mmarine3 17:b8b68ff5e33a 159 case 0:
mmarine3 17:b8b68ff5e33a 160 printf("Beep\r\n");
rmalik8 12:bebfef8d229f 161 //do nothing
rmalik8 12:bebfef8d229f 162 break;
mmarine3 17:b8b68ff5e33a 163 case 1:
mmarine3 17:b8b68ff5e33a 164 //wave_file=fopen("/sd/test.wav","r");
mmarine3 17:b8b68ff5e33a 165 //if(wave_file==NULL) {printf("file open error!\n\n\r");}
mmarine3 17:b8b68ff5e33a 166 //waver.play(wave_file);
mmarine3 17:b8b68ff5e33a 167 //fclose(wave_file);
mmarine3 20:1dc516a6a941 168 mySpeaker.PlaySong(note1,duration,volume);
rmalik8 12:bebfef8d229f 169 break;
mmarine3 17:b8b68ff5e33a 170 case 2:
mmarine3 20:1dc516a6a941 171 mySpeaker.PlaySong(note2,duration,volume);
rmalik8 12:bebfef8d229f 172 break;
mmarine3 17:b8b68ff5e33a 173 case 3:
mmarine3 20:1dc516a6a941 174 mySpeaker.PlaySong(note3,duration,volume);
rmalik8 12:bebfef8d229f 175 break;
rmalik8 12:bebfef8d229f 176 default:
mmarine3 17:b8b68ff5e33a 177 printf("Invalid Choice\r\n");
rmalik8 12:bebfef8d229f 178 break;
rmalik8 12:bebfef8d229f 179 }
mmarine3 17:b8b68ff5e33a 180 last_counter = counter;
rmalik8 12:bebfef8d229f 181 }
mmarine3 17:b8b68ff5e33a 182 //printf("Last Counter: %d\r\n",last_counter);
rmalik8 12:bebfef8d229f 183 Thread::wait(100);
mmarine3 8:c0a6a3363e43 184 }
mmarine3 8:c0a6a3363e43 185 }
mmarine3 8:c0a6a3363e43 186
rmalik8 10:7a939a54515e 187 void bluetooth(void const *arguments)
rmalik8 10:7a939a54515e 188 {
rmalik8 10:7a939a54515e 189 char bnum=0;
rmalik8 10:7a939a54515e 190 char bhit=0;
rmalik8 12:bebfef8d229f 191 while(1) {
rmalik8 10:7a939a54515e 192 serial_mutex.lock();
rmalik8 12:bebfef8d229f 193 if(!blue.readable()) {
rmalik8 12:bebfef8d229f 194 Thread::yield();
rmalik8 10:7a939a54515e 195 } else {
rmalik8 12:bebfef8d229f 196 if (blue.getc()=='!') {
rmalik8 12:bebfef8d229f 197 if (blue.getc()=='B') { //button data packet
rmalik8 12:bebfef8d229f 198 bnum = blue.getc(); //button number
rmalik8 12:bebfef8d229f 199 bhit = blue.getc(); //1=hit, 0=release
rmalik8 12:bebfef8d229f 200 if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
rmalik8 12:bebfef8d229f 201
rmalik8 12:bebfef8d229f 202 switch (bnum) {
rmalik8 12:bebfef8d229f 203 case '1': //number button 1
rmalik8 12:bebfef8d229f 204 if (bhit=='1') {
rmalik8 12:bebfef8d229f 205 musicChoice = 0;
rmalik8 12:bebfef8d229f 206 } else {
rmalik8 12:bebfef8d229f 207 //add release code here
rmalik8 12:bebfef8d229f 208 }
rmalik8 12:bebfef8d229f 209 break;
rmalik8 12:bebfef8d229f 210 case '2': //number button 2
rmalik8 12:bebfef8d229f 211 if (bhit=='1') {
rmalik8 12:bebfef8d229f 212 musicChoice = 1;
rmalik8 12:bebfef8d229f 213 } else {
rmalik8 12:bebfef8d229f 214 //add release code here
rmalik8 12:bebfef8d229f 215 }
rmalik8 12:bebfef8d229f 216 break;
rmalik8 12:bebfef8d229f 217 case '3': //number button 3
rmalik8 12:bebfef8d229f 218 if (bhit=='1') {
rmalik8 12:bebfef8d229f 219 musicChoice = 2;
rmalik8 12:bebfef8d229f 220 } else {
rmalik8 12:bebfef8d229f 221 //add release code here
rmalik8 12:bebfef8d229f 222 }
rmalik8 12:bebfef8d229f 223 break;
rmalik8 12:bebfef8d229f 224 case '4': //number button 4
rmalik8 12:bebfef8d229f 225 if (bhit=='1') {
mmarine3 20:1dc516a6a941 226 musicChoice = 3;
rmalik8 12:bebfef8d229f 227 //add hit code here
rmalik8 12:bebfef8d229f 228 } else {
rmalik8 12:bebfef8d229f 229 //add release code here
rmalik8 12:bebfef8d229f 230 }
rmalik8 12:bebfef8d229f 231 break;
rmalik8 12:bebfef8d229f 232 case '5': //button 5 up arrow
rmalik8 12:bebfef8d229f 233 if (bhit=='1') {
mmarine3 20:1dc516a6a941 234 volume += .1;
rmalik8 12:bebfef8d229f 235 } else {
rmalik8 12:bebfef8d229f 236 //add release code here
rmalik8 12:bebfef8d229f 237 }
rmalik8 12:bebfef8d229f 238 break;
rmalik8 12:bebfef8d229f 239 case '6': //button 6 down arrow
rmalik8 12:bebfef8d229f 240 if (bhit=='1') {
mmarine3 20:1dc516a6a941 241 volume -= .1;
rmalik8 12:bebfef8d229f 242 //add hit code here
rmalik8 12:bebfef8d229f 243 } else {
rmalik8 12:bebfef8d229f 244 //add release code here
rmalik8 12:bebfef8d229f 245 }
rmalik8 12:bebfef8d229f 246 break;
rmalik8 12:bebfef8d229f 247 case '7': //button 7 left arrow
rmalik8 12:bebfef8d229f 248 if (bhit=='1') {
mmarine3 21:4fef6bf7582c 249 volume = .05;
rmalik8 12:bebfef8d229f 250 //add hit code here
rmalik8 12:bebfef8d229f 251 } else {
rmalik8 12:bebfef8d229f 252 //add release code here
rmalik8 12:bebfef8d229f 253 }
rmalik8 12:bebfef8d229f 254 break;
rmalik8 12:bebfef8d229f 255 case '8': //button 8 right arrow
rmalik8 12:bebfef8d229f 256 if (bhit=='1') {
mmarine3 21:4fef6bf7582c 257 volume = 1;
rmalik8 12:bebfef8d229f 258 //add hit code here
rmalik8 12:bebfef8d229f 259 } else {
rmalik8 12:bebfef8d229f 260 //add release code here
rmalik8 12:bebfef8d229f 261 }
rmalik8 12:bebfef8d229f 262 break;
rmalik8 12:bebfef8d229f 263 default:
rmalik8 12:bebfef8d229f 264 break;
rmalik8 12:bebfef8d229f 265 }
mmarine3 20:1dc516a6a941 266 if(volume > 1)
mmarine3 20:1dc516a6a941 267 {
mmarine3 20:1dc516a6a941 268 volume = 1;
mmarine3 20:1dc516a6a941 269 }
mmarine3 20:1dc516a6a941 270 if(volume < 0)
mmarine3 20:1dc516a6a941 271 {
mmarine3 20:1dc516a6a941 272 volume = 0;
mmarine3 20:1dc516a6a941 273 }
rmalik8 10:7a939a54515e 274 }
rmalik8 10:7a939a54515e 275 }
rmalik8 10:7a939a54515e 276 }
rmalik8 10:7a939a54515e 277 }
rmalik8 10:7a939a54515e 278 serial_mutex.unlock();
rmalik8 10:7a939a54515e 279 }
rmalik8 10:7a939a54515e 280 }
rmalik8 10:7a939a54515e 281
mmarine3 2:1ec843c95914 282 int main()
mmarine3 2:1ec843c95914 283 {
rmalik8 12:bebfef8d229f 284
mmarine3 7:85d42006e380 285 //blu.attach(&parse_message,Serial::RxIrq);
rmalik8 12:bebfef8d229f 286 //Was used in lab 3 to interupt if reading in a blutooth command
mmarine3 7:85d42006e380 287 //Thread t#(name_of_thread_function);
mmarine3 8:c0a6a3363e43 288 Thread t1(LCD);//Initialize LCD thread
mmarine3 9:f4f03767acc0 289 Thread t2(Sonar);//Initialize Sonar thread
rmalik8 14:88c09f96bdda 290 Thread t3(sounds);//Initialize sound thread
rmalik8 14:88c09f96bdda 291 Thread t4(bluetooth);//Initialize bluetooth thread
rmalik8 12:bebfef8d229f 292
rmalik8 12:bebfef8d229f 293
mmarine3 8:c0a6a3363e43 294 /* //Code to read and play a file
mmarine3 7:85d42006e380 295 FILE *wave_file;
mmarine3 7:85d42006e380 296 //printf("Hello World");
mmarine3 7:85d42006e380 297 Thread::wait(1000);
mmarine3 7:85d42006e380 298 wave_file=fopen("/sd/test.wav","r");
mmarine3 7:85d42006e380 299 //serial_mutex.lock();
mmarine3 7:85d42006e380 300 if(wave_file==NULL) printf("file open error!\n\n\r");
mmarine3 7:85d42006e380 301 //serial_mutex.unlock();
mmarine3 7:85d42006e380 302 waver.play(wave_file);
mmarine3 7:85d42006e380 303 fclose(wave_file);
mmarine3 8:c0a6a3363e43 304 */
rmalik8 12:bebfef8d229f 305
mmarine3 8:c0a6a3363e43 306 //Loop to validate the main loop is executing
rmalik8 12:bebfef8d229f 307 while(1) {
rmalik8 15:827fbc2e07d6 308
rmalik8 15:827fbc2e07d6 309 //this 'state machine' should make sure that individuals are only counted once
rmalik8 15:827fbc2e07d6 310 if ((state != 1) && !allZero){
mmarine3 19:c78aafa613b8 311 //state = 1;
mmarine3 19:c78aafa613b8 312 //do logic to figure out direction remove state = 1 above and counter++ below
mmarine3 19:c78aafa613b8 313 //NEW CODE
mmarine3 19:c78aafa613b8 314 if(movingTowards)
mmarine3 19:c78aafa613b8 315 {
mmarine3 21:4fef6bf7582c 316 ++numTowards;
mmarine3 21:4fef6bf7582c 317 counter++;
mmarine3 19:c78aafa613b8 318 state = 1;
mmarine3 21:4fef6bf7582c 319 }else if(movingAway)
mmarine3 19:c78aafa613b8 320 {
mmarine3 21:4fef6bf7582c 321 ++numAway;
mmarine3 21:4fef6bf7582c 322 counter++;
mmarine3 19:c78aafa613b8 323 state = 1;
mmarine3 19:c78aafa613b8 324 }
mmarine3 19:c78aafa613b8 325 //
rmalik8 15:827fbc2e07d6 326 //right now just adding to main counter
rmalik8 15:827fbc2e07d6 327 }
rmalik8 15:827fbc2e07d6 328 if (state == 1 && allZero){
rmalik8 15:827fbc2e07d6 329 state = 0;
rmalik8 15:827fbc2e07d6 330 }
mmarine3 9:f4f03767acc0 331 //MyLED = !MyLED;
rmalik8 15:827fbc2e07d6 332 Thread::wait(100);
mmarine3 8:c0a6a3363e43 333 }
mmarine3 2:1ec843c95914 334 }