diff --git a/arduino/leads_vec_power/Pedal.cpp b/arduino/leads_vec_power/Pedal.cpp deleted file mode 100644 index e444cbe..0000000 --- a/arduino/leads_vec_power/Pedal.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "Pedal.h" - -Pedal::Pedal(const ArrayList &pins, float restValue, float maxValue) : Device(pins), _restValue(restValue), _maxValue(maxValue) {} - -Pedal::initialize(const ArrayList &parentTags) { - Device::initialize(parentTags); - pinMode(_pins[0], INPUT); -} - -float Pedal::read() { return ((float) analogRead(_pins[0]) / 1023 - _restValue) / _maxValue; } diff --git a/arduino/leads_vec_power/Pedal.h b/arduino/leads_vec_power/Pedal.h deleted file mode 100644 index e945660..0000000 --- a/arduino/leads_vec_power/Pedal.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef PEDAL_H -#define PEDAL_H - - -#include "Device.h" - -class Pedal : public Device { -protected: - float _restValue, _maxValue; - -public: - VoltageSensor(const ArrayList &pins, float restValue, float maxValue); - void initialize(const ArrayList &parentTags) override; - float read() override; -}; - - -#endif // PEDAL_H \ No newline at end of file diff --git a/arduino/leads_vec_wsc/Accelerometer.h b/arduino/leads_vec_wsc/Accelerometer.h deleted file mode 100644 index 68915dd..0000000 --- a/arduino/leads_vec_wsc/Accelerometer.h +++ /dev/null @@ -1,23 +0,0 @@ -#include "Adafruit_BNO08x_RVC.h" -#include "LEADS.h" - - -struct Acceleration { - float yaw = 0, pitch = 0, roll = 0; - float forwardAcceleration = 0, lateralAcceleration = 0, verticalAcceleration = 0; - String toString(); -}; - - -typedef void (*OnAccelerometerUpdate)(Acceleration acceleration); - - -class Accelerometer : public Device { -protected: - Adafruit_BNO08x_RVC _rvc = Adafruit_BNO08x_RVC(); - const OnAccelerometerUpdate _onUpdate; -public: - Accelerometer(OnAccelerometerUpdate onUpdate); - void initialize(const ArrayList &parentTags) override; - Acceleration read() override; -}; diff --git a/arduino/leads_vec_wsc/Adafruit_BNO08x_RVC.cpp b/arduino/leads_vec_wsc/Adafruit_BNO08x_RVC.cpp new file mode 100644 index 0000000..356d4d3 --- /dev/null +++ b/arduino/leads_vec_wsc/Adafruit_BNO08x_RVC.cpp @@ -0,0 +1,129 @@ +/*! +* @file Adafruit_BNO08x_RVC.cpp +* +* @mainpage Adafruit BNO08x RVC A simple library to use the UART-RVC mode of +* the BNO08x sensors from Hillcrest Laboratories +* +* @section intro_sec Introduction +* +* I2C Driver for the Library for the BNO08x_RVC A simple library to use +* the UART-RVC mode of the BNO08x sensors from Hillcrest Laboratories +* +* This is a library for the Adafruit BNO08x_RVC breakout: +* https://www.adafruit.com/product/4754 +* +* Adafruit invests time and resources providing this open source code, +* please support Adafruit and open-source hardware by purchasing products from +* Adafruit! +* +* @section dependencies Dependencies +* This library depends on the Adafruit BusIO library +* +* This library depends on the Adafruit Unified Sensor library +* +* @section author Author +* +* Bryan Siepert for Adafruit Industries +* +* @section license License +* +* BSD (see license.txt) +* +* @section HISTORY +* +* v1.0 - First release +*/ + +#include "Arduino.h" +#include + +#include "Adafruit_BNO08x_RVC.h" + +/** +* @brief Construct a new Adafruit_BNO08x_RVC::Adafruit_BNO08x_RVC object +* +*/ +Adafruit_BNO08x_RVC::Adafruit_BNO08x_RVC(void) {} + +/** +* @brief Destroy the Adafruit_BNO08x_RVC::Adafruit_BNO08x_RVC object +* +*/ +Adafruit_BNO08x_RVC::~Adafruit_BNO08x_RVC(void) {} + +/*! +* @brief Setups the hardware +* @param theSerial +* Pointer to Stream (HardwareSerial/SoftwareSerial) interface +* @return True +*/ +bool Adafruit_BNO08x_RVC::begin(Stream *theSerial) { + serial_dev = theSerial; + return true; +} + +/** +* @brief Get the next available heading and acceleration data from the sensor +* +* @param heading pointer to a BNO08x_RVC_Data struct to hold the measurements +* @return true: success false: failure +*/ +bool Adafruit_BNO08x_RVC::read(BNO08x_RVC_Data *heading) { + if (!heading) { + return false; + } + + if (!serial_dev->available()) { + return false; + } + if (serial_dev->peek() != 0xAA) { + serial_dev->read(); + return false; + } + // Now read all 19 bytes + + if (serial_dev->available() < 19) { + return false; + } + // at this point we know there's at least 19 bytes available and the first + if (serial_dev->read() != 0xAA) { + // shouldn't happen baecause peek said it was 0xAA + return false; + } + // make sure the next byte is the second 0xAA + if (serial_dev->read() != 0xAA) { + return false; + } + uint8_t buffer[19]; + // ok, we've got our header, read the actual data+crc + if (!serial_dev->readBytes(buffer, 17)) { + return false; + }; + + uint8_t sum = 0; + // get checksum ready + for (uint8_t i = 0; i < 16; i++) { + sum += buffer[i]; + } + if (sum != buffer[16]) { + return false; + } + + // The data comes in endian'd, this solves it so it works on all platforms + int16_t buffer_16[6]; + + for (uint8_t i = 0; i < 6; i++) { + + buffer_16[i] = (buffer[1 + (i * 2)]); + buffer_16[i] += (buffer[1 + (i * 2) + 1] << 8); + } + heading->yaw = (float)buffer_16[0] * DEGREE_SCALE; + heading->pitch = (float)buffer_16[1] * DEGREE_SCALE; + heading->roll = (float)buffer_16[2] * DEGREE_SCALE; + + heading->x_accel = (float)buffer_16[3] * MILLI_G_TO_MS2; + heading->y_accel = (float)buffer_16[4] * MILLI_G_TO_MS2; + heading->z_accel = (float)buffer_16[5] * MILLI_G_TO_MS2; + + return true; +} \ No newline at end of file diff --git a/arduino/leads_vec_wsc/Adafruit_BNO08x_RVC.h b/arduino/leads_vec_wsc/Adafruit_BNO08x_RVC.h new file mode 100644 index 0000000..1d2e8cd --- /dev/null +++ b/arduino/leads_vec_wsc/Adafruit_BNO08x_RVC.h @@ -0,0 +1,59 @@ +/*! +* @file Adafruit_BNO08x_RVC.h +* +* I2C Driver for the Adafruit BNO08x RVC A simple library to use the +*UART-RVC mode of the BNO08x sensors from Hillcrest Laboratories +* +* This is a library for use with thethe Adafruit BNO08x breakout: +* https://www.adafruit.com/products/4754 +* +* Adafruit invests time and resources providing this open source code, +* please support Adafruit and open-source hardware by purchasing products from +* Adafruit! +* +* +* BSD license (see license.txt) +*/ + +#ifndef _ADAFRUIT_BNO08x_RVC_H +#define _ADAFRUIT_BNO08x_RVC_H + +#define MILLI_G_TO_MS2 0.0098067 ///< Scalar to convert milli-gs to m/s^2 +#define DEGREE_SCALE 0.01 ///< To convert the degree values + +#include "Arduino.h" + +/*! +* @brief Class that stores state and functions for interacting with +* the BNO08x_RVC A simple library to use the UART-RVC mode of the +* BNO08x sensors from Hillcrest Laboratories +*/ +/**! Struct to hold a UART-RVC packet **/ +typedef struct BNO08xRVCData { + float yaw, ///< Yaw in Degrees + pitch, ///< Pitch in Degrees + roll; ///< Roll in Degrees + float x_accel, ///< The X acceleration value in m/s^2 + y_accel, ///< The Y acceleration value in m/s^2 + z_accel; ///< The Z acceleration value in m/s^2 + +} BNO08x_RVC_Data; + +/** +* @brief A class to interact with the BNO08x sensors from Hillcrest +* Laboritories using the UART-RVC mode +* +*/ +class Adafruit_BNO08x_RVC { +public: + Adafruit_BNO08x_RVC(); + ~Adafruit_BNO08x_RVC(); + + bool begin(Stream *theStream); + bool read(BNO08x_RVC_Data *heading); + +private: + Stream *serial_dev; +}; + +#endif \ No newline at end of file diff --git a/arduino/leads_vec_wsc/Accelerometer.cpp b/arduino/leads_vec_wsc/BNO08x.cpp similarity index 50% rename from arduino/leads_vec_wsc/Accelerometer.cpp rename to arduino/leads_vec_wsc/BNO08x.cpp index 373bfe6..0fb2718 100644 --- a/arduino/leads_vec_wsc/Accelerometer.cpp +++ b/arduino/leads_vec_wsc/BNO08x.cpp @@ -1,16 +1,13 @@ -#include "Accelerometer.h" +#include "BNO08x.h" -String Acceleration::toString() { - return String(yaw) + "," + pitch + "," + roll + "," + forwardAcceleration + "," + lateralAcceleration + "," + verticalAcceleration; -} -Accelerometer::Accelerometer(OnAccelerometerUpdate onUpdate) : _onUpdate(onUpdate) {} -void Accelerometer::initialize(const ArrayList &parentTags) { - Device::initialize(parentTags); +BNO08x::BNO08x(OnAccelerometerUpdate onUpdate) : Accelerometer(onUpdate) {} +void BNO08x::initialize(const ArrayList &parentTags) { + Accelerometer::initialize(parentTags); Serial1.begin(115200); while (!Serial1) delay(10); if (!_rvc.begin(&Serial1)) delay(10); } -Acceleration Accelerometer::read() { +Acceleration BNO08x::read() { BNO08x_RVC_Data heading; Acceleration r = Acceleration(); if (!_rvc.read(&heading)) return r; diff --git a/arduino/leads_vec_wsc/BNO08x.h b/arduino/leads_vec_wsc/BNO08x.h new file mode 100644 index 0000000..e76ae66 --- /dev/null +++ b/arduino/leads_vec_wsc/BNO08x.h @@ -0,0 +1,18 @@ +#ifndef BNO08X_H +#define BNO08X_H + + +#include "Adafruit_BNO08x_RVC.h" +#include "LEADS.h" + +class BNO08x : public Accelerometer { +protected: + Adafruit_BNO08x_RVC _rvc = Adafruit_BNO08x_RVC(); +public: + explicit BNO08x(OnAccelerometerUpdate onUpdate); + void initialize(const ArrayList &parentTags) override; + Acceleration read() override; +}; + + +#endif // BNO08X_H diff --git a/arduino/leads_vec_wsc/leads_vec_wsc.ino b/arduino/leads_vec_wsc/leads_vec_wsc.ino index c6e1b49..1707664 100644 --- a/arduino/leads_vec_wsc/leads_vec_wsc.ino +++ b/arduino/leads_vec_wsc/leads_vec_wsc.ino @@ -1,4 +1,4 @@ -#include "Accelerometer.h" +#include "BNO08x.h" const int PIN_LFWSS[] = {2}; const int PIN_RFWSS[] = {3}; @@ -12,7 +12,7 @@ WheelSpeedSensor RFWSS{ArrayList(PIN_RFWSS, 1), [](float ws) {returnFloat(P WheelSpeedSensor LRWSS{ArrayList(PIN_LRWSS, 1), [](float ws) {returnFloat(P, LEFT_REAR_WHEEL_SPEED_SENSOR, ws);}}; WheelSpeedSensor RRWSS{ArrayList(PIN_RRWSS, 1), [](float ws) {returnFloat(P, RIGHT_REAR_WHEEL_SPEED_SENSOR, ws);}}; WheelSpeedSensor CRWSS{ArrayList(PIN_CRWSS, 1), [](float ws) {returnFloat(P, CENTER_REAR_WHEEL_SPEED_SENSOR, ws);}}; -Accelerometer ACCL{[](Acceleration acceleration) {returnString(P, ACCELEROMETER, acceleration.toString());}}; +BNO08x ACCL{[](Acceleration acceleration) {returnString(P, ACCELEROMETER, acceleration.toString());}}; void setup() { P.initializeAsRoot();