diff --git a/library.properties b/library.properties index e7dabbf..1cdb256 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Leaphy Extra Extension -version=0.0.16 +version=0.0.17 author=Leaphy Robotics maintainer=Leaphy Robotics sentence=Provides Extra functionality to Leaphy robots @@ -7,5 +7,5 @@ paragraph= category=Device Control url=https://github.com/leaphy-robotics/leaphy-extensions-extra architectures=avr -includes=Adafruit_NeoPixel.h,Adafruit_TCS34725.h,ledstrip.h,Leaphy_Extra.h,OLED_Display.h,LSM303AGR.h -depends=Adafruit SSD1306,Adafruit GFX Library,Adafruit LSM303 Accel,Adafruit LIS2MDL,Adafruit BusIO Register,Adafruit I2CDevice,Adafruit SPIDevice,Adafruit Sensor,Wire +includes=Adafruit_NeoPixel.h,Adafruit_TCS34725.h,ledstrip.h,Leaphy_Extra.h,OLED_Display.h,LSM303AGR.h,LightSensor.h,TimeOfFlight.h,AirQuality.h,Magnet.h +depends=Adafruit SSD1306,Adafruit GFX Library,Adafruit LSM303 Accel,Adafruit LIS2MDL,Adafruit BusIO Register,Adafruit I2CDevice,Adafruit SPIDevice,Adafruit Sensor,Wire,Adafruit_VL53L0X diff --git a/src/AirQuality.cpp b/src/AirQuality.cpp new file mode 100644 index 0000000..df74456 --- /dev/null +++ b/src/AirQuality.cpp @@ -0,0 +1,204 @@ +#include "AirQuality.h" + +AirQuality::AirQuality(TwoWire *wire) +{ + _address = 0x58; + _wire = wire; + + _tvoc = 0; + _co2 = 0; + _h2 = 0; + _ethanol = 0; + + _lastTime = 0; + _error = AirQuality_OK; +} + + +bool AirQuality::begin() +{ + _wire->begin(); + if (! isConnected()) return false; + _init(); + return true; +} + + +bool AirQuality::isConnected() +{ + _wire->beginTransmission(_address); + return ( _wire->endTransmission() == 0); +} + +bool AirQuality::measure(bool all) +{ + if (millis() - _lastTime < 1000) return false; + _lastTime = millis(); + + request(); + delay(12); + read(); + + if (not all) return true; + + requestRaw(); + delay(25); + readRaw(); + return true; +} + + +void AirQuality::request() +{ + _lastRequest = millis(); + _command(0x2008); +} + + +bool AirQuality::read() +{ + if (_lastRequest == 0) return false; + if (millis() - _lastRequest < 13) return false; + _lastRequest = 0; + + if (_wire->requestFrom(_address, (uint8_t)6) != 6) + { + _error = AirQuality_ERROR_I2C; + return false; + } + _co2 = _wire->read() << 8; + _co2 += _wire->read(); + uint8_t crc = _wire->read(); + if (_CRC8(_co2) != crc) + { + _error = AirQuality_ERROR_CRC; + return false; + } + _tvoc = _wire->read() << 8; + _tvoc += _wire->read(); + crc = _wire->read(); + if (_CRC8(_tvoc) != crc) + { + _error = AirQuality_ERROR_CRC; + return false; + } + _error = AirQuality_OK; + return true; +} + + +void AirQuality::requestRaw() +{ + _lastRequest = millis(); + _command(0x2050); +} + + +bool AirQuality::readRaw() +{ + if (_lastRequest == 0) return false; + if (millis() - _lastRequest < 26) return false; + _lastRequest = 0; + + if (_wire->requestFrom(_address, (uint8_t)6) != 6) + { + _error = AirQuality_ERROR_I2C; + return false; + } + _h2 = _wire->read() << 8; + _h2 += _wire->read(); + uint8_t crc = _wire->read(); + if (_CRC8(_h2) != crc) + { + _error = AirQuality_ERROR_CRC; + return false; + } + _ethanol = _wire->read() << 8; + _ethanol += _wire->read(); + crc = _wire->read(); + if (_CRC8(_ethanol) != crc) + { + _error = AirQuality_ERROR_CRC; + return false; + } + _error = AirQuality_OK; + return true; +} + +float AirQuality::getH2() +{ + float cref = 0.5; // ppm + return cref * exp((_srefH2 - _h2) * 1.953125e-3); +} + + +float AirQuality::getEthanol() +{ + float cref = 0.4; // ppm + return cref * exp((_srefEth - _ethanol) * 1.953125e-3); +} + +int AirQuality::_command(uint16_t cmd) +{ + _wire->beginTransmission(_address); + _wire->write(cmd >> 8); + _wire->write(cmd & 0xFF); + _error = _wire->endTransmission(); + return _error; +} + + +int AirQuality::_command(uint16_t cmd, uint16_t v1) +{ + _wire->beginTransmission(_address); + _wire->write(cmd >> 8); + _wire->write(cmd & 0xFF); + _wire->write(v1 >> 8); + _wire->write(v1 & 0xFF); + _wire->write(_CRC8(v1)); + _error = _wire->endTransmission(); + return _error; +} + + +int AirQuality::_command(uint16_t cmd, uint16_t v1, uint16_t v2) +{ + _wire->beginTransmission(_address); + _wire->write(cmd >> 8); + _wire->write(cmd & 0xFF); + _wire->write(v1 >> 8); + _wire->write(v1 & 0xFF); + _wire->write(_CRC8(v1)); + _wire->write(v2 >> 8); + _wire->write(v2 & 0xFF); + _wire->write(_CRC8(v2)); + _error = _wire->endTransmission(); + return _error; +} + +uint8_t AirQuality::_CRC8(uint16_t data) +{ + uint8_t val[2]; + val[0] = data >> 8; + val[1] = data & 0xFF; + + uint8_t crc = 0xFF; + for(uint8_t i = 0; i < 2; i++) + { + crc ^= val[i]; + for (uint8_t b = 8; b > 0; b--) + { + if (crc & 0x80) + crc = (crc << 1) ^ 0x31; + else + crc <<= 1; + } + } + return crc; +}; + + +void AirQuality::_init() +{ + _command(0x2003); +}; diff --git a/src/AirQuality.h b/src/AirQuality.h new file mode 100644 index 0000000..0a81140 --- /dev/null +++ b/src/AirQuality.h @@ -0,0 +1,76 @@ +#pragma once +// +// FILE: SGP30.h +// AUTHOR: Rob Tillaart +// VERSION: 0.2.0 +// DATE: 2021-06-24 +// PURPOSE: Arduino library for SGP30 environment sensor. +// URL: https://github.com/RobTillaart/SGP30 +// https://www.adafruit.com/product/3709 + + +#include "Arduino.h" +#include "Wire.h" + +#define AirQuality_OK 0x00 +#define AirQuality_ERROR_CRC 0xFF +#define AirQuality_ERROR_I2C 0xFE + + +class AirQuality +{ +public: + explicit AirQuality(TwoWire *wire = &Wire); + bool begin(); + bool isConnected(); + void GenericReset(); + bool getID(); + uint16_t getFeatureSet(); + bool measureTest(); + uint32_t lastMeasurement() { return _lastTime; }; + bool measure(bool all = false); + void request(); + bool read(); + void requestRaw(); + bool readRaw(); + + uint16_t getTVOC() { return _tvoc; }; // PPB + uint16_t getCO2() { return _co2; }; // PPM + uint16_t getH2_raw() { return _h2; }; // UNKNOWN + uint16_t getEthanol_raw() { return _ethanol; }; // UNKNOWN + float getH2(); // experimental // PPM + float getEthanol(); // experimental // PPM + + void setSrefH2(uint16_t s = 13119) { _srefH2 = s; }; + uint16_t getSrefH2() { return _srefH2; }; + void setSrefEthanol(uint16_t s = 18472) { _srefEth = s; }; + uint16_t getSrefEthanol() { return _srefEth; }; + + + int lastError(); + uint8_t _id[6]; + + +private: + uint8_t _address; + int _error; + uint32_t _lastTime = 0; + uint32_t _lastRequest = 0; + int _command(uint16_t cmd); + int _command(uint16_t cmd, uint16_t v1); + int _command(uint16_t cmd, uint16_t v1, uint16_t v2); + uint8_t _CRC8(uint16_t val); + void _init(); + + uint16_t _tvoc; + uint16_t _co2; + uint16_t _h2; + uint16_t _ethanol; + uint16_t _srefH2 = 13119; + uint16_t _srefEth = 18472; + + TwoWire* _wire; +}; + + +// -- END OF FILE -- diff --git a/src/LeaphyCompass.cpp b/src/LeaphyCompass.cpp new file mode 100644 index 0000000..4142204 --- /dev/null +++ b/src/LeaphyCompass.cpp @@ -0,0 +1,179 @@ + + + + +#include "Arduino.h" +#include "QMC5883LCompass.h" +#include + +void QMC5883LCompass::init(){ + Wire.begin(); + _writeReg(0x0B,0x01); + setMode(0x01,0x0C,0x10,0X00); +} + +void QMC5883LCompass::_writeReg(byte r, byte v){ + Wire.beginTransmission(_ADDR); + Wire.write(r); + Wire.write(v); + Wire.endTransmission(); +} + + + + +void QMC5883LCompass::setMode(byte mode, byte odr, byte rng, byte osr){ + _writeReg(0x09,mode|odr|rng|osr); +} + + + + + +void QMC5883LCompass::setSmoothing(byte steps, bool adv){ + _smoothUse = true; + _smoothSteps = ( steps > 10) ? 10 : steps; + _smoothAdvanced = (adv == true) ? true : false; +} + + +void QMC5883LCompass::setCalibration(int x_min, int x_max, int y_min, int y_max, int z_min, int z_max){ + _calibrationUse = true; + + _vCalibration[0][0] = x_min; + _vCalibration[0][1] = x_max; + _vCalibration[1][0] = y_min; + _vCalibration[1][1] = y_max; + _vCalibration[2][0] = z_min; + _vCalibration[2][1] = z_max; +} + + + + +void QMC5883LCompass::read(){ + Wire.beginTransmission(_ADDR); + Wire.write(0x00); + int err = Wire.endTransmission(); + if (!err) { + Wire.requestFrom(_ADDR, (byte)6); + _vRaw[0] = (int)(int16_t)(Wire.read() | Wire.read() << 8); + _vRaw[1] = (int)(int16_t)(Wire.read() | Wire.read() << 8); + _vRaw[2] = (int)(int16_t)(Wire.read() | Wire.read() << 8); + + if ( _calibrationUse ) { + _applyCalibration(); + } + + if ( _smoothUse ) { + _smoothing(); + } + + + + } +} + + +void QMC5883LCompass::_applyCalibration(){ + int x_offset = (_vCalibration[0][0] + _vCalibration[0][1])/2; + int y_offset = (_vCalibration[1][0] + _vCalibration[1][1])/2; + int z_offset = (_vCalibration[2][0] + _vCalibration[2][1])/2; + int x_avg_delta = (_vCalibration[0][1] - _vCalibration[0][0])/2; + int y_avg_delta = (_vCalibration[1][1] - _vCalibration[1][0])/2; + int z_avg_delta = (_vCalibration[2][1] - _vCalibration[2][0])/2; + + int avg_delta = (x_avg_delta + y_avg_delta + z_avg_delta) / 3; + + float x_scale = (float)avg_delta / x_avg_delta; + float y_scale = (float)avg_delta / y_avg_delta; + float z_scale = (float)avg_delta / z_avg_delta; + + _vCalibrated[0] = (_vRaw[0] - x_offset) * x_scale; + _vCalibrated[1] = (_vRaw[1] - y_offset) * y_scale; + _vCalibrated[2] = (_vRaw[2] - z_offset) * z_scale; +} + + + +void QMC5883LCompass::_smoothing(){ + byte max = 0; + byte min = 0; + + if ( _vScan > _smoothSteps - 1 ) { _vScan = 0; } + + for ( int i = 0; i < 3; i++ ) { + if ( _vTotals[i] != 0 ) { + _vTotals[i] = _vTotals[i] - _vHistory[_vScan][i]; + } + _vHistory[_vScan][i] = ( _calibrationUse ) ? _vCalibrated[i] : _vRaw[i]; + _vTotals[i] = _vTotals[i] + _vHistory[_vScan][i]; + + if ( _smoothAdvanced ) { + max = 0; + for (int j = 0; j < _smoothSteps - 1; j++) { + max = ( _vHistory[j][i] > _vHistory[max][i] ) ? j : max; + } + + min = 0; + for (int k = 0; k < _smoothSteps - 1; k++) { + min = ( _vHistory[k][i] < _vHistory[min][i] ) ? k : min; + } + + _vSmooth[i] = ( _vTotals[i] - (_vHistory[max][i] + _vHistory[min][i]) ) / (_smoothSteps - 2); + } else { + _vSmooth[i] = _vTotals[i] / _smoothSteps; + } + } + + _vScan++; +} + + + +int QMC5883LCompass::getX(){ + return _get(0); +} + + + +int QMC5883LCompass::getY(){ + return _get(1); +} + + + +int QMC5883LCompass::getZ(){ + return _get(2); +} + + +int QMC5883LCompass::_get(int i){ + if ( _smoothUse ) + return _vSmooth[i]; + + if ( _calibrationUse ) + return _vCalibrated[i]; + + return _vRaw[i]; +} + + + + +byte QMC5883LCompass::getBearing(int azimuth){ + unsigned long a = azimuth / 22.5; + unsigned long r = a - (int)a; + byte sexdec = 0; + sexdec = ( r >= .5 ) ? ceil(a) : floor(a); + return sexdec; +} + + + +void QMC5883LCompass::getDirection(char* myArray, int azimuth){ + int d = getBearing(azimuth); + myArray[0] = _bearings[d][0]; + myArray[1] = _bearings[d][1]; + myArray[2] = _bearings[d][2]; +} \ No newline at end of file diff --git a/src/LeaphyCompass.h b/src/LeaphyCompass.h new file mode 100644 index 0000000..4dc4b9c --- /dev/null +++ b/src/LeaphyCompass.h @@ -0,0 +1,62 @@ +#ifndef QMC5883L_Compass +#define QMC5883L_Compass + +#include "Arduino.h" +#include "Wire.h" + + +class QMC5883LCompass{ + + public: + void init(); + void setMode(byte mode, byte odr, byte rng, byte osr); + void setSmoothing(byte steps, bool adv); + void setCalibration(int x_min, int x_max, int y_min, int y_max, int z_min, int z_max); + void read(); + int getX(); + int getY(); + int getZ(); + byte getBearing(int azimuth); + void getDirection(char* myArray, int azimuth); + + private: + void _writeReg(byte reg,byte val); + int _get(int index); + bool _smoothUse = false; + byte _smoothSteps = 5; + bool _smoothAdvanced = false; + byte _ADDR = 0x0D; + int _vRaw[3] = {0,0,0}; + int _vHistory[10][3]; + int _vScan = 0; + long _vTotals[3] = {0,0,0}; + int _vSmooth[3] = {0,0,0}; + void _smoothing(); + bool _calibrationUse = false; + int _vCalibration[3][2]; + int _vCalibrated[3]; + void _applyCalibration(); + const char _bearings[16][3] = { + {' ', ' ', 'N'}, + {'N', 'N', 'E'}, + {' ', 'N', 'E'}, + {'E', 'N', 'E'}, + {' ', ' ', 'E'}, + {'E', 'S', 'E'}, + {' ', 'S', 'E'}, + {'S', 'S', 'E'}, + {' ', ' ', 'S'}, + {'S', 'S', 'W'}, + {' ', 'S', 'W'}, + {'W', 'S', 'W'}, + {' ', ' ', 'W'}, + {'W', 'N', 'W'}, + {' ', 'N', 'W'}, + {'N', 'N', 'W'}, + }; + + + +}; + +#endif diff --git a/src/LightSensor.cpp b/src/LightSensor.cpp new file mode 100644 index 0000000..83a71de --- /dev/null +++ b/src/LightSensor.cpp @@ -0,0 +1,18 @@ +#include "LightSensor.h" +#include + +LIGHTSENSOR::LIGHTSENSOR(int pin) +{ + this->pin = pin; +} + +bool LIGHTSENSOR::begin() +{ + pinMode(pin, INPUT); + return true; +} + +int LIGHTSENSOR::read() +{ + return analogRead(pin); +} \ No newline at end of file diff --git a/src/LightSensor.h b/src/LightSensor.h new file mode 100644 index 0000000..8b94ce0 --- /dev/null +++ b/src/LightSensor.h @@ -0,0 +1,16 @@ +#ifndef LIGHTSENSOR_H +#define LIGHTSENSOR_H + +class LIGHTSENSOR +{ +public: + private: + int pin; + public: + LIGHTSENSOR(int pin); + bool begin(); + int read(); +}; + + +#endif \ No newline at end of file diff --git a/src/Magnet.cpp b/src/Magnet.cpp new file mode 100644 index 0000000..efe9196 --- /dev/null +++ b/src/Magnet.cpp @@ -0,0 +1,19 @@ +#include "Magnet.h" + +Magnet::Magnet(int d0, int a0) +{ + this->d0 = d0; + this->a0 = a0; + pinMode(d0, INPUT); + pinMode(a0, INPUT); +} + +bool Magnet::readD() +{ + return digitalRead(d0); +} + +int Magnet::readA() +{ + return analogRead(a0); +} \ No newline at end of file diff --git a/src/Magnet.h b/src/Magnet.h new file mode 100644 index 0000000..4412472 --- /dev/null +++ b/src/Magnet.h @@ -0,0 +1,17 @@ +#ifndef MAGNET_H +#define MAGNET_H + +#include "Arduino.h" + +class Magnet +{ + public: + Magnet(int d0 = -1, int a0 = -1); + bool readD(); + int readA(); + private: + int d0; + int a0; +}; + +#endif \ No newline at end of file diff --git a/src/TimeofFlight.cpp b/src/TimeofFlight.cpp new file mode 100644 index 0000000..982c0ce --- /dev/null +++ b/src/TimeofFlight.cpp @@ -0,0 +1,30 @@ +#include "TimeofFlight.h" +#include "Adafruit_VL53L0X.h" + +bool TimeOfFlight::begin() +{ + if (!sensor.begin()) { + return false; + } + return true; +} + +int TimeOfFlight::read() +{ + VL53L0X_RangingMeasurementData_t measure; + sensor.rangingTest(&measure, false); + if (measure.RangeStatus == 4) { + return -1; + } + return measure.RangeMilliMeter; +} + +int TimeOfFlight::readCm() +{ + return this->read() / 10; +} + +int TimeOfFlight::readMm() +{ + return this->read(); +} \ No newline at end of file diff --git a/src/TimeofFlight.h b/src/TimeofFlight.h new file mode 100644 index 0000000..fda90ae --- /dev/null +++ b/src/TimeofFlight.h @@ -0,0 +1,18 @@ +#ifndef tof +#define tof +#include "Adafruit_VL53L0X.h" + +class TimeOfFlight +{ +public: + private: + Adafruit_VL53L0X sensor = Adafruit_VL53L0X(); + int read(); + public: + bool begin(); + int readCm(); + int readMm(); +}; + + +#endif \ No newline at end of file