From dfa440727ca2c753aed458dc8eceebb63ca06baa Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Fri, 1 Jan 2021 17:43:58 -0500
Subject: [PATCH 01/15] First Commit

---
 CMakeLists.txt                      |   2 +
 apps/test-pwm/CMakeLists.txt        |   1 +
 apps/test-urm04/CMakeLists.txt      |   3 +
 apps/test-urm04/src/main.cpp        |  13 ++
 lib/sensors/CMakeLists.txt          |   5 +
 lib/sensors/include/URM04Sensor.h   |  54 ++++++++
 lib/sensors/src/URM04Sensor.cpp     | 189 ++++++++++++++++++++++++++++
 supported_build_configurations.yaml |   3 +
 8 files changed, 270 insertions(+)
 create mode 100644 apps/test-urm04/CMakeLists.txt
 create mode 100644 apps/test-urm04/src/main.cpp
 create mode 100644 lib/sensors/include/URM04Sensor.h
 create mode 100644 lib/sensors/src/URM04Sensor.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 37dc8a11e..76eb3acf8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -134,6 +134,7 @@ add_subdirectory(lib/pid)
 add_subdirectory(lib/sensors)
 add_subdirectory(lib/servo)
 
+
 if (NOT DEFINED APP)
     message(FATAL_ERROR "APP variable not set in CACHE. Please invoke CMake with \"-DAPP=<app-name>\"")
 elseif (NOT EXISTS "${CMAKE_SOURCE_DIR}/apps/${APP}")
@@ -170,3 +171,4 @@ add_subdirectory(apps/test-moisture)
 add_subdirectory(apps/test-pid)
 add_subdirectory(apps/test-pwm)
 add_subdirectory(apps/test-pwmin)
+add_subdirectory(apps/test-urm04)
diff --git a/apps/test-pwm/CMakeLists.txt b/apps/test-pwm/CMakeLists.txt
index 2bfb47c72..4977c3b00 100644
--- a/apps/test-pwm/CMakeLists.txt
+++ b/apps/test-pwm/CMakeLists.txt
@@ -1,3 +1,4 @@
 add_executable(test-pwm.${TARGET}-board.elf)
 target_sources(test-pwm.${TARGET}-board.elf PRIVATE src/main.cpp)
 target_set_firmware_properties(test-pwm.${TARGET}-board.elf)
+
diff --git a/apps/test-urm04/CMakeLists.txt b/apps/test-urm04/CMakeLists.txt
new file mode 100644
index 000000000..30d53d57a
--- /dev/null
+++ b/apps/test-urm04/CMakeLists.txt
@@ -0,0 +1,3 @@
+add_executable(test-urm04.${TARGET}-board.elf)
+target_sources(test-urm04.${TARGET}-board.elf PRIVATE src/main.cpp)
+target_set_firmware_properties(test-urm04.${TARGET}-board.elf)
\ No newline at end of file
diff --git a/apps/test-urm04/src/main.cpp b/apps/test-urm04/src/main.cpp
new file mode 100644
index 000000000..21897fe04
--- /dev/null
+++ b/apps/test-urm04/src/main.cpp
@@ -0,0 +1,13 @@
+#include "URM04Sensor.h"
+
+int main() {
+  // D2 - trigpin
+  // D1 - TX
+  // D0 - RX
+  URM04Sensor sensor(D2, D0, D1);
+  while (1) {
+    sensor.compute_distance();
+    sensor.print_distance();
+  }
+  return 0;
+}
\ No newline at end of file
diff --git a/lib/sensors/CMakeLists.txt b/lib/sensors/CMakeLists.txt
index b11099c35..5bb57697f 100644
--- a/lib/sensors/CMakeLists.txt
+++ b/lib/sensors/CMakeLists.txt
@@ -27,3 +27,8 @@ add_library(QEI STATIC)
 target_sources(QEI PRIVATE src/QEI.cpp)
 target_include_directories(QEI PUBLIC include)
 target_set_mbed_dependency(QEI)
+
+add_library(URM04Sensor STATIC)
+target_sources(URM04Sensor PRIVATE src/URM04Sensor.cpp)
+target_include_directories(URM04Sensor PUBLIC include)
+target_set_mbed_dependency(URM04Sensor)
diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
new file mode 100644
index 000000000..cbff5db44
--- /dev/null
+++ b/lib/sensors/include/URM04Sensor.h
@@ -0,0 +1,54 @@
+#pragma once
+// includes
+#include "BufferedSerial.h"
+#include "Thread.h"
+#include "mbed.h"
+#include "rtos.h"
+
+// constants
+#define urmAccount 1
+#define MAX_TRIES  40
+#define BUAD_RATE  19200
+
+class URM04Sensor {
+ private:
+  // trigger pin
+  DigitalOut _trigPin;
+  // start address
+  uint8_t startAddr;
+  // command buffer
+  uint8_t cmdst[10];
+  // state machine reading step
+  uint8_t readingStep;
+  // arrays needed to read and write distance from sensor
+  int urmID[urmAccount];
+  uint32_t urmData[urmAccount];
+  // timer related members
+  uint64_t managerTimer = 20;
+  Timer clock;
+  // UART protocol pins
+  PinName RX;
+  PinName TX;
+
+  // trigger the mesausrements from URM04
+  void urmTrigger(int id);
+  // reads the distance from URM04
+  void urmReader(int id);
+  // transmit commands
+  void transmitCommands();
+
+  // analyzes the distance**
+  void analyzeUrmData(uint8_t cmd[]);
+  // runs the sensor
+  void runUrm04();
+  // decodes URM04 data
+  void decodeURM04();
+
+ public:
+  // constructor
+  URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX);
+  // destructor
+  ~URM04Sensor();
+  void compute_distance();
+  void print_distance();
+};
\ No newline at end of file
diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
new file mode 100644
index 000000000..84cae0e1e
--- /dev/null
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -0,0 +1,189 @@
+#include "URM04Sensor.h"
+
+// instantiate pin connected to the URM04 sensor
+URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX)
+    : _trigPin(trig_pin), startAddr(0x11), readingStep(0), RX(_RX), TX(_TX) {
+  // start timer
+  clock.start();
+  // get start time
+  managerTimer = clock.read_ms();
+
+  // write low to pin to start instructions
+  _trigPin.write(0);
+
+  // Initialize urm04 command recieving address
+  for (int i{0}; i < urmAccount; i++) {
+    urmID[i]   = startAddr + 1;
+    urmData[i] = 0;
+  }
+
+  // initialise commands - set zero everywhere
+  for (int i{0}; i < 10; i++) cmdst[i] = 0;
+
+  // initialize urm04 protocol
+  cmdst[0] = 0x55;
+  cmdst[1] = 0xaa;
+
+  // set nucleo_board baud rate
+
+  // set up serial communication uart bufferserial with baud rate
+  // serial communication - D1/TX , D0/RX,
+  BufferedSerial nucleo_board(TX, RX, BUAD_RATE);
+}
+// destructor
+URM04Sensor::~URM04Sensor() {
+  // kill the timer
+  clock.stop();
+}
+// trigger the measurements
+void URM04Sensor::urmTrigger(int id) {
+  // fill the command buffer with trigger commands
+  cmdst[2] = id;
+  cmdst[3] = 0x00;
+  cmdst[4] = 0x01;
+  // send command over serial
+  transmitCommands();
+}
+// reads the distance from URM04
+void URM04Sensor::urmReader(int id) {
+  // fill command buffer with read commands
+  cmdst[2] = id;
+  cmdst[3] = 0x00;
+  cmdst[4] = 0x02;
+  // send command over serial
+  transmitCommands();
+}
+// transmit commands
+void URM04Sensor::transmitCommands() {
+  cmdst[5] = cmdst[0] + cmdst[2] + cmdst[3] + cmdst[4];
+  // delay for 1 second using thread  1000ms = 1sec
+  ThisThread::sleep_for(1000);
+  // send command over serial connection with BufferedSerial write and put into cmdst
+  nucleo_board.write(&cmdst[0], sizeof(cmdst));
+  // delay for 2 seconds 2000ms = 2sec
+  ThisThread::sleep_for(2000);
+}
+
+// this function actually gets the data
+void URM04Sensor::analyzeUrmData(uint8_t cmd[]) {
+  uint8_t checkSum = 0;
+  // add each byte in the command array to create checksum
+  for (int i{0}; i < 7; i++) checkSum += cmd[i];
+  // check is checksum is correct and that the commands are correct as well
+  if (checkSum == cmd[7] && cmd[3] == 2 && cmd[4] == 2) {
+    uint8_t id  = cmd[2] - startAddr;
+    urmData[id] = cmd[5] * 256 + cmd[6];
+  }
+  // else an error occured during transmission
+  else if (cmd[3] == 2 && cmd[4] == 2) {
+    printf("CheckSum Error");
+  }
+}
+
+// decodes URM04 data
+void URM04Sensor::decodeURM04() {
+  // make sure serial is still running
+  if (nucleo_board.readable()) {
+    // timer
+    int timerPoint = clock.read_ms();
+    // counter
+    int RetryCounter{0};
+    // array to hold commands
+    uint8_t cmdrd[10];
+    // fill the array
+    for (int i{0}; i < 10; i++) cmdrd[i] = 0;
+    // start index & indices
+    int indices{0};
+    int start_index{0};
+    // flag to check if the indices in the buffer are valid
+    bool flag{true};
+    // variable to check if the whole buffer is valid
+    bool valid{false};
+    // header bit
+    uint8_t headerNo{0};
+
+    while (RetryCounter < MAX_TRIES && flag) {
+      // ensure serial is still running
+      if (nucleo_board.available()) {
+        // read from the serial port with BufferedSerial read and put  data into cmdrd
+        indices = nucleo_board.read(cmdrd, sizeof(cmdrd));
+        // did not read from serial port properly
+        if (indices == -1) {
+          flag = false;
+          // flush buffer
+          nucleo_board.sync();
+          break;  // exit loop
+        }
+        // confirm the header address is correct
+        if (cmdrd[start_index] == 0xAA) {
+          headerNo = 1;
+          valid    = true;
+        }
+        // confirm every bit is within the range of the buffer
+        if (valid && start_index == headerNo + 6) {
+          flag = false;
+          break;  // exit loop
+        }
+
+        start_index++;
+        RetryCounter = 0;
+
+      } else {
+        RetryCounter++;
+        ThisThread::sleep_for(std::chrono::microseconds(15));
+      }
+    }
+    // since all the data has proven to be valid send the data to be analyzed
+    if (valid) {
+      analyzeUrmData(cmdrd);
+    }
+  }
+}
+// run the urm04
+void URM04Sensor::runUrm04() {
+  static uint64_t timer{0};
+  static int num{0};
+
+  if ((uint64_t)clock.read_ms() - timer > managerTimer) {
+    // write high to trip pin to turn on RS485 Transmitting mode
+    _trigPin.write(1);
+
+    // state machine to read measurements from the sensor
+    switch (readingStep) {
+      case 0:
+        urmTrigger(urmID[num]);
+        managerTimer = 40;  // set interval for timed measurement after triggering measurements
+        break;
+      case 1:
+        urmReader(urmID[num]);
+        managerTimer = 0;  // set interval for measurements after reading distance command
+        break;
+      case 2:
+        _trigPin.write(0);  // turn on reading mode for RS485
+        managerTimer = 10;
+        break;
+      default:
+        readingStep = 0;
+        break;
+    }
+
+    if (readingStep < 2)
+      readingStep++;
+    else
+      readingStep = 0;
+
+    timer = clock.read_ms();
+  }
+}
+// this function runs the sensor
+void URM04Sensor::compute_distance() {
+  runUrm04();
+  decodeURM04();
+}
+// this function prints the data from the sensor
+void URM04Sensor::print_distance() {
+  for (int i{0}; i < urmAccount; i++) {
+    printf("Distance: %i\n", urmData[i]);
+  }
+  ThisThread::sleep_for(std::chrono::milliseconds(100));
+}
diff --git a/supported_build_configurations.yaml b/supported_build_configurations.yaml
index a9a006191..f006f4bc2 100644
--- a/supported_build_configurations.yaml
+++ b/supported_build_configurations.yaml
@@ -73,3 +73,6 @@ test-pwmin:
 #  - gamepad
 #  - gimbtonomy
 #  - science
+
+test-urm04:
+  -nucleo

From d3ed65d320eb37b9ec9c71c96a5290b6d8cc1ac7 Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Sat, 2 Jan 2021 21:43:33 -0500
Subject: [PATCH 02/15] Second Commit - More Updates

---
 apps/test-urm04/CMakeLists.txt      |  1 +
 apps/test-urm04/src/main.cpp        |  6 ++--
 lib/sensors/include/URM04Sensor.h   | 33 ++++++++++--------
 lib/sensors/src/URM04Sensor.cpp     | 52 +++++++++++++++++------------
 supported_build_configurations.yaml |  2 +-
 5 files changed, 55 insertions(+), 39 deletions(-)

diff --git a/apps/test-urm04/CMakeLists.txt b/apps/test-urm04/CMakeLists.txt
index 30d53d57a..ad1169470 100644
--- a/apps/test-urm04/CMakeLists.txt
+++ b/apps/test-urm04/CMakeLists.txt
@@ -1,3 +1,4 @@
 add_executable(test-urm04.${TARGET}-board.elf)
 target_sources(test-urm04.${TARGET}-board.elf PRIVATE src/main.cpp)
+target_link_libraries(test-urm04.${TARGET}-board.elf PRIVATE URM04Sensor uwrt-mars-rover-hw-bridge)
 target_set_firmware_properties(test-urm04.${TARGET}-board.elf)
\ No newline at end of file
diff --git a/apps/test-urm04/src/main.cpp b/apps/test-urm04/src/main.cpp
index 21897fe04..323323eb1 100644
--- a/apps/test-urm04/src/main.cpp
+++ b/apps/test-urm04/src/main.cpp
@@ -1,13 +1,13 @@
 #include "URM04Sensor.h"
-
 int main() {
   // D2 - trigpin
   // D1 - TX
   // D0 - RX
-  URM04Sensor sensor(D2, D0, D1);
+  sensor::URM04Sensor sensor(D2, D0, D1);
   while (1) {
-    sensor.compute_distance();
+    sensor.read();
     sensor.print_distance();
+    ThisThread::sleep_for(std::chrono::milliseconds(10));
   }
   return 0;
 }
\ No newline at end of file
diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
index cbff5db44..3af3c2041 100644
--- a/lib/sensors/include/URM04Sensor.h
+++ b/lib/sensors/include/URM04Sensor.h
@@ -1,19 +1,22 @@
 #pragma once
-// includes
-#include "BufferedSerial.h"
-#include "Thread.h"
+
 #include "mbed.h"
-#include "rtos.h"
 
-// constants
-#define urmAccount 1
-#define MAX_TRIES  40
-#define BUAD_RATE  19200
+namespace sensor {
 
 class URM04Sensor {
+ protected:
+  // constants
+  static constexpr int urmAccount    = 1;
+  static constexpr int MAX_TRIES     = 10;
+  static constexpr int BAUD_RATE     = 19200;
+  static constexpr int START_ADDRESS = 0x11;
+  static constexpr int LOW           = 0;
+  static constexpr int HIGH          = 1;
+
  private:
   // trigger pin
-  DigitalOut _trigPin;
+  DigitalOut m_trigPin;
   // start address
   uint8_t startAddr;
   // command buffer
@@ -24,11 +27,13 @@ class URM04Sensor {
   int urmID[urmAccount];
   uint32_t urmData[urmAccount];
   // timer related members
-  uint64_t managerTimer = 20;
+  uint64_t managerTimer;
   Timer clock;
   // UART protocol pins
   PinName RX;
   PinName TX;
+  // successful read
+  bool read_success;
 
   // trigger the mesausrements from URM04
   void urmTrigger(int id);
@@ -39,7 +44,7 @@ class URM04Sensor {
 
   // analyzes the distance**
   void analyzeUrmData(uint8_t cmd[]);
-  // runs the sensor
+  // runs the sensor - starts giving commands to the sensor
   void runUrm04();
   // decodes URM04 data
   void decodeURM04();
@@ -49,6 +54,8 @@ class URM04Sensor {
   URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX);
   // destructor
   ~URM04Sensor();
-  void compute_distance();
+  bool read();
   void print_distance();
-};
\ No newline at end of file
+};
+
+}  // namespace sensor
\ No newline at end of file
diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index 84cae0e1e..7763bce21 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -1,15 +1,15 @@
 #include "URM04Sensor.h"
 
 // instantiate pin connected to the URM04 sensor
-URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX)
-    : _trigPin(trig_pin), startAddr(0x11), readingStep(0), RX(_RX), TX(_TX) {
+sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX)
+    : m_trigPin(trig_pin), startAddr(START_ADDRESS), readingStep(LOW), RX(_RX), TX(_TX), read_success(false) {
   // start timer
   clock.start();
   // get start time
   managerTimer = clock.read_ms();
 
   // write low to pin to start instructions
-  _trigPin.write(0);
+  m_trigPin.write(LOW);
 
   // Initialize urm04 command recieving address
   for (int i{0}; i < urmAccount; i++) {
@@ -18,7 +18,9 @@ URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX)
   }
 
   // initialise commands - set zero everywhere
-  for (int i{0}; i < 10; i++) cmdst[i] = 0;
+  for (int i{0}; i < 10; i++) {
+    cmdst[i] = 0;
+  }
 
   // initialize urm04 protocol
   cmdst[0] = 0x55;
@@ -28,15 +30,15 @@ URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX)
 
   // set up serial communication uart bufferserial with baud rate
   // serial communication - D1/TX , D0/RX,
-  BufferedSerial nucleo_board(TX, RX, BUAD_RATE);
+  BufferedSerial nucleo_board(TX, RX, BAUD_RATE);
 }
 // destructor
-URM04Sensor::~URM04Sensor() {
+sensor::URM04Sensor::~URM04Sensor() {
   // kill the timer
   clock.stop();
 }
 // trigger the measurements
-void URM04Sensor::urmTrigger(int id) {
+void sensor::URM04Sensor::urmTrigger(int id) {
   // fill the command buffer with trigger commands
   cmdst[2] = id;
   cmdst[3] = 0x00;
@@ -45,7 +47,7 @@ void URM04Sensor::urmTrigger(int id) {
   transmitCommands();
 }
 // reads the distance from URM04
-void URM04Sensor::urmReader(int id) {
+void sensor::URM04Sensor::urmReader(int id) {
   // fill command buffer with read commands
   cmdst[2] = id;
   cmdst[3] = 0x00;
@@ -54,21 +56,23 @@ void URM04Sensor::urmReader(int id) {
   transmitCommands();
 }
 // transmit commands
-void URM04Sensor::transmitCommands() {
+void sensor::URM04Sensor::transmitCommands() {
   cmdst[5] = cmdst[0] + cmdst[2] + cmdst[3] + cmdst[4];
   // delay for 1 second using thread  1000ms = 1sec
-  ThisThread::sleep_for(1000);
+  ThisThread::sleep_for(std::chrono::milliseconds(1000));
   // send command over serial connection with BufferedSerial write and put into cmdst
   nucleo_board.write(&cmdst[0], sizeof(cmdst));
   // delay for 2 seconds 2000ms = 2sec
-  ThisThread::sleep_for(2000);
+  ThisThread::sleep_for(std::chrono::milliseconds(2000));
 }
 
 // this function actually gets the data
-void URM04Sensor::analyzeUrmData(uint8_t cmd[]) {
+void sensor::URM04Sensor::analyzeUrmData(uint8_t cmd[]) {
   uint8_t checkSum = 0;
   // add each byte in the command array to create checksum
-  for (int i{0}; i < 7; i++) checkSum += cmd[i];
+  for (int i{0}; i < 7; i++) {
+    checkSum += cmd[i];
+  }
   // check is checksum is correct and that the commands are correct as well
   if (checkSum == cmd[7] && cmd[3] == 2 && cmd[4] == 2) {
     uint8_t id  = cmd[2] - startAddr;
@@ -76,12 +80,12 @@ void URM04Sensor::analyzeUrmData(uint8_t cmd[]) {
   }
   // else an error occured during transmission
   else if (cmd[3] == 2 && cmd[4] == 2) {
-    printf("CheckSum Error");
+    read_success = false;
   }
 }
 
 // decodes URM04 data
-void URM04Sensor::decodeURM04() {
+void sensor::URM04Sensor::decodeURM04() {
   // make sure serial is still running
   if (nucleo_board.readable()) {
     // timer
@@ -91,7 +95,7 @@ void URM04Sensor::decodeURM04() {
     // array to hold commands
     uint8_t cmdrd[10];
     // fill the array
-    for (int i{0}; i < 10; i++) cmdrd[i] = 0;
+    for (int i{0}; i < 10; i++) {cmdrd[i] = 0;}
     // start index & indices
     int indices{0};
     int start_index{0};
@@ -112,6 +116,8 @@ void URM04Sensor::decodeURM04() {
           flag = false;
           // flush buffer
           nucleo_board.sync();
+          // read failed
+          read_success = false;
           break;  // exit loop
         }
         // confirm the header address is correct
@@ -135,18 +141,19 @@ void URM04Sensor::decodeURM04() {
     }
     // since all the data has proven to be valid send the data to be analyzed
     if (valid) {
+      read_success = true;
       analyzeUrmData(cmdrd);
     }
   }
 }
 // run the urm04
-void URM04Sensor::runUrm04() {
+void sensor::URM04Sensor::runUrm04() {
   static uint64_t timer{0};
   static int num{0};
 
   if ((uint64_t)clock.read_ms() - timer > managerTimer) {
     // write high to trip pin to turn on RS485 Transmitting mode
-    _trigPin.write(1);
+    m_trigPin.write(1);
 
     // state machine to read measurements from the sensor
     switch (readingStep) {
@@ -159,7 +166,7 @@ void URM04Sensor::runUrm04() {
         managerTimer = 0;  // set interval for measurements after reading distance command
         break;
       case 2:
-        _trigPin.write(0);  // turn on reading mode for RS485
+        m_trigPin.write(LOW);  // turn on reading mode for RS485
         managerTimer = 10;
         break;
       default:
@@ -176,14 +183,15 @@ void URM04Sensor::runUrm04() {
   }
 }
 // this function runs the sensor
-void URM04Sensor::compute_distance() {
+bool sensor::URM04Sensor::read() {
   runUrm04();
   decodeURM04();
+  return read_success;
 }
 // this function prints the data from the sensor
-void URM04Sensor::print_distance() {
+void sensor::URM04Sensor::print_distance() {
   for (int i{0}; i < urmAccount; i++) {
-    printf("Distance: %i\n", urmData[i]);
+    printf("Distance: %u\n", urmData[i]);
   }
   ThisThread::sleep_for(std::chrono::milliseconds(100));
 }
diff --git a/supported_build_configurations.yaml b/supported_build_configurations.yaml
index f006f4bc2..c17f51f41 100644
--- a/supported_build_configurations.yaml
+++ b/supported_build_configurations.yaml
@@ -75,4 +75,4 @@ test-pwmin:
 #  - science
 
 test-urm04:
-  -nucleo
+  - nucleo

From 5696acc542f81f33565274b26543ad946122803f Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Sun, 3 Jan 2021 13:02:21 -0500
Subject: [PATCH 03/15] Third-Commit

---
 apps/test-urm04/src/main.cpp      |  2 +-
 lib/sensors/include/URM04Sensor.h |  4 ++-
 lib/sensors/src/URM04Sensor.cpp   | 59 ++++++++++++++++---------------
 3 files changed, 34 insertions(+), 31 deletions(-)

diff --git a/apps/test-urm04/src/main.cpp b/apps/test-urm04/src/main.cpp
index 323323eb1..9a000794c 100644
--- a/apps/test-urm04/src/main.cpp
+++ b/apps/test-urm04/src/main.cpp
@@ -3,7 +3,7 @@ int main() {
   // D2 - trigpin
   // D1 - TX
   // D0 - RX
-  sensor::URM04Sensor sensor(D2, D0, D1);
+  URM04Sensor::URM04Sensor sensor(D2, D0, D1);
   while (1) {
     sensor.read();
     sensor.print_distance();
diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
index 3af3c2041..94269be7c 100644
--- a/lib/sensors/include/URM04Sensor.h
+++ b/lib/sensors/include/URM04Sensor.h
@@ -2,7 +2,7 @@
 
 #include "mbed.h"
 
-namespace sensor {
+namespace URM04Sensor {
 
 class URM04Sensor {
  protected:
@@ -34,6 +34,8 @@ class URM04Sensor {
   PinName TX;
   // successful read
   bool read_success;
+  //serial
+  BufferedSerial serial; 
 
   // trigger the mesausrements from URM04
   void urmTrigger(int id);
diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index 7763bce21..6b2fa6ad1 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -1,13 +1,13 @@
 #include "URM04Sensor.h"
 
-// instantiate pin connected to the URM04 sensor
-sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX)
-    : m_trigPin(trig_pin), startAddr(START_ADDRESS), readingStep(LOW), RX(_RX), TX(_TX), read_success(false) {
+// instantiate pin connected to the URM04 URM04Sensor
+URM04Sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX)
+    : m_trigPin(trig_pin), startAddr(START_ADDRESS), readingStep(LOW), RX(_RX), TX(_TX), read_success(false), serial(TX, RX, BAUD_RATE) {
   // start timer
   clock.start();
   // get start time
-  managerTimer = clock.read_ms();
-
+  managerTimer =
+    std::chrono::duration_cast<std::chrono::milliseconds>(chrono::duration_cast<chrono::duration<uint64_t>>(clock.elapsed_time())).count();
   // write low to pin to start instructions
   m_trigPin.write(LOW);
 
@@ -30,15 +30,14 @@ sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX)
 
   // set up serial communication uart bufferserial with baud rate
   // serial communication - D1/TX , D0/RX,
-  BufferedSerial nucleo_board(TX, RX, BAUD_RATE);
 }
 // destructor
-sensor::URM04Sensor::~URM04Sensor() {
+URM04Sensor::URM04Sensor::~URM04Sensor() {
   // kill the timer
   clock.stop();
 }
 // trigger the measurements
-void sensor::URM04Sensor::urmTrigger(int id) {
+void URM04Sensor::URM04Sensor::urmTrigger(int id) {
   // fill the command buffer with trigger commands
   cmdst[2] = id;
   cmdst[3] = 0x00;
@@ -47,7 +46,7 @@ void sensor::URM04Sensor::urmTrigger(int id) {
   transmitCommands();
 }
 // reads the distance from URM04
-void sensor::URM04Sensor::urmReader(int id) {
+void URM04Sensor::URM04Sensor::urmReader(int id) {
   // fill command buffer with read commands
   cmdst[2] = id;
   cmdst[3] = 0x00;
@@ -56,18 +55,18 @@ void sensor::URM04Sensor::urmReader(int id) {
   transmitCommands();
 }
 // transmit commands
-void sensor::URM04Sensor::transmitCommands() {
+void URM04Sensor::URM04Sensor::transmitCommands() {
   cmdst[5] = cmdst[0] + cmdst[2] + cmdst[3] + cmdst[4];
   // delay for 1 second using thread  1000ms = 1sec
   ThisThread::sleep_for(std::chrono::milliseconds(1000));
   // send command over serial connection with BufferedSerial write and put into cmdst
-  nucleo_board.write(&cmdst[0], sizeof(cmdst));
+  serial.write(&cmdst[0], sizeof(cmdst));
   // delay for 2 seconds 2000ms = 2sec
   ThisThread::sleep_for(std::chrono::milliseconds(2000));
 }
 
 // this function actually gets the data
-void sensor::URM04Sensor::analyzeUrmData(uint8_t cmd[]) {
+void URM04Sensor::URM04Sensor::analyzeUrmData(uint8_t cmd[]) {
   uint8_t checkSum = 0;
   // add each byte in the command array to create checksum
   for (int i{0}; i < 7; i++) {
@@ -85,11 +84,11 @@ void sensor::URM04Sensor::analyzeUrmData(uint8_t cmd[]) {
 }
 
 // decodes URM04 data
-void sensor::URM04Sensor::decodeURM04() {
+void URM04Sensor::URM04Sensor::decodeURM04() {
   // make sure serial is still running
-  if (nucleo_board.readable()) {
+  if (serial.readable()) {
     // timer
-    int timerPoint = clock.read_ms();
+    //int timerPoint = clock.elapsed_time(); //cast this 
     // counter
     int RetryCounter{0};
     // array to hold commands
@@ -108,14 +107,14 @@ void sensor::URM04Sensor::decodeURM04() {
 
     while (RetryCounter < MAX_TRIES && flag) {
       // ensure serial is still running
-      if (nucleo_board.available()) {
+      if (serial.readable()) {
         // read from the serial port with BufferedSerial read and put  data into cmdrd
-        indices = nucleo_board.read(cmdrd, sizeof(cmdrd));
+        indices = serial.read(cmdrd, sizeof(cmdrd));
         // did not read from serial port properly
         if (indices == -1) {
           flag = false;
           // flush buffer
-          nucleo_board.sync();
+          serial.sync();
           // read failed
           read_success = false;
           break;  // exit loop
@@ -136,7 +135,7 @@ void sensor::URM04Sensor::decodeURM04() {
 
       } else {
         RetryCounter++;
-        ThisThread::sleep_for(std::chrono::microseconds(15));
+        wait_us(15);
       }
     }
     // since all the data has proven to be valid send the data to be analyzed
@@ -147,15 +146,15 @@ void sensor::URM04Sensor::decodeURM04() {
   }
 }
 // run the urm04
-void sensor::URM04Sensor::runUrm04() {
+void URM04Sensor::URM04Sensor::runUrm04() {
   static uint64_t timer{0};
   static int num{0};
 
   if ((uint64_t)clock.read_ms() - timer > managerTimer) {
     // write high to trip pin to turn on RS485 Transmitting mode
-    m_trigPin.write(1);
+    m_trigPin.write(HIGH);
 
-    // state machine to read measurements from the sensor
+    // state machine to read measurements from the URM04Sensor
     switch (readingStep) {
       case 0:
         urmTrigger(urmID[num]);
@@ -179,19 +178,21 @@ void sensor::URM04Sensor::runUrm04() {
     else
       readingStep = 0;
 
-    timer = clock.read_ms();
+    timer = 
+          std::chrono::duration_cast<std::chrono::milliseconds>(chrono::duration_cast<chrono::duration<uint64_t>>(clock.elapsed_time())).count();
+
   }
 }
-// this function runs the sensor
-bool sensor::URM04Sensor::read() {
+// this function runs the URM04Sensor
+bool URM04Sensor::URM04Sensor::read() {
   runUrm04();
   decodeURM04();
   return read_success;
 }
-// this function prints the data from the sensor
-void sensor::URM04Sensor::print_distance() {
+// this function prints the data from the URM04Sensor
+void URM04Sensor::URM04Sensor::print_distance() {
   for (int i{0}; i < urmAccount; i++) {
-    printf("Distance: %u\n", urmData[i]);
+    printf("%lu", urmData[i]); //read 
   }
-  ThisThread::sleep_for(std::chrono::milliseconds(100));
+  wait_us(15);
 }

From fe2c00485a1946f8f4eadb77a7b4b30685a3d9da Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Sun, 3 Jan 2021 18:15:26 -0500
Subject: [PATCH 04/15] Fourth Commit - Younes' Requested Changes

---
 apps/test-urm04/src/main.cpp      |   7 +-
 lib/sensors/include/URM04Sensor.h |  42 ++---
 lib/sensors/src/URM04Sensor.cpp   | 280 +++++++++++-------------------
 3 files changed, 118 insertions(+), 211 deletions(-)

diff --git a/apps/test-urm04/src/main.cpp b/apps/test-urm04/src/main.cpp
index 9a000794c..e1f196c41 100644
--- a/apps/test-urm04/src/main.cpp
+++ b/apps/test-urm04/src/main.cpp
@@ -5,9 +5,10 @@ int main() {
   // D0 - RX
   URM04Sensor::URM04Sensor sensor(D2, D0, D1);
   while (1) {
-    sensor.read();
-    sensor.print_distance();
+    sensor.trigger_sensor();
+    sensor.read_distance();
     ThisThread::sleep_for(std::chrono::milliseconds(10));
-  }
+  }    
+
   return 0;
 }
\ No newline at end of file
diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
index 94269be7c..667af4a27 100644
--- a/lib/sensors/include/URM04Sensor.h
+++ b/lib/sensors/include/URM04Sensor.h
@@ -5,14 +5,12 @@
 namespace URM04Sensor {
 
 class URM04Sensor {
+
  protected:
   // constants
-  static constexpr int urmAccount    = 1;
-  static constexpr int MAX_TRIES     = 10;
   static constexpr int BAUD_RATE     = 19200;
   static constexpr int START_ADDRESS = 0x11;
   static constexpr int LOW           = 0;
-  static constexpr int HIGH          = 1;
 
  private:
   // trigger pin
@@ -21,43 +19,25 @@ class URM04Sensor {
   uint8_t startAddr;
   // command buffer
   uint8_t cmdst[10];
-  // state machine reading step
-  uint8_t readingStep;
-  // arrays needed to read and write distance from sensor
-  int urmID[urmAccount];
-  uint32_t urmData[urmAccount];
-  // timer related members
-  uint64_t managerTimer;
-  Timer clock;
   // UART protocol pins
   PinName RX;
   PinName TX;
-  // successful read
-  bool read_success;
   //serial
   BufferedSerial serial; 
-
-  // trigger the mesausrements from URM04
-  void urmTrigger(int id);
-  // reads the distance from URM04
-  void urmReader(int id);
-  // transmit commands
-  void transmitCommands();
-
-  // analyzes the distance**
-  void analyzeUrmData(uint8_t cmd[]);
-  // runs the sensor - starts giving commands to the sensor
-  void runUrm04();
-  // decodes URM04 data
-  void decodeURM04();
+  //variable to check if success_read
+  bool success;
+  //sensor distance
+  float m_distance; 
 
  public:
   // constructor
   URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX);
-  // destructor
-  ~URM04Sensor();
-  bool read();
-  void print_distance();
+
+  //Trigger Sensor
+  void trigger_sensor();
+
+  //Read Distance
+  float read_distance();
 };
 
 }  // namespace sensor
\ No newline at end of file
diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index 6b2fa6ad1..f10009585 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -2,197 +2,123 @@
 
 // instantiate pin connected to the URM04 URM04Sensor
 URM04Sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX)
-    : m_trigPin(trig_pin), startAddr(START_ADDRESS), readingStep(LOW), RX(_RX), TX(_TX), read_success(false), serial(TX, RX, BAUD_RATE) {
-  // start timer
-  clock.start();
-  // get start time
-  managerTimer =
-    std::chrono::duration_cast<std::chrono::milliseconds>(chrono::duration_cast<chrono::duration<uint64_t>>(clock.elapsed_time())).count();
+    : m_trigPin(trig_pin), startAddr(START_ADDRESS), RX(_RX), TX(_TX), serial(TX, RX, BAUD_RATE) {
   // write low to pin to start instructions
   m_trigPin.write(LOW);
 
-  // Initialize urm04 command recieving address
-  for (int i{0}; i < urmAccount; i++) {
-    urmID[i]   = startAddr + 1;
-    urmData[i] = 0;
-  }
-
-  // initialise commands - set zero everywhere
-  for (int i{0}; i < 10; i++) {
-    cmdst[i] = 0;
-  }
-
-  // initialize urm04 protocol
-  cmdst[0] = 0x55;
-  cmdst[1] = 0xaa;
+  // instantiate command buffer with all zeros
 
-  // set nucleo_board baud rate
+  /***
+   * for(int i{0}; i<10; i++){
+   *   cmdst[i] = 0;
+   * }
+   *
+   ***/
 
-  // set up serial communication uart bufferserial with baud rate
-  // serial communication - D1/TX , D0/RX,
-}
-// destructor
-URM04Sensor::URM04Sensor::~URM04Sensor() {
-  // kill the timer
-  clock.stop();
+  memset(&cmdst[0], 0, sizeof(cmdst));
 }
-// trigger the measurements
-void URM04Sensor::URM04Sensor::urmTrigger(int id) {
-  // fill the command buffer with trigger commands
-  cmdst[2] = id;
+
+void URM04Sensor::URM04Sensor::trigger_sensor() {
+  /************************************ INSTANTIATE COMMANDS TO BE SENT OVER SERIAL
+   * *****************************************/
+
+  // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
+  uint8_t checkSum;
+  // buffer header
+  cmdst[0] = 0x055;
+  cmdst[1] = 0xAA;
+  // device address
+  cmdst[2] = startAddr;
+  // length
   cmdst[3] = 0x00;
+  // the command itself
   cmdst[4] = 0x01;
-  // send command over serial
-  transmitCommands();
-}
-// reads the distance from URM04
-void URM04Sensor::URM04Sensor::urmReader(int id) {
-  // fill command buffer with read commands
-  cmdst[2] = id;
-  cmdst[3] = 0x00;
-  cmdst[4] = 0x02;
-  // send command over serial
-  transmitCommands();
-}
-// transmit commands
-void URM04Sensor::URM04Sensor::transmitCommands() {
-  cmdst[5] = cmdst[0] + cmdst[2] + cmdst[3] + cmdst[4];
-  // delay for 1 second using thread  1000ms = 1sec
-  ThisThread::sleep_for(std::chrono::milliseconds(1000));
-  // send command over serial connection with BufferedSerial write and put into cmdst
-  serial.write(&cmdst[0], sizeof(cmdst));
-  // delay for 2 seconds 2000ms = 2sec
-  ThisThread::sleep_for(std::chrono::milliseconds(2000));
-}
-
-// this function actually gets the data
-void URM04Sensor::URM04Sensor::analyzeUrmData(uint8_t cmd[]) {
-  uint8_t checkSum = 0;
-  // add each byte in the command array to create checksum
-  for (int i{0}; i < 7; i++) {
-    checkSum += cmd[i];
-  }
-  // check is checksum is correct and that the commands are correct as well
-  if (checkSum == cmd[7] && cmd[3] == 2 && cmd[4] == 2) {
-    uint8_t id  = cmd[2] - startAddr;
-    urmData[id] = cmd[5] * 256 + cmd[6];
-  }
-  // else an error occured during transmission
-  else if (cmd[3] == 2 && cmd[4] == 2) {
-    read_success = false;
+  // checksum bit
+  checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4];
+  // instantiate the last element in the command buffer with checksum value
+  cmdst[5] = checkSum;
+
+  /******** SEND COMMANDS OVER SERIAL *********************/
+  int num_bytes;
+  // returns the number of bytes if successful write
+  num_bytes = serial.write(&cmdst[0], sizeof(cmdst));
+
+  // else if numb_btyes < 0 serial failed or if number of bytes written != 6 (because 6 bytes sent over serial)
+  if (num_bytes < 0 || num_bytes != 6) {
+    success = false;
   }
+
+  // flush the buffer from serial
+  serial.sync();
+
+  // wait for at least 30 ms after calling trigger function
+  ThisThread::sleep_for(std::chrono::milliseconds(35));
+
+  // reset command buffer - fill whole array with zeros
+  memset(&cmdst[0], 0, sizeof(cmdst));
 }
 
-// decodes URM04 data
-void URM04Sensor::URM04Sensor::decodeURM04() {
-  // make sure serial is still running
-  if (serial.readable()) {
-    // timer
-    //int timerPoint = clock.elapsed_time(); //cast this 
-    // counter
-    int RetryCounter{0};
-    // array to hold commands
-    uint8_t cmdrd[10];
-    // fill the array
-    for (int i{0}; i < 10; i++) {cmdrd[i] = 0;}
-    // start index & indices
-    int indices{0};
-    int start_index{0};
-    // flag to check if the indices in the buffer are valid
-    bool flag{true};
-    // variable to check if the whole buffer is valid
-    bool valid{false};
-    // header bit
-    uint8_t headerNo{0};
-
-    while (RetryCounter < MAX_TRIES && flag) {
-      // ensure serial is still running
-      if (serial.readable()) {
-        // read from the serial port with BufferedSerial read and put  data into cmdrd
-        indices = serial.read(cmdrd, sizeof(cmdrd));
-        // did not read from serial port properly
-        if (indices == -1) {
-          flag = false;
-          // flush buffer
-          serial.sync();
-          // read failed
-          read_success = false;
-          break;  // exit loop
-        }
-        // confirm the header address is correct
-        if (cmdrd[start_index] == 0xAA) {
-          headerNo = 1;
-          valid    = true;
-        }
-        // confirm every bit is within the range of the buffer
-        if (valid && start_index == headerNo + 6) {
-          flag = false;
-          break;  // exit loop
-        }
-
-        start_index++;
-        RetryCounter = 0;
-
-      } else {
-        RetryCounter++;
-        wait_us(15);
-      }
-    }
-    // since all the data has proven to be valid send the data to be analyzed
-    if (valid) {
-      read_success = true;
-      analyzeUrmData(cmdrd);
-    }
+float URM04Sensor::URM04Sensor::read_distance() {
+  /************ INSTANTIATE COMMANDS TO BE SENT OVER SERIAL************/
+  // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
+  uint8_t checkSum;
+  // buffer header
+  cmdst[0] = 0x55;
+  cmdst[1] = 0xAA;
+  // device address
+  cmdst[2] = startAddr;
+  // command length
+  cmdst[3] = 0x00;
+  // the command itself
+  cmdst[4] = 0x02;
+  // checksum bit
+  checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4];
+  // instantiate the last element in the command buffer with checksum value
+  cmdst[5] = checkSum;
+
+  /************** SEND COMMANDS OVER SERIAL***********/
+  int w_num_bytes;
+  // returns the number of bytes if successful read
+  w_num_bytes = serial.write(&cmdst[0], sizeof(cmdst));
+
+  // else if numb_btyes < 0 serial failed or if number of bytes read != 6 (because 6 bytes sent over serial)
+  if (w_num_bytes < 0 || w_num_bytes != 6) {
+    success = false;
+    // return -1 indicating read command has failed
+    return -1;
   }
-}
-// run the urm04
-void URM04Sensor::URM04Sensor::runUrm04() {
-  static uint64_t timer{0};
-  static int num{0};
-
-  if ((uint64_t)clock.read_ms() - timer > managerTimer) {
-    // write high to trip pin to turn on RS485 Transmitting mode
-    m_trigPin.write(HIGH);
-
-    // state machine to read measurements from the URM04Sensor
-    switch (readingStep) {
-      case 0:
-        urmTrigger(urmID[num]);
-        managerTimer = 40;  // set interval for timed measurement after triggering measurements
-        break;
-      case 1:
-        urmReader(urmID[num]);
-        managerTimer = 0;  // set interval for measurements after reading distance command
-        break;
-      case 2:
-        m_trigPin.write(LOW);  // turn on reading mode for RS485
-        managerTimer = 10;
-        break;
-      default:
-        readingStep = 0;
-        break;
-    }
 
-    if (readingStep < 2)
-      readingStep++;
-    else
-      readingStep = 0;
+  // flush buffer from serial
+  serial.sync();
 
-    timer = 
-          std::chrono::duration_cast<std::chrono::milliseconds>(chrono::duration_cast<chrono::duration<uint64_t>>(clock.elapsed_time())).count();
+  // reset command buffer - fill whole array with zeros
+  memset(&cmdst[0], 0, sizeof(cmdst));
+  /******* READ RETURN VALUE FROM SERIAL**************/
+  int r_num_bytes;
+  // returns the number of bytes if successful read
+  r_num_bytes = serial.read(&cmdst[0], sizeof(cmdst));
 
+  // else if numb_btyes < 0 serial failed or if number of bytes read != 8 (because 8 bytes sent over serial)
+  if (r_num_bytes < 0 || r_num_bytes != 8) {
+    success = false;
+    return -1;
   }
-}
-// this function runs the URM04Sensor
-bool URM04Sensor::URM04Sensor::read() {
-  runUrm04();
-  decodeURM04();
-  return read_success;
-}
-// this function prints the data from the URM04Sensor
-void URM04Sensor::URM04Sensor::print_distance() {
-  for (int i{0}; i < urmAccount; i++) {
-    printf("%lu", urmData[i]); //read 
+  /******** PARSE THROUGH THE DATA READ FROM SERIAL*********/
+  else {
+    // create the checksum
+    checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4] + cmdst[5] + cmdst[6];
+
+    // check if the checksum is incorrect
+    if (checkSum != cmdst[7]) {
+      success = false;
+      // return -1 if checksum failed
+      return -1;
+    } else {
+      // get distance from sensor
+      m_distance = (float)(cmdst[5] << 8) + (float)cmdst[6];
+      // --------------------- or --------------------------
+      // m_distance = (float)(cmdst[5]*256) + (float)cmdst[6];
+      return m_distance;
+    }
   }
-  wait_us(15);
-}
+}
\ No newline at end of file

From a279500cf594d560a69a5891e2e3ace934d30aa8 Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Sun, 3 Jan 2021 18:24:26 -0500
Subject: [PATCH 05/15] Fifth Commit - Younes' Updated Requested Changes

---
 apps/test-urm04/src/main.cpp      |  2 +-
 lib/sensors/include/URM04Sensor.h | 17 ++++++++---------
 2 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/apps/test-urm04/src/main.cpp b/apps/test-urm04/src/main.cpp
index e1f196c41..0245d9713 100644
--- a/apps/test-urm04/src/main.cpp
+++ b/apps/test-urm04/src/main.cpp
@@ -8,7 +8,7 @@ int main() {
     sensor.trigger_sensor();
     sensor.read_distance();
     ThisThread::sleep_for(std::chrono::milliseconds(10));
-  }    
+  }
 
   return 0;
 }
\ No newline at end of file
diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
index 667af4a27..194ee6d66 100644
--- a/lib/sensors/include/URM04Sensor.h
+++ b/lib/sensors/include/URM04Sensor.h
@@ -5,7 +5,6 @@
 namespace URM04Sensor {
 
 class URM04Sensor {
-
  protected:
   // constants
   static constexpr int BAUD_RATE     = 19200;
@@ -22,22 +21,22 @@ class URM04Sensor {
   // UART protocol pins
   PinName RX;
   PinName TX;
-  //serial
-  BufferedSerial serial; 
-  //variable to check if success_read
+  // serial
+  BufferedSerial serial;
+  // variable to check if success_read
   bool success;
-  //sensor distance
-  float m_distance; 
+  // sensor distance
+  float m_distance;
 
  public:
   // constructor
   URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX);
 
-  //Trigger Sensor
+  // Trigger Sensor
   void trigger_sensor();
 
-  //Read Distance
+  // Read Distance
   float read_distance();
 };
 
-}  // namespace sensor
\ No newline at end of file
+}  // namespace URM04Sensor
\ No newline at end of file

From dd18b998a4c7ac6346e2f0a97b9345cba1ccd129 Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Sun, 3 Jan 2021 20:35:48 -0500
Subject: [PATCH 06/15] Sixth Commit - Younes' Updated Request Changes Pt2.

---
 apps/test-urm04/src/main.cpp      |  8 +++---
 lib/sensors/include/URM04Sensor.h | 14 ++++-------
 lib/sensors/src/URM04Sensor.cpp   | 41 ++++++++++++++++++-------------
 3 files changed, 34 insertions(+), 29 deletions(-)

diff --git a/apps/test-urm04/src/main.cpp b/apps/test-urm04/src/main.cpp
index 0245d9713..5fef4cfe1 100644
--- a/apps/test-urm04/src/main.cpp
+++ b/apps/test-urm04/src/main.cpp
@@ -4,10 +4,12 @@ int main() {
   // D1 - TX
   // D0 - RX
   URM04Sensor::URM04Sensor sensor(D2, D0, D1);
+  float get_distance;
   while (1) {
-    sensor.trigger_sensor();
-    sensor.read_distance();
-    ThisThread::sleep_for(std::chrono::milliseconds(10));
+    sensor.read_distance(get_distance);
+    // wait 10 milliseconds
+    ThisThread::sleep_for(10ms);
+    printf("Distance: %f cm\n", get_distance);
   }
 
   return 0;
diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
index 194ee6d66..5bf641228 100644
--- a/lib/sensors/include/URM04Sensor.h
+++ b/lib/sensors/include/URM04Sensor.h
@@ -23,20 +23,16 @@ class URM04Sensor {
   PinName TX;
   // serial
   BufferedSerial serial;
-  // variable to check if success_read
-  bool success;
-  // sensor distance
-  float m_distance;
+  // Trigger Sensor
+  void trigger_sensor(float& distance);
 
  public:
   // constructor
   URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX);
 
-  // Trigger Sensor
-  void trigger_sensor();
-
-  // Read Distance
-  float read_distance();
+  // Read Distance in CENTIMETER returns true if successful read
+  // pass by reference a variable to hold the distance
+  bool read_distance(float& distance);
 };
 
 }  // namespace URM04Sensor
\ No newline at end of file
diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index f10009585..24d08fce2 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -18,9 +18,8 @@ URM04Sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX
   memset(&cmdst[0], 0, sizeof(cmdst));
 }
 
-void URM04Sensor::URM04Sensor::trigger_sensor() {
-  /************************************ INSTANTIATE COMMANDS TO BE SENT OVER SERIAL
-   * *****************************************/
+void URM04Sensor::URM04Sensor::trigger_sensor(float& distance) {
+  /************ INSTANTIATE COMMANDS TO BE SENT OVER SERIAL*********/
 
   // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
   uint8_t checkSum;
@@ -45,20 +44,28 @@ void URM04Sensor::URM04Sensor::trigger_sensor() {
 
   // else if numb_btyes < 0 serial failed or if number of bytes written != 6 (because 6 bytes sent over serial)
   if (num_bytes < 0 || num_bytes != 6) {
-    success = false;
+    // error occured in trigger step
+    distance = -1;
+  } else {
+    // no error occured in trigger step
+    distance = 0;
   }
 
   // flush the buffer from serial
   serial.sync();
 
   // wait for at least 30 ms after calling trigger function
-  ThisThread::sleep_for(std::chrono::milliseconds(35));
+  ThisThread::sleep_for(35ms);
 
   // reset command buffer - fill whole array with zeros
   memset(&cmdst[0], 0, sizeof(cmdst));
 }
 
-float URM04Sensor::URM04Sensor::read_distance() {
+// pass by reference a variable to hold the distance value
+bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
+  /****************** TRIGGER SENSOR BEFORE READING DISTANCE ********************/
+  trigger_sensor(distance);
+
   /************ INSTANTIATE COMMANDS TO BE SENT OVER SERIAL************/
   // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
   uint8_t checkSum;
@@ -83,9 +90,9 @@ float URM04Sensor::URM04Sensor::read_distance() {
 
   // else if numb_btyes < 0 serial failed or if number of bytes read != 6 (because 6 bytes sent over serial)
   if (w_num_bytes < 0 || w_num_bytes != 6) {
-    success = false;
-    // return -1 indicating read command has failed
-    return -1;
+    // return false indicating read command has failed
+    distance = -1;
+    return false;
   }
 
   // flush buffer from serial
@@ -100,8 +107,8 @@ float URM04Sensor::URM04Sensor::read_distance() {
 
   // else if numb_btyes < 0 serial failed or if number of bytes read != 8 (because 8 bytes sent over serial)
   if (r_num_bytes < 0 || r_num_bytes != 8) {
-    success = false;
-    return -1;
+    distance = -1;
+    return false;
   }
   /******** PARSE THROUGH THE DATA READ FROM SERIAL*********/
   else {
@@ -110,15 +117,15 @@ float URM04Sensor::URM04Sensor::read_distance() {
 
     // check if the checksum is incorrect
     if (checkSum != cmdst[7]) {
-      success = false;
-      // return -1 if checksum failed
-      return -1;
+      // return false if checksum failed
+      distance = -1;
+      return false;
     } else {
       // get distance from sensor
-      m_distance = (float)(cmdst[5] << 8) + (float)cmdst[6];
+      distance = (float)(cmdst[5] << 8) + (float)cmdst[6];
       // --------------------- or --------------------------
-      // m_distance = (float)(cmdst[5]*256) + (float)cmdst[6];
-      return m_distance;
+      // distance = (float)(cmdst[5]*256) + (float)cmdst[6];
+      return true;
     }
   }
 }
\ No newline at end of file

From 631162c077b14c717e1c1ea6b718d3776887c9b2 Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Sun, 3 Jan 2021 23:26:28 -0500
Subject: [PATCH 07/15] Seventh Commit - Cindy's Requested Changes

---
 lib/sensors/include/URM04Sensor.h |  4 ++
 lib/sensors/src/URM04Sensor.cpp   | 67 +++++++++++++++++++++++++++++++
 2 files changed, 71 insertions(+)

diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
index 5bf641228..46ab9d1c8 100644
--- a/lib/sensors/include/URM04Sensor.h
+++ b/lib/sensors/include/URM04Sensor.h
@@ -33,6 +33,10 @@ class URM04Sensor {
   // Read Distance in CENTIMETER returns true if successful read
   // pass by reference a variable to hold the distance
   bool read_distance(float& distance);
+
+  // return true if address has been changed successfully
+  // takes in a address as as paramater
+  bool set_address(uint8_t _address);
 };
 
 }  // namespace URM04Sensor
\ No newline at end of file
diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index 24d08fce2..0cf50e7f9 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -128,4 +128,71 @@ bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
       return true;
     }
   }
+  // flush serial
+  serial.sync();
+}
+
+bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
+  /************ INSTANTIATE COMMANDS TO BE SENT OVER SERIAL************/
+  // reset the command array with all zeros
+  memset(&cmdst[0], 0, sizeof(cmdst));
+  // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
+  uint8_t checkSum;
+  // buffer header
+  cmdst[0] = 0x55;
+  cmdst[1] = 0xAA;
+  // device address
+  cmdst[2] = startAddr;
+  // command length
+  cmdst[3] = 0x01;
+  // the command itself
+  cmdst[4] = 0x55;
+  // set new Address
+  cmdst[5] = _address;
+  // compute checksum bit
+  checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4] + cmdst[5];
+  // instantiate the last element in the command buffer with checksum value
+  cmdst[6] = checkSum;
+
+  /************** SEND COMMANDS OVER SERIAL***********/
+  int w_num_bytes;
+  // send command over serial - write
+  w_num_bytes = serial.write(&cmdst[0], sizeof(cmdst));
+  // check if command was successfully written to the serial
+  if (w_num_bytes < 0 || w_num_bytes != 7) {
+    return false;
+  }
+  // flush the serial
+  serial.sync();
+  // clean buffer
+  memset(&cmdst[0], 0, sizeof(cmdst));
+
+  /************* PARSE THROUGH DATA RECIEVED***********/
+  int r_num_bytes;
+  // read return value from serial
+  r_num_bytes = serial.read(&cmdst[0], sizeof(cmdst));
+  // check if read is successful
+  if (r_num_bytes < 0 || r_num_bytes != 7) {
+    return false;
+  }
+  // check the checksum
+  checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4] + cmdst[5];
+  // check if the checksum is incorrect
+  if (checkSum != cmdst[6]) {
+    return false;
+  } else {
+    // check if the command went through successfully by checking if the
+    // 6th element of the return buffer array == 0x01
+    if (cmdst[5] == 0x01) {
+      // new address is set
+      startAddr = _address;
+
+      // clean up
+      serial.sync();
+      memset(&cmdst[0], 0, sizeof(cmdst));
+
+      return true;
+    }
+    return false;
+  }
 }
\ No newline at end of file

From 5a62e19905e0f57d156b0029d4af135d9ce98a97 Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Mon, 4 Jan 2021 11:53:06 -0500
Subject: [PATCH 08/15] Eigth Commit - Cindy's Requested Changes (Updated
 Constructor) PT2

---
 lib/sensors/include/URM04Sensor.h | 4 +++-
 lib/sensors/src/URM04Sensor.cpp   | 6 +++---
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
index 46ab9d1c8..f513ade31 100644
--- a/lib/sensors/include/URM04Sensor.h
+++ b/lib/sensors/include/URM04Sensor.h
@@ -28,7 +28,9 @@ class URM04Sensor {
 
  public:
   // constructor
-  URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX);
+  // allows for default address of device to be changed within constructor
+  // @param default_address is a fixed parameter that is already set to the default address
+  URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX, uint8_t default_address = START_ADDRESS);
 
   // Read Distance in CENTIMETER returns true if successful read
   // pass by reference a variable to hold the distance
diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index 0cf50e7f9..40a517d40 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -1,8 +1,8 @@
 #include "URM04Sensor.h"
 
 // instantiate pin connected to the URM04 URM04Sensor
-URM04Sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX)
-    : m_trigPin(trig_pin), startAddr(START_ADDRESS), RX(_RX), TX(_TX), serial(TX, RX, BAUD_RATE) {
+URM04Sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX, uint8_t default_address)
+    : m_trigPin(trig_pin), startAddr(default_address), RX(_RX), TX(_TX), serial(TX, RX, BAUD_RATE) {
   // write low to pin to start instructions
   m_trigPin.write(LOW);
 
@@ -24,7 +24,7 @@ void URM04Sensor::URM04Sensor::trigger_sensor(float& distance) {
   // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
   uint8_t checkSum;
   // buffer header
-  cmdst[0] = 0x055;
+  cmdst[0] = 0x55;
   cmdst[1] = 0xAA;
   // device address
   cmdst[2] = startAddr;

From 30ef9900f7fec6e7882a13beafed0a13d2c4bb5f Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Mon, 4 Jan 2021 16:49:39 -0500
Subject: [PATCH 09/15] Ninth Commit - Added RS485 TX/RX Mode with TriggerPin

---
 lib/sensors/include/URM04Sensor.h |  1 +
 lib/sensors/src/URM04Sensor.cpp   | 21 +++++++++++++++++----
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
index f513ade31..c4b65cc6a 100644
--- a/lib/sensors/include/URM04Sensor.h
+++ b/lib/sensors/include/URM04Sensor.h
@@ -10,6 +10,7 @@ class URM04Sensor {
   static constexpr int BAUD_RATE     = 19200;
   static constexpr int START_ADDRESS = 0x11;
   static constexpr int LOW           = 0;
+  static constexpr int HIGH          = 1;
 
  private:
   // trigger pin
diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index 40a517d40..2a83965cb 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -3,9 +3,6 @@
 // instantiate pin connected to the URM04 URM04Sensor
 URM04Sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX, uint8_t default_address)
     : m_trigPin(trig_pin), startAddr(default_address), RX(_RX), TX(_TX), serial(TX, RX, BAUD_RATE) {
-  // write low to pin to start instructions
-  m_trigPin.write(LOW);
-
   // instantiate command buffer with all zeros
 
   /***
@@ -14,11 +11,13 @@ URM04Sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX
    * }
    *
    ***/
-
   memset(&cmdst[0], 0, sizeof(cmdst));
 }
 
 void URM04Sensor::URM04Sensor::trigger_sensor(float& distance) {
+  // turn on transmitting mode for RS485
+  m_trigPin.write(HIGH);
+  ThisThread::sleep_for(1ms);
   /************ INSTANTIATE COMMANDS TO BE SENT OVER SERIAL*********/
 
   // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
@@ -101,6 +100,11 @@ bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
   // reset command buffer - fill whole array with zeros
   memset(&cmdst[0], 0, sizeof(cmdst));
   /******* READ RETURN VALUE FROM SERIAL**************/
+
+  // turn on reading mode for RS485
+  m_trigPin.write(LOW);
+  ThisThread::sleep_for(1ms);
+
   int r_num_bytes;
   // returns the number of bytes if successful read
   r_num_bytes = serial.read(&cmdst[0], sizeof(cmdst));
@@ -133,6 +137,10 @@ bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
 }
 
 bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
+  // turn on RS485 transmit mode
+  m_trigPin.write(HIGH);
+  ThisThread::sleep_for(1ms);
+
   /************ INSTANTIATE COMMANDS TO BE SENT OVER SERIAL************/
   // reset the command array with all zeros
   memset(&cmdst[0], 0, sizeof(cmdst));
@@ -168,6 +176,11 @@ bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
   memset(&cmdst[0], 0, sizeof(cmdst));
 
   /************* PARSE THROUGH DATA RECIEVED***********/
+
+  // turn on RS485 reading mode
+  m_trigPin.write(LOW);
+  ThisThread::sleep_for(1ms);
+
   int r_num_bytes;
   // read return value from serial
   r_num_bytes = serial.read(&cmdst[0], sizeof(cmdst));

From c4541c02545802255abf8b12ed6b657c2741a44f Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Mon, 4 Jan 2021 17:21:43 -0500
Subject: [PATCH 10/15] Ninth Commit PT2 - Error checking for when distance
 read is out of range

---
 apps/test-urm04/src/main.cpp      | 13 ++++++++++---
 lib/sensors/include/URM04Sensor.h |  3 +++
 lib/sensors/src/URM04Sensor.cpp   |  7 ++++++-
 3 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/apps/test-urm04/src/main.cpp b/apps/test-urm04/src/main.cpp
index 5fef4cfe1..2eb1ecdda 100644
--- a/apps/test-urm04/src/main.cpp
+++ b/apps/test-urm04/src/main.cpp
@@ -6,11 +6,18 @@ int main() {
   URM04Sensor::URM04Sensor sensor(D2, D0, D1);
   float get_distance;
   while (1) {
-    sensor.read_distance(get_distance);
+    sensor.read_distance(get_distance);  // measurements returned in Centimeters
     // wait 10 milliseconds
     ThisThread::sleep_for(10ms);
-    printf("Distance: %f cm\n", get_distance);
-  }
 
+    /************* PRINT DISTANCE **************/
+    if (get_distance > 10000000) {
+      printf("Sensor is out of range");
+    } else if (get_distance == -1) {
+      printf("ERROR");
+    } else {
+      printf("Distance: %f cm\n", get_distance);
+    }
+  }
   return 0;
 }
\ No newline at end of file
diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
index c4b65cc6a..4710b75ac 100644
--- a/lib/sensors/include/URM04Sensor.h
+++ b/lib/sensors/include/URM04Sensor.h
@@ -11,6 +11,7 @@ class URM04Sensor {
   static constexpr int START_ADDRESS = 0x11;
   static constexpr int LOW           = 0;
   static constexpr int HIGH          = 1;
+  static constexpr float MAX_FLOAT   = 1e+37;
 
  private:
   // trigger pin
@@ -35,6 +36,8 @@ class URM04Sensor {
 
   // Read Distance in CENTIMETER returns true if successful read
   // pass by reference a variable to hold the distance
+  // sensors range is 4cm - 500cm
+  // will give MAX_FLOAT value if out of range
   bool read_distance(float& distance);
 
   // return true if address has been changed successfully
diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index 2a83965cb..1cd60081a 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -60,7 +60,7 @@ void URM04Sensor::URM04Sensor::trigger_sensor(float& distance) {
   memset(&cmdst[0], 0, sizeof(cmdst));
 }
 
-// pass by reference a variable to hold the distance value
+// pass by reference a variable to hold the distance value - will give MAX_FLOAT if out of range
 bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
   /****************** TRIGGER SENSOR BEFORE READING DISTANCE ********************/
   trigger_sensor(distance);
@@ -129,6 +129,11 @@ bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
       distance = (float)(cmdst[5] << 8) + (float)cmdst[6];
       // --------------------- or --------------------------
       // distance = (float)(cmdst[5]*256) + (float)cmdst[6];
+
+      // check if distance is out of range - if so return maximum float value
+      if ((int)distance % (int)cmdst[5] == (int)cmdst[6]) {
+        distance = MAX_FLOAT;
+      }
       return true;
     }
   }

From b32161157ef1542958ac1b1c551078d2190d8d22 Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Sun, 10 Jan 2021 00:20:36 -0500
Subject: [PATCH 11/15] Tenth Commit - Added Cindy's Requested Changes

---
 apps/test-urm04/src/main.cpp      | 12 ++++----
 lib/sensors/include/URM04Sensor.h |  4 +--
 lib/sensors/src/URM04Sensor.cpp   | 49 ++++++++++++++-----------------
 3 files changed, 30 insertions(+), 35 deletions(-)

diff --git a/apps/test-urm04/src/main.cpp b/apps/test-urm04/src/main.cpp
index 2eb1ecdda..9ad354ca2 100644
--- a/apps/test-urm04/src/main.cpp
+++ b/apps/test-urm04/src/main.cpp
@@ -4,20 +4,20 @@ int main() {
   // D1 - TX
   // D0 - RX
   URM04Sensor::URM04Sensor sensor(D2, D0, D1);
-  float get_distance;
+  float distance_cm;
   while (1) {
-    sensor.read_distance(get_distance);  // measurements returned in Centimeters
+    sensor.read_distance(distance_cm);  // measurements returned in Centimeters
     // wait 10 milliseconds
     ThisThread::sleep_for(10ms);
 
     /************* PRINT DISTANCE **************/
-    if (get_distance > 10000000) {
+    if (distance_cm > 10000000) {
       printf("Sensor is out of range");
-    } else if (get_distance == -1) {
+    } else if (distance_cm == -1) {
       printf("ERROR");
     } else {
-      printf("Distance: %f cm\n", get_distance);
+      printf("Distance: %f cm\n", distance_cm);
     }
   }
   return 0;
-}
\ No newline at end of file
+}
diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
index 4710b75ac..3b2f33411 100644
--- a/lib/sensors/include/URM04Sensor.h
+++ b/lib/sensors/include/URM04Sensor.h
@@ -26,7 +26,7 @@ class URM04Sensor {
   // serial
   BufferedSerial serial;
   // Trigger Sensor
-  void trigger_sensor(float& distance);
+  bool trigger_sensor();
 
  public:
   // constructor
@@ -45,4 +45,4 @@ class URM04Sensor {
   bool set_address(uint8_t _address);
 };
 
-}  // namespace URM04Sensor
\ No newline at end of file
+}  // namespace URM04Sensor
diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index 1cd60081a..bfa6eb176 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -5,16 +5,10 @@ URM04Sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX
     : m_trigPin(trig_pin), startAddr(default_address), RX(_RX), TX(_TX), serial(TX, RX, BAUD_RATE) {
   // instantiate command buffer with all zeros
 
-  /***
-   * for(int i{0}; i<10; i++){
-   *   cmdst[i] = 0;
-   * }
-   *
-   ***/
   memset(&cmdst[0], 0, sizeof(cmdst));
 }
 
-void URM04Sensor::URM04Sensor::trigger_sensor(float& distance) {
+bool URM04Sensor::URM04Sensor::trigger_sensor() {
   // turn on transmitting mode for RS485
   m_trigPin.write(HIGH);
   ThisThread::sleep_for(1ms);
@@ -39,31 +33,32 @@ void URM04Sensor::URM04Sensor::trigger_sensor(float& distance) {
   /******** SEND COMMANDS OVER SERIAL *********************/
   int num_bytes;
   // returns the number of bytes if successful write
-  num_bytes = serial.write(&cmdst[0], sizeof(cmdst));
+  num_bytes = serial.write(&cmdst[0], 6);
 
   // else if numb_btyes < 0 serial failed or if number of bytes written != 6 (because 6 bytes sent over serial)
   if (num_bytes < 0 || num_bytes != 6) {
     // error occured in trigger step
-    distance = -1;
+    return false;
   } else {
     // no error occured in trigger step
-    distance = 0;
+    // flush the buffer from serial
+    serial.sync();
+    // wait for at least 30 ms after calling trigger function
+    ThisThread::sleep_for(35ms);
+    // reset command buffer - fill whole array with zeros
+    memset(&cmdst[0], 0, sizeof(cmdst));
+    return true;
   }
-
-  // flush the buffer from serial
-  serial.sync();
-
-  // wait for at least 30 ms after calling trigger function
-  ThisThread::sleep_for(35ms);
-
-  // reset command buffer - fill whole array with zeros
-  memset(&cmdst[0], 0, sizeof(cmdst));
 }
 
 // pass by reference a variable to hold the distance value - will give MAX_FLOAT if out of range
 bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
   /****************** TRIGGER SENSOR BEFORE READING DISTANCE ********************/
-  trigger_sensor(distance);
+  // if trigger fails set distance to -1 and retuen false
+  if (trigger_sensor() == false) {
+    distance = -1;
+    return false;
+  }
 
   /************ INSTANTIATE COMMANDS TO BE SENT OVER SERIAL************/
   // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
@@ -85,7 +80,7 @@ bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
   /************** SEND COMMANDS OVER SERIAL***********/
   int w_num_bytes;
   // returns the number of bytes if successful read
-  w_num_bytes = serial.write(&cmdst[0], sizeof(cmdst));
+  w_num_bytes = serial.write(&cmdst[0], 6);
 
   // else if numb_btyes < 0 serial failed or if number of bytes read != 6 (because 6 bytes sent over serial)
   if (w_num_bytes < 0 || w_num_bytes != 6) {
@@ -107,7 +102,7 @@ bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
 
   int r_num_bytes;
   // returns the number of bytes if successful read
-  r_num_bytes = serial.read(&cmdst[0], sizeof(cmdst));
+  r_num_bytes = serial.read(&cmdst[0], 8);
 
   // else if numb_btyes < 0 serial failed or if number of bytes read != 8 (because 8 bytes sent over serial)
   if (r_num_bytes < 0 || r_num_bytes != 8) {
@@ -131,7 +126,7 @@ bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
       // distance = (float)(cmdst[5]*256) + (float)cmdst[6];
 
       // check if distance is out of range - if so return maximum float value
-      if ((int)distance % (int)cmdst[5] == (int)cmdst[6]) {
+      if (cmdst[5] == 0xFF && cmdst[6] == 0xFF) {
         distance = MAX_FLOAT;
       }
       return true;
@@ -155,7 +150,7 @@ bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
   cmdst[0] = 0x55;
   cmdst[1] = 0xAA;
   // device address
-  cmdst[2] = startAddr;
+  cmdst[2] = 0xAB;
   // command length
   cmdst[3] = 0x01;
   // the command itself
@@ -170,7 +165,7 @@ bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
   /************** SEND COMMANDS OVER SERIAL***********/
   int w_num_bytes;
   // send command over serial - write
-  w_num_bytes = serial.write(&cmdst[0], sizeof(cmdst));
+  w_num_bytes = serial.write(&cmdst[0], 7);
   // check if command was successfully written to the serial
   if (w_num_bytes < 0 || w_num_bytes != 7) {
     return false;
@@ -188,7 +183,7 @@ bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
 
   int r_num_bytes;
   // read return value from serial
-  r_num_bytes = serial.read(&cmdst[0], sizeof(cmdst));
+  r_num_bytes = serial.read(&cmdst[0], 7);
   // check if read is successful
   if (r_num_bytes < 0 || r_num_bytes != 7) {
     return false;
@@ -213,4 +208,4 @@ bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
     }
     return false;
   }
-}
\ No newline at end of file
+}

From 99c8d4d8c3fb33325f30309d6780600195d6090c Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Mon, 11 Jan 2021 22:21:51 -0500
Subject: [PATCH 12/15] Minor Changes to Constructor/ Serial Communication /
 Command Buffer - URM04 Driver

---
 lib/sensors/include/URM04Sensor.h |  13 ++--
 lib/sensors/src/URM04Sensor.cpp   | 113 ++++++++++++------------------
 2 files changed, 51 insertions(+), 75 deletions(-)

diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
index 3b2f33411..b3abdcec8 100644
--- a/lib/sensors/include/URM04Sensor.h
+++ b/lib/sensors/include/URM04Sensor.h
@@ -11,20 +11,17 @@ class URM04Sensor {
   static constexpr int START_ADDRESS = 0x11;
   static constexpr int LOW           = 0;
   static constexpr int HIGH          = 1;
-  static constexpr float MAX_FLOAT   = 1e+37;
+  static constexpr float MAX_FLOAT   = std::numeric_limits<float>::max();
 
  private:
   // trigger pin
   DigitalOut m_trigPin;
   // start address
-  uint8_t startAddr;
+  uint8_t m_startAddr;
   // command buffer
-  uint8_t cmdst[10];
-  // UART protocol pins
-  PinName RX;
-  PinName TX;
+  uint8_t m_cmdst[8];
   // serial
-  BufferedSerial serial;
+  BufferedSerial m_serial;
   // Trigger Sensor
   bool trigger_sensor();
 
@@ -41,7 +38,7 @@ class URM04Sensor {
   bool read_distance(float& distance);
 
   // return true if address has been changed successfully
-  // takes in a address as as paramater
+  // takes in the new address to set to as paramater
   bool set_address(uint8_t _address);
 };
 
diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index bfa6eb176..9f93a6228 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -2,10 +2,7 @@
 
 // instantiate pin connected to the URM04 URM04Sensor
 URM04Sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX, uint8_t default_address)
-    : m_trigPin(trig_pin), startAddr(default_address), RX(_RX), TX(_TX), serial(TX, RX, BAUD_RATE) {
-  // instantiate command buffer with all zeros
-
-  memset(&cmdst[0], 0, sizeof(cmdst));
+    : m_trigPin(trig_pin), m_startAddr(default_address), m_serial(_TX, _RX, BAUD_RATE) {
 }
 
 bool URM04Sensor::URM04Sensor::trigger_sensor() {
@@ -17,36 +14,33 @@ bool URM04Sensor::URM04Sensor::trigger_sensor() {
   // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
   uint8_t checkSum;
   // buffer header
-  cmdst[0] = 0x55;
-  cmdst[1] = 0xAA;
+  m_cmdst[0] = 0x55;
+  m_cmdst[1] = 0xAA;
   // device address
-  cmdst[2] = startAddr;
+  m_cmdst[2] = m_startAddr;
   // length
-  cmdst[3] = 0x00;
+  m_cmdst[3] = 0x00;
   // the command itself
-  cmdst[4] = 0x01;
+  m_cmdst[4] = 0x01;
   // checksum bit
-  checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4];
+  checkSum = m_cmdst[0] + m_cmdst[1] + m_cmdst[2] + m_cmdst[3] + m_cmdst[4];
   // instantiate the last element in the command buffer with checksum value
-  cmdst[5] = checkSum;
+  m_cmdst[5] = checkSum;
 
   /******** SEND COMMANDS OVER SERIAL *********************/
-  int num_bytes;
   // returns the number of bytes if successful write
-  num_bytes = serial.write(&cmdst[0], 6);
+  int num_bytes = m_serial.write(&m_cmdst[0], 6);
 
   // else if numb_btyes < 0 serial failed or if number of bytes written != 6 (because 6 bytes sent over serial)
-  if (num_bytes < 0 || num_bytes != 6) {
+  if (num_bytes != 6) {
     // error occured in trigger step
     return false;
   } else {
     // no error occured in trigger step
     // flush the buffer from serial
-    serial.sync();
+    m_serial.sync();
     // wait for at least 30 ms after calling trigger function
     ThisThread::sleep_for(35ms);
-    // reset command buffer - fill whole array with zeros
-    memset(&cmdst[0], 0, sizeof(cmdst));
     return true;
   }
 }
@@ -54,7 +48,7 @@ bool URM04Sensor::URM04Sensor::trigger_sensor() {
 // pass by reference a variable to hold the distance value - will give MAX_FLOAT if out of range
 bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
   /****************** TRIGGER SENSOR BEFORE READING DISTANCE ********************/
-  // if trigger fails set distance to -1 and retuen false
+  // if trigger fails set distance to -1 and return false
   if (trigger_sensor() == false) {
     distance = -1;
     return false;
@@ -64,76 +58,70 @@ bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
   // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
   uint8_t checkSum;
   // buffer header
-  cmdst[0] = 0x55;
-  cmdst[1] = 0xAA;
+  m_cmdst[0] = 0x55;
+  m_cmdst[1] = 0xAA;
   // device address
-  cmdst[2] = startAddr;
+  m_cmdst[2] = m_startAddr;
   // command length
-  cmdst[3] = 0x00;
+  m_cmdst[3] = 0x00;
   // the command itself
-  cmdst[4] = 0x02;
+  m_cmdst[4] = 0x02;
   // checksum bit
-  checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4];
+  checkSum = m_cmdst[0] + m_cmdst[1] + m_cmdst[2] + m_cmdst[3] + m_cmdst[4];
   // instantiate the last element in the command buffer with checksum value
-  cmdst[5] = checkSum;
+  m_cmdst[5] = checkSum;
 
   /************** SEND COMMANDS OVER SERIAL***********/
-  int w_num_bytes;
   // returns the number of bytes if successful read
-  w_num_bytes = serial.write(&cmdst[0], 6);
+  int w_num_bytes = m_serial.write(&m_cmdst[0], 6);
 
   // else if numb_btyes < 0 serial failed or if number of bytes read != 6 (because 6 bytes sent over serial)
-  if (w_num_bytes < 0 || w_num_bytes != 6) {
+  if (w_num_bytes != 6) {
     // return false indicating read command has failed
     distance = -1;
     return false;
   }
 
   // flush buffer from serial
-  serial.sync();
+  m_serial.sync();
 
-  // reset command buffer - fill whole array with zeros
-  memset(&cmdst[0], 0, sizeof(cmdst));
   /******* READ RETURN VALUE FROM SERIAL**************/
 
   // turn on reading mode for RS485
   m_trigPin.write(LOW);
   ThisThread::sleep_for(1ms);
 
-  int r_num_bytes;
   // returns the number of bytes if successful read
-  r_num_bytes = serial.read(&cmdst[0], 8);
+  int r_num_bytes = m_serial.read(&m_cmdst[0], 8);
 
   // else if numb_btyes < 0 serial failed or if number of bytes read != 8 (because 8 bytes sent over serial)
-  if (r_num_bytes < 0 || r_num_bytes != 8) {
+  if (r_num_bytes != 8) {
     distance = -1;
     return false;
   }
   /******** PARSE THROUGH THE DATA READ FROM SERIAL*********/
   else {
-    // create the checksum
-    checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4] + cmdst[5] + cmdst[6];
 
     // check if the checksum is incorrect
-    if (checkSum != cmdst[7]) {
+    if (m_cmdst[7] != m_cmdst[0] + m_cmdst[1] + m_cmdst[2] + m_cmdst[3] + m_cmdst[4] + m_cmdst[5] + m_cmdst[6]) {
       // return false if checksum failed
       distance = -1;
       return false;
     } else {
       // get distance from sensor
-      distance = (float)(cmdst[5] << 8) + (float)cmdst[6];
+      distance = (float)(m_cmdst[5] << 8) + (float)m_cmdst[6];
       // --------------------- or --------------------------
-      // distance = (float)(cmdst[5]*256) + (float)cmdst[6];
+      // distance = (float)(m_cmdst[5]*256) + (float)m_cmdst[6];
 
       // check if distance is out of range - if so return maximum float value
-      if (cmdst[5] == 0xFF && cmdst[6] == 0xFF) {
+      if (m_cmdst[5] == 0xFF && m_cmdst[6] == 0xFF) {
         distance = MAX_FLOAT;
       }
       return true;
     }
   }
   // flush serial
-  serial.sync();
+  m_serial.sync();
 }
 
 bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
@@ -142,38 +130,33 @@ bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
   ThisThread::sleep_for(1ms);
 
   /************ INSTANTIATE COMMANDS TO BE SENT OVER SERIAL************/
-  // reset the command array with all zeros
-  memset(&cmdst[0], 0, sizeof(cmdst));
   // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
   uint8_t checkSum;
   // buffer header
-  cmdst[0] = 0x55;
-  cmdst[1] = 0xAA;
+  m_cmdst[0] = 0x55;
+  m_cmdst[1] = 0xAA;
   // device address
-  cmdst[2] = 0xAB;
+  m_cmdst[2] = 0xAB;
   // command length
-  cmdst[3] = 0x01;
+  m_cmdst[3] = 0x01;
   // the command itself
-  cmdst[4] = 0x55;
+  m_cmdst[4] = 0x55;
   // set new Address
-  cmdst[5] = _address;
+  m_cmdst[5] = _address;
   // compute checksum bit
-  checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4] + cmdst[5];
+  checkSum = m_cmdst[0] + m_cmdst[1] + m_cmdst[2] + m_cmdst[3] + m_cmdst[4] + m_cmdst[5];
   // instantiate the last element in the command buffer with checksum value
-  cmdst[6] = checkSum;
+  m_cmdst[6] = checkSum;
 
   /************** SEND COMMANDS OVER SERIAL***********/
-  int w_num_bytes;
   // send command over serial - write
-  w_num_bytes = serial.write(&cmdst[0], 7);
+  int w_num_bytes = m_serial.write(&m_cmdst[0], 7);
   // check if command was successfully written to the serial
-  if (w_num_bytes < 0 || w_num_bytes != 7) {
+  if (w_num_bytes != 7) {
     return false;
   }
   // flush the serial
-  serial.sync();
-  // clean buffer
-  memset(&cmdst[0], 0, sizeof(cmdst));
+  m_serial.sync();
 
   /************* PARSE THROUGH DATA RECIEVED***********/
 
@@ -181,28 +164,24 @@ bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
   m_trigPin.write(LOW);
   ThisThread::sleep_for(1ms);
 
-  int r_num_bytes;
   // read return value from serial
-  r_num_bytes = serial.read(&cmdst[0], 7);
+  int r_num_bytes = m_serial.read(&m_cmdst[0], 7);
   // check if read is successful
-  if (r_num_bytes < 0 || r_num_bytes != 7) {
+  if (r_num_bytes != 7) {
     return false;
   }
-  // check the checksum
-  checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4] + cmdst[5];
   // check if the checksum is incorrect
-  if (checkSum != cmdst[6]) {
+  if (m_cmdst[6] != m_cmdst[0] + m_cmdst[1] + m_cmdst[2] + m_cmdst[3] + m_cmdst[4] + m_cmdst[5]) {
     return false;
   } else {
     // check if the command went through successfully by checking if the
     // 6th element of the return buffer array == 0x01
-    if (cmdst[5] == 0x01) {
+    if (m_cmdst[5] == 0x01) {
       // new address is set
-      startAddr = _address;
+      m_startAddr = _address;
 
       // clean up
-      serial.sync();
-      memset(&cmdst[0], 0, sizeof(cmdst));
+      m_serial.sync();
 
       return true;
     }

From 162e05ccf1bb9751876377ba217453b2e26ceb12 Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Mon, 11 Jan 2021 22:21:51 -0500
Subject: [PATCH 13/15] Minor Changes to Constructor/ Serial Communication /
 Command Buffer - URM04 Driver

---
 lib/sensors/include/URM04Sensor.h |  13 ++--
 lib/sensors/src/URM04Sensor.cpp   | 115 ++++++++++++------------------
 2 files changed, 51 insertions(+), 77 deletions(-)

diff --git a/lib/sensors/include/URM04Sensor.h b/lib/sensors/include/URM04Sensor.h
index 3b2f33411..b3abdcec8 100644
--- a/lib/sensors/include/URM04Sensor.h
+++ b/lib/sensors/include/URM04Sensor.h
@@ -11,20 +11,17 @@ class URM04Sensor {
   static constexpr int START_ADDRESS = 0x11;
   static constexpr int LOW           = 0;
   static constexpr int HIGH          = 1;
-  static constexpr float MAX_FLOAT   = 1e+37;
+  static constexpr float MAX_FLOAT   = std::numeric_limits<float>::max();
 
  private:
   // trigger pin
   DigitalOut m_trigPin;
   // start address
-  uint8_t startAddr;
+  uint8_t m_startAddr;
   // command buffer
-  uint8_t cmdst[10];
-  // UART protocol pins
-  PinName RX;
-  PinName TX;
+  uint8_t m_cmdst[8];
   // serial
-  BufferedSerial serial;
+  BufferedSerial m_serial;
   // Trigger Sensor
   bool trigger_sensor();
 
@@ -41,7 +38,7 @@ class URM04Sensor {
   bool read_distance(float& distance);
 
   // return true if address has been changed successfully
-  // takes in a address as as paramater
+  // takes in the new address to set to as paramater
   bool set_address(uint8_t _address);
 };
 
diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index bfa6eb176..87b415af0 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -2,11 +2,7 @@
 
 // instantiate pin connected to the URM04 URM04Sensor
 URM04Sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX, uint8_t default_address)
-    : m_trigPin(trig_pin), startAddr(default_address), RX(_RX), TX(_TX), serial(TX, RX, BAUD_RATE) {
-  // instantiate command buffer with all zeros
-
-  memset(&cmdst[0], 0, sizeof(cmdst));
-}
+    : m_trigPin(trig_pin), m_startAddr(default_address), m_serial(_TX, _RX, BAUD_RATE) {}
 
 bool URM04Sensor::URM04Sensor::trigger_sensor() {
   // turn on transmitting mode for RS485
@@ -17,36 +13,33 @@ bool URM04Sensor::URM04Sensor::trigger_sensor() {
   // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
   uint8_t checkSum;
   // buffer header
-  cmdst[0] = 0x55;
-  cmdst[1] = 0xAA;
+  m_cmdst[0] = 0x55;
+  m_cmdst[1] = 0xAA;
   // device address
-  cmdst[2] = startAddr;
+  m_cmdst[2] = m_startAddr;
   // length
-  cmdst[3] = 0x00;
+  m_cmdst[3] = 0x00;
   // the command itself
-  cmdst[4] = 0x01;
+  m_cmdst[4] = 0x01;
   // checksum bit
-  checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4];
+  checkSum = m_cmdst[0] + m_cmdst[1] + m_cmdst[2] + m_cmdst[3] + m_cmdst[4];
   // instantiate the last element in the command buffer with checksum value
-  cmdst[5] = checkSum;
+  m_cmdst[5] = checkSum;
 
   /******** SEND COMMANDS OVER SERIAL *********************/
-  int num_bytes;
   // returns the number of bytes if successful write
-  num_bytes = serial.write(&cmdst[0], 6);
+  int num_bytes = m_serial.write(&m_cmdst[0], 6);
 
   // else if numb_btyes < 0 serial failed or if number of bytes written != 6 (because 6 bytes sent over serial)
-  if (num_bytes < 0 || num_bytes != 6) {
+  if (num_bytes != 6) {
     // error occured in trigger step
     return false;
   } else {
     // no error occured in trigger step
     // flush the buffer from serial
-    serial.sync();
+    m_serial.sync();
     // wait for at least 30 ms after calling trigger function
     ThisThread::sleep_for(35ms);
-    // reset command buffer - fill whole array with zeros
-    memset(&cmdst[0], 0, sizeof(cmdst));
     return true;
   }
 }
@@ -54,7 +47,7 @@ bool URM04Sensor::URM04Sensor::trigger_sensor() {
 // pass by reference a variable to hold the distance value - will give MAX_FLOAT if out of range
 bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
   /****************** TRIGGER SENSOR BEFORE READING DISTANCE ********************/
-  // if trigger fails set distance to -1 and retuen false
+  // if trigger fails set distance to -1 and return false
   if (trigger_sensor() == false) {
     distance = -1;
     return false;
@@ -64,76 +57,69 @@ bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
   // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
   uint8_t checkSum;
   // buffer header
-  cmdst[0] = 0x55;
-  cmdst[1] = 0xAA;
+  m_cmdst[0] = 0x55;
+  m_cmdst[1] = 0xAA;
   // device address
-  cmdst[2] = startAddr;
+  m_cmdst[2] = m_startAddr;
   // command length
-  cmdst[3] = 0x00;
+  m_cmdst[3] = 0x00;
   // the command itself
-  cmdst[4] = 0x02;
+  m_cmdst[4] = 0x02;
   // checksum bit
-  checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4];
+  checkSum = m_cmdst[0] + m_cmdst[1] + m_cmdst[2] + m_cmdst[3] + m_cmdst[4];
   // instantiate the last element in the command buffer with checksum value
-  cmdst[5] = checkSum;
+  m_cmdst[5] = checkSum;
 
   /************** SEND COMMANDS OVER SERIAL***********/
-  int w_num_bytes;
   // returns the number of bytes if successful read
-  w_num_bytes = serial.write(&cmdst[0], 6);
+  int w_num_bytes = m_serial.write(&m_cmdst[0], 6);
 
   // else if numb_btyes < 0 serial failed or if number of bytes read != 6 (because 6 bytes sent over serial)
-  if (w_num_bytes < 0 || w_num_bytes != 6) {
+  if (w_num_bytes != 6) {
     // return false indicating read command has failed
     distance = -1;
     return false;
   }
 
   // flush buffer from serial
-  serial.sync();
+  m_serial.sync();
 
-  // reset command buffer - fill whole array with zeros
-  memset(&cmdst[0], 0, sizeof(cmdst));
   /******* READ RETURN VALUE FROM SERIAL**************/
 
   // turn on reading mode for RS485
   m_trigPin.write(LOW);
   ThisThread::sleep_for(1ms);
 
-  int r_num_bytes;
   // returns the number of bytes if successful read
-  r_num_bytes = serial.read(&cmdst[0], 8);
+  int r_num_bytes = m_serial.read(&m_cmdst[0], 8);
 
   // else if numb_btyes < 0 serial failed or if number of bytes read != 8 (because 8 bytes sent over serial)
-  if (r_num_bytes < 0 || r_num_bytes != 8) {
+  if (r_num_bytes != 8) {
     distance = -1;
     return false;
   }
   /******** PARSE THROUGH THE DATA READ FROM SERIAL*********/
   else {
-    // create the checksum
-    checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4] + cmdst[5] + cmdst[6];
-
     // check if the checksum is incorrect
-    if (checkSum != cmdst[7]) {
+    if (m_cmdst[7] != m_cmdst[0] + m_cmdst[1] + m_cmdst[2] + m_cmdst[3] + m_cmdst[4] + m_cmdst[5] + m_cmdst[6]) {
       // return false if checksum failed
       distance = -1;
       return false;
     } else {
       // get distance from sensor
-      distance = (float)(cmdst[5] << 8) + (float)cmdst[6];
+      distance = (float)(m_cmdst[5] << 8) + (float)m_cmdst[6];
       // --------------------- or --------------------------
-      // distance = (float)(cmdst[5]*256) + (float)cmdst[6];
+      // distance = (float)(m_cmdst[5]*256) + (float)m_cmdst[6];
 
       // check if distance is out of range - if so return maximum float value
-      if (cmdst[5] == 0xFF && cmdst[6] == 0xFF) {
+      if (m_cmdst[5] == 0xFF && m_cmdst[6] == 0xFF) {
         distance = MAX_FLOAT;
       }
       return true;
     }
   }
   // flush serial
-  serial.sync();
+  m_serial.sync();
 }
 
 bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
@@ -142,38 +128,33 @@ bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
   ThisThread::sleep_for(1ms);
 
   /************ INSTANTIATE COMMANDS TO BE SENT OVER SERIAL************/
-  // reset the command array with all zeros
-  memset(&cmdst[0], 0, sizeof(cmdst));
   // check sum represents the final bit in command buffer - made by adding all previous bits in command buffer
   uint8_t checkSum;
   // buffer header
-  cmdst[0] = 0x55;
-  cmdst[1] = 0xAA;
+  m_cmdst[0] = 0x55;
+  m_cmdst[1] = 0xAA;
   // device address
-  cmdst[2] = 0xAB;
+  m_cmdst[2] = 0xAB;
   // command length
-  cmdst[3] = 0x01;
+  m_cmdst[3] = 0x01;
   // the command itself
-  cmdst[4] = 0x55;
+  m_cmdst[4] = 0x55;
   // set new Address
-  cmdst[5] = _address;
+  m_cmdst[5] = _address;
   // compute checksum bit
-  checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4] + cmdst[5];
+  checkSum = m_cmdst[0] + m_cmdst[1] + m_cmdst[2] + m_cmdst[3] + m_cmdst[4] + m_cmdst[5];
   // instantiate the last element in the command buffer with checksum value
-  cmdst[6] = checkSum;
+  m_cmdst[6] = checkSum;
 
   /************** SEND COMMANDS OVER SERIAL***********/
-  int w_num_bytes;
   // send command over serial - write
-  w_num_bytes = serial.write(&cmdst[0], 7);
+  int w_num_bytes = m_serial.write(&m_cmdst[0], 7);
   // check if command was successfully written to the serial
-  if (w_num_bytes < 0 || w_num_bytes != 7) {
+  if (w_num_bytes != 7) {
     return false;
   }
   // flush the serial
-  serial.sync();
-  // clean buffer
-  memset(&cmdst[0], 0, sizeof(cmdst));
+  m_serial.sync();
 
   /************* PARSE THROUGH DATA RECIEVED***********/
 
@@ -181,28 +162,24 @@ bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {
   m_trigPin.write(LOW);
   ThisThread::sleep_for(1ms);
 
-  int r_num_bytes;
   // read return value from serial
-  r_num_bytes = serial.read(&cmdst[0], 7);
+  int r_num_bytes = m_serial.read(&m_cmdst[0], 7);
   // check if read is successful
-  if (r_num_bytes < 0 || r_num_bytes != 7) {
+  if (r_num_bytes != 7) {
     return false;
   }
-  // check the checksum
-  checkSum = cmdst[0] + cmdst[1] + cmdst[2] + cmdst[3] + cmdst[4] + cmdst[5];
   // check if the checksum is incorrect
-  if (checkSum != cmdst[6]) {
+  if (m_cmdst[6] != m_cmdst[0] + m_cmdst[1] + m_cmdst[2] + m_cmdst[3] + m_cmdst[4] + m_cmdst[5]) {
     return false;
   } else {
     // check if the command went through successfully by checking if the
     // 6th element of the return buffer array == 0x01
-    if (cmdst[5] == 0x01) {
+    if (m_cmdst[5] == 0x01) {
       // new address is set
-      startAddr = _address;
+      m_startAddr = _address;
 
       // clean up
-      serial.sync();
-      memset(&cmdst[0], 0, sizeof(cmdst));
+      m_serial.sync();
 
       return true;
     }

From 9c9aa6b820b20717352e2fb4015bbafe36f606a7 Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Mon, 11 Jan 2021 22:45:50 -0500
Subject: [PATCH 14/15] Fixing Git Issues

---
 lib/sensors/src/URM04Sensor.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index 9f93a6228..87b415af0 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -2,8 +2,7 @@
 
 // instantiate pin connected to the URM04 URM04Sensor
 URM04Sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX, uint8_t default_address)
-    : m_trigPin(trig_pin), m_startAddr(default_address), m_serial(_TX, _RX, BAUD_RATE) {
-}
+    : m_trigPin(trig_pin), m_startAddr(default_address), m_serial(_TX, _RX, BAUD_RATE) {}
 
 bool URM04Sensor::URM04Sensor::trigger_sensor() {
   // turn on transmitting mode for RS485
@@ -101,7 +100,6 @@ bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
   }
   /******** PARSE THROUGH THE DATA READ FROM SERIAL*********/
   else {
-
     // check if the checksum is incorrect
     if (m_cmdst[7] != m_cmdst[0] + m_cmdst[1] + m_cmdst[2] + m_cmdst[3] + m_cmdst[4] + m_cmdst[5] + m_cmdst[6]) {
       // return false if checksum failed

From 8caefc5608a7a7ebd0184ab2431b584af001e16a Mon Sep 17 00:00:00 2001
From: niiquaye <omarmon@uwaterloo.ca>
Date: Mon, 11 Jan 2021 22:45:50 -0500
Subject: [PATCH 15/15] Removed Flushing the Serial buffer - Cindy's Requested
 Changes

	The serial was not being flushed properly as
	it was being called after function returned.
	Thus, I moved it before function returns.
---
 lib/sensors/src/URM04Sensor.cpp | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/lib/sensors/src/URM04Sensor.cpp b/lib/sensors/src/URM04Sensor.cpp
index 9f93a6228..f3191d8bd 100644
--- a/lib/sensors/src/URM04Sensor.cpp
+++ b/lib/sensors/src/URM04Sensor.cpp
@@ -2,8 +2,7 @@
 
 // instantiate pin connected to the URM04 URM04Sensor
 URM04Sensor::URM04Sensor::URM04Sensor(PinName trig_pin, PinName _RX, PinName _TX, uint8_t default_address)
-    : m_trigPin(trig_pin), m_startAddr(default_address), m_serial(_TX, _RX, BAUD_RATE) {
-}
+    : m_trigPin(trig_pin), m_startAddr(default_address), m_serial(_TX, _RX, BAUD_RATE) {}
 
 bool URM04Sensor::URM04Sensor::trigger_sensor() {
   // turn on transmitting mode for RS485
@@ -101,7 +100,6 @@ bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
   }
   /******** PARSE THROUGH THE DATA READ FROM SERIAL*********/
   else {
-
     // check if the checksum is incorrect
     if (m_cmdst[7] != m_cmdst[0] + m_cmdst[1] + m_cmdst[2] + m_cmdst[3] + m_cmdst[4] + m_cmdst[5] + m_cmdst[6]) {
       // return false if checksum failed
@@ -117,11 +115,11 @@ bool URM04Sensor::URM04Sensor::read_distance(float& distance) {
       if (m_cmdst[5] == 0xFF && m_cmdst[6] == 0xFF) {
         distance = MAX_FLOAT;
       }
+      // flush serial
+      m_serial.sync();
       return true;
     }
   }
-  // flush serial
-  m_serial.sync();
 }
 
 bool URM04Sensor::URM04Sensor::set_address(uint8_t _address) {