Closes #195: Create A Driver for URM04 Sensor

CMakeLists.txt

@@ -135,6 +135,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}")
@@ -172,3 +173,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)

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)
+

apps/test-urm04/CMakeLists.txt

@@ -0,0 +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)

apps/test-urm04/src/main.cpp

@@ -0,0 +1,23 @@
+#include "URM04Sensor.h"
+int main() {
+  // D2 - trigpin
+  // D1 - TX
+  // D0 - RX
+  URM04Sensor::URM04Sensor sensor(D2, D0, D1);
+  float get_distance;
+  while (1) {
+    sensor.read_distance(get_distance);  // measurements returned in Centimeters
+    // wait 10 milliseconds
+    ThisThread::sleep_for(10ms);
+
+    /************* 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;
+}

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)

lib/sensors/include/URM04Sensor.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include "mbed.h"
+
+namespace URM04Sensor {
+
+class URM04Sensor {
+ protected:
+  // constants
+  static constexpr int BAUD_RATE     = 19200;
+  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
+  DigitalOut m_trigPin;
+  // start address
+  uint8_t startAddr;
+  // command buffer
+  uint8_t cmdst[10];
+  // UART protocol pins
+  PinName RX;
+  PinName TX;
+  // serial
+  BufferedSerial serial;
+  // Trigger Sensor
+  void trigger_sensor(float& distance);
+
+ public:
+  // constructor
+  // 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
+  // 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
+  // takes in a address as as paramater
+  bool set_address(uint8_t _address);
+};
+
+}  // namespace URM04Sensor

lib/sensors/src/URM04Sensor.cpp

@@ -0,0 +1,216 @@
+#include "URM04Sensor.h"
+
+// 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
+
+  /***
+   * for(int i{0}; i<10; i++){
+   *   cmdst[i] = 0;
+   * }
+   *
+   ***/
+  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
+  uint8_t checkSum;
+  // buffer header
+  cmdst[0] = 0x55;
+  cmdst[1] = 0xAA;
+  // device address
+  cmdst[2] = startAddr;
+  // length
+  cmdst[3] = 0x00;
+  // the command itself
+  cmdst[4] = 0x01;
+  // 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) {
+    // 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(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);
+
+  /************ 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) {
+    // return false indicating read command has failed
+    distance = -1;
+    return false;
+  }
+
+  // flush buffer from serial
+  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], 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) {
+    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]) {
+      // return false if checksum failed
+      distance = -1;
+      return false;
+    } else {
+      // get distance from sensor
+      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;
+    }
+  }
+  // flush serial
+  serial.sync();
+}
+
+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));
+  // 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***********/
+
+  // 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));
+  // 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;
+  }
+}

supported_build_configurations.yaml

@@ -74,3 +74,6 @@ test-pwmin:
 #  - gamepad
 #  - gimbtonomy
 #  - science
+
+test-urm04:
+  - nucleo