Diff: main.cpp

Revision:

12:dd53c18e9cf2

Parent:

11:5186cc367be0

Child:

13:74d440e3000b

--- a/main.cpp	Thu Sep 07 00:01:45 2017 +0000
+++ b/main.cpp	Thu Apr 23 23:53:58 2020 +0000
@@ -1,7 +1,11 @@
 #include "mbed.h"
 #include "XNucleo53L0A1.h"
+#include "rtos.h"
+#include "Servo.h"
 #include <stdio.h>
+
 Serial pc(USBTX,USBRX);
+Servo myservo(p21);
 DigitalOut shdn(p26);
 // This VL53L0X board test application performs a range measurement in polling mode
 // Use 3.3(Vout) for Vin, p28 for SDA, p27 for SCL, P26 for shdn on mbed LPC1768
@@ -12,28 +16,92 @@
 
 static XNucleo53L0A1 *board=NULL;
 
+Thread lidar_thread;
+Thread motor_thread;
+
+uint32_t volatile distance_copy;
+int volatile status;
+double volatile on_speed = .1;
+
+Mutex distance_lock;
+Mutex status_lock;
+Mutex speed_lock;
+
+void check_distance()
+{
+    uint32_t distance;
+    
+    //loop taking and printing distance
+    while (1) 
+    {
+        distance_lock.lock();
+        status_lock.lock();
+        status = board->sensor_centre->get_distance(&distance);
+        distance_copy = distance;
+        
+        // for debugging, print distance to pc
+        if (status == VL53L0X_ERROR_NONE) 
+        {
+            pc.printf("D=%ld mm\r\n", distance_copy);
+        }
+        distance_lock.unlock();
+        status_lock.unlock();
+    }
+
+}
+
+void fan_control()
+{   
+    while (1)
+    {
+        status_lock.lock();
+        distance_lock.lock();
+        speed_lock.lock();
+        if (status == VL53L0X_ERROR_NONE)
+        {
+            if (distance_copy <= 610) // within 2 feet (distance is in mm)
+            {
+                myservo = on_speed;
+            }
+            else
+            {
+                myservo = 0;
+            }
+            
+        }
+        status_lock.unlock();
+        distance_lock.unlock();
+        speed_lock.unlock();
+
+        Thread::yield();
+    }
+}
+
 int main()
 {
-    int status;
-    uint32_t distance;
     DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
     /* creates the 53L0A1 expansion board singleton obj */
     board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
+    
     shdn = 0; //must reset sensor for an mbed reset to work
-    wait(0.1);
+    Thread::wait(100);
     shdn = 1;
-    wait(0.1);
+    Thread::wait(100);
+    
+    status_lock.lock();
     /* init the 53L0A1 board with default values */
     status = board->init_board();
-    while (status) {
+    
+    while (status) 
+    {
         pc.printf("Failed to init board! \r\n");
         status = board->init_board();
+        Thread::yield();
     }
-    //loop taking and printing distance
-    while (1) {
-        status = board->sensor_centre->get_distance(&distance);
-        if (status == VL53L0X_ERROR_NONE) {
-            pc.printf("D=%ld mm\r\n", distance);
-        }
-    }
+    status_lock.unlock();
+    
+    myservo = 0;
+
+    lidar_thread.start(check_distance);
+    motor_thread.start(fan_control);
 }