From 3238c16e899fa383d3d49a015eb5c4921905ae8f Mon Sep 17 00:00:00 2001 From: jkerpe Date: Wed, 4 Oct 2023 14:37:08 +0200 Subject: [PATCH] implemented IMU on Target --- lib/HALSim/Gyroscope.cpp | 113 ------------- lib/HALSim/Gyroscope.h | 141 ---------------- lib/HALSim/IMU.cpp | 6 +- lib/HALSim/IMU.h | 2 +- lib/HALTarget/Accelerometer.cpp | 0 lib/HALTarget/Accelerometer.h | 158 ------------------ lib/HALTarget/Board.h | 7 +- .../Accelerometer.cpp => HALTarget/IMU.cpp} | 79 ++++++--- .../Accelerometer.h => HALTarget/IMU.h} | 86 ++++++---- webots/protos/Zumo32U4.proto | 1 + 10 files changed, 115 insertions(+), 478 deletions(-) delete mode 100644 lib/HALSim/Gyroscope.cpp delete mode 100644 lib/HALSim/Gyroscope.h delete mode 100644 lib/HALTarget/Accelerometer.cpp delete mode 100644 lib/HALTarget/Accelerometer.h rename lib/{HALSim/Accelerometer.cpp => HALTarget/IMU.cpp} (66%) rename lib/{HALSim/Accelerometer.h => HALTarget/IMU.h} (60%) diff --git a/lib/HALSim/Gyroscope.cpp b/lib/HALSim/Gyroscope.cpp deleted file mode 100644 index 646042d9..00000000 --- a/lib/HALSim/Gyroscope.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 Juliane Kerpe - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Gyroscope implementation - * @author Juliane Kerpe - * - * @addtogroup HALInterfaces - * - * @{ - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Gyroscope.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Gyroscope::init() -{ - for (uint8_t axisIndex = 0; axisIndex < NUMBER_OF_AXIS; ++axisIndex) - { - m_sensorValues[axisIndex] = 0; - - } - m_gyroscope->enable(m_samplingPeriod); - -} - -void Gyroscope::calibrate() -{ - m_sensorCalibStarted = true; -} - - -const double* Gyroscope::getSensorValues() -{ - for (uint8_t axisIndex = 0; axisIndex < NUMBER_OF_AXIS; ++axisIndex) - { - ; - - } - - return m_sensorValues; -} - -bool Gyroscope::isCalibrationSuccessful() -{ - - - return true; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/HALSim/Gyroscope.h b/lib/HALSim/Gyroscope.h deleted file mode 100644 index d2b3647b..00000000 --- a/lib/HALSim/Gyroscope.h +++ /dev/null @@ -1,141 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Gyroscope realization - * @author Juliane Kerpe - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef GYROSCOPE_H -#define GYROSCOPE_H - - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IGyroscope.h" -#include "SimTime.h" - -#include - -class Gyroscope : public IGyroscope -{ -public: - /** - * Constructs the gyroscope adapter. - * - * @param[in] simTime Simulation time - * @param[in] emitter0 The most left infrared emitter 0 - */ - Gyroscope(const SimTime& simTime, webots::Gyro* gyroscope) : - IGyroscope(), - m_simTime(simTime), - m_gyroscope(gyroscope), - m_sensorCalibSuccessfull(false), - m_sensorCalibStarted(false), - m_samplingPeriod(10) - { - } - - /** - * Destroys the Gyroscope adapter. - */ - ~Gyroscope() - { - } - - /** - * Initializes the gyroscope. - */ - void init(); - - /** - * Reads the gyroscope for calibration. - * - * The calibration factors are stored internally. - */ - void calibrate() final; - - - /** - * Get last accelerometer values. - * - * @return gyroscope values - */ - const double* getSensorValues() final; - - /** - * Checks whether the calibration was successful or not. - * - * @return If successful, it will return true otherwise false. - */ - bool isCalibrationSuccessful() final; - - /** - * If calibration was successful, it will return 0xFF. - * If calibration was not not done yet, it will return 0xFE. - * - * @return Sensor index, starting with 0. Note the other cases in description. - */ - uint8_t getCalibErrorInfo() const final - { - return m_calibErrorInfo; - } - - -private: - /** - * Number of used line sensors. This depends on the Zumo hardware configuration. - */ - static const uint8_t NUMBER_OF_AXIS = 3; - - - const SimTime& m_simTime; /**< Simulation time */ - webots::Gyro* m_gyroscope; /**< The gyroscope */ - uint8_t m_calibErrorInfo; /**< Indicates which sensor failed the calibration, if the calibration failed. */ - bool m_sensorCalibSuccessfull; /**< Indicates weather the calibration was successfull or not. */ - bool m_sensorCalibStarted; /**< Indicates weather the calibration has started or not. */ - double m_sensorValues[NUMBER_OF_AXIS]; /**< The last value of each sensor */ - int m_samplingPeriod; - - /* Default constructor not allowed. */ - Gyroscope(); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* GYROSCOPE_H */ -/** @} */ diff --git a/lib/HALSim/IMU.cpp b/lib/HALSim/IMU.cpp index d17b9534..6f467e2c 100644 --- a/lib/HALSim/IMU.cpp +++ b/lib/HALSim/IMU.cpp @@ -93,9 +93,9 @@ const double* IMU::getAccelerometerValues() m_accelerationValues[axisIndex] = accelerationValues[axisIndex]; } } - if (abs(m_accelerationValues[0])>0.1 || abs(m_accelerationValues[1])>0.1){ - std::cout<<"Accelerometer.cpp: ACCEL: x = " << m_accelerationValues[0]<< "; y = "<0.1 || abs(m_accelerationValues[1])>0.1){ + std::cout<<"Accelerometer.cpp: ACCEL: x = " << m_accelerationValues[0]<< "; y = "< - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef LINESENSORS_H -#define LINESENSORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IAccelerometer.h" -#include "Zumo32U4.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target line sensors. */ -class Accelerometer : public IAccelerometer -{ -public: - /** - * Constructs the line sensors adapter. - */ - Accelerometer() : IAccelerometer(), - m_accelerometer(accelerometer), - m_sensorCalibSuccessfull(false), - m_sensorCalibStarted(false), - m_samplingPeriod(10) - { - } - - /** - * Destroys the line sensors adapter. - */ - ~Accelerometer() - { - } - - /** - * Initializes the line sensors. - */ - void init() final - { - m_accelerometer.initFiveSensors(); - } - - /** - * - * The calibration factors are stored internally. - */ - void calibrate() final - { - m_accelerometer.calibrate(); - } - - - /** - * Get last line sensor values. - * - * @return Line sensor values - */ - const double* getSensorValues() final - { - return 0.0; - - } - - /** - * Checks whether the calibration was successful or not. - * It assumes that the environment brightness compensation is active. - * - * @return If successful, it will return true otherwise false. - */ - bool isCalibrationSuccessful() final; - - /** - * It will return the index of the sensor, which caused to fail the calibration. - * If calibration was successful, it will return 0xFF. - * If calibration was not not done yet, it will return 0xFE. - * - * @return Sensor index, starting with 0. Note the other cases in description. - */ - uint8_t getCalibErrorInfo() const final - { - return m_calibErrorInfo; - } - - /** - * Get number of used line sensors. - * - * @return Number of used line sensors - */ - uint8_t getNumberOfAxis() const final - { - return NUMBER_OF_AXIS; - } - - -private: - /** - * Number of used axis. - */ - static const uint8_t NUMBER_OF_AXIS = 2; - - - - Zumo32U4IMU m_accelerometer; /**< Zumo line sensors driver from Pololu */ - double m_sensorValues[NUMBER_OF_AXIS]; /**< The last value of each sensor */ - int m_samplingPeriod; - uint8_t m_calibErrorInfo; /**< Calibration error information. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LINESENSORS_H */ -/** @} */ diff --git a/lib/HALTarget/Board.h b/lib/HALTarget/Board.h index de9e336e..5b20ac6f 100644 --- a/lib/HALTarget/Board.h +++ b/lib/HALTarget/Board.h @@ -56,6 +56,7 @@ #include #include #include +#include /****************************************************************************** * Macros @@ -249,6 +250,9 @@ class Board : public IBoard /** Proximity sensors */ ProximitySensors m_proximitySensors; + /** IMU */ + IMU m_imu; + /** * Constructs the concrete board. */ @@ -265,7 +269,8 @@ class Board : public IBoard m_ledRed(), m_ledYellow(), m_ledGreen(), - m_proximitySensors() + m_proximitySensors(), + m_imu() { } diff --git a/lib/HALSim/Accelerometer.cpp b/lib/HALTarget/IMU.cpp similarity index 66% rename from lib/HALSim/Accelerometer.cpp rename to lib/HALTarget/IMU.cpp index 26b8849d..a6dbcfce 100644 --- a/lib/HALSim/Accelerometer.cpp +++ b/lib/HALTarget/IMU.cpp @@ -24,10 +24,10 @@ DESCRIPTION *******************************************************************************/ /** - * @brief Accelerometer implementation + * @brief IMU implementation * @author Juliane Kerpe * - * @addtogroup HALInterfaces + * @addtogroup HALTarget * * @{ */ @@ -35,8 +35,7 @@ /****************************************************************************** * Includes *****************************************************************************/ -#include "Accelerometer.h" -#include +#include "IMU.h" /****************************************************************************** * Compiler Switches @@ -62,51 +61,77 @@ * Public Methods *****************************************************************************/ -void Accelerometer::init() +void IMU::init() { for (uint8_t axisIndex = 0; axisIndex < NUMBER_OF_AXIS; ++axisIndex) { - m_sensorValues[axisIndex] = 0; + m_accelerationValues[axisIndex] = 0.0; + m_gyroValue = 0.0; } - m_accelerometer->enable(m_samplingPeriod); - std::cout << "accel an"<< std::endl; - + if(m_imu.init()){ + ; + } + m_imu.enableDefault(); + m_imu.configureForTurnSensing(); } -void Accelerometer::calibrate() +void IMU::calibrate() { + uint8_t number_of_measurements = 100; /**< Define how many measurements should be made for calibration */ + + int16_t sum_of_raw_accel_x = 0; + int16_t sum_of_raw_accel_y = 0; + int16_t sum_of_raw_gyro_x = 0; + m_sensorCalibStarted = true; + for (uint8_t measurementIdx=0; measurementIdxgetValues(); + while(!m_imu.accDataReady()){} + m_imu.readAcc(); - if (values != nullptr) { - for (uint8_t axisIndex = 0; axisIndex < NUMBER_OF_AXIS; ++axisIndex) - { - m_sensorValues[axisIndex] = values[axisIndex]; - } - } - if (abs(m_sensorValues[0])>0.1 || abs(m_sensorValues[1])>0.1){ - std::cout<<"Accelerometer.cpp: ACCEL: x = " << m_sensorValues[0]<< "; y = "< * - * @addtogroup HALSim + * @addtogroup HALTarget * * @{ */ -#ifndef ACCELEROMETER_H -#define ACCELEROMETER_H +#ifndef IMU_H +#define IMU_H /****************************************************************************** @@ -44,57 +44,62 @@ /****************************************************************************** * Includes *****************************************************************************/ -#include "IAccelerometer.h" -#include "SimTime.h" +#include "IIMU.h" +#include "Zumo32U4.h" +#include -#include -class Accelerometer : public IAccelerometer +class IMU : public IIMU { public: /** - * Constructs the accelerometer adapter. + * Constructs the IMU adapter. * - * @param[in] simTime Simulation time - * @param[in] accelerometer The accelerometer */ - Accelerometer(const SimTime& simTime, webots::Accelerometer* accelerometer) : - IAccelerometer(), - m_simTime(simTime), - m_accelerometer(accelerometer), - m_sensorCalibSuccessfull(false), + IMU() : + IIMU(), + + m_sensorCalibSuccessful(false), m_sensorCalibStarted(false), m_samplingPeriod(10) { } /** - * Destroys the accelerometer adapter. + * Destroys the IMU adapter. */ - ~Accelerometer() + ~IMU() { } /** - * Initializes the accelerometer. + * Initializes the IMU. */ void init(); /** - * Reads the accelerometer for calibration. - * + * Reads the IMU for calibration for a defined amount of times. + * Calculates the offset at rest. * The calibration factors are stored internally. */ void calibrate() final; /** - * Get last accelerometer values. + * Get last IMU values. + * + * @return IMU values + */ + // + const double* getAccelerometerValues() final; + + /** + * Get last IMU values. * - * @return accelerometer values + * @return IMU values */ // - const double* getSensorValues() final; + const double getGyroValue() final; /** @@ -102,7 +107,7 @@ class Accelerometer : public IAccelerometer * * @return number of axis which are evaluated */ - const uint8_t getNumberOfAxis() final; + const uint8_t getNumberOfAccelerometerAxis() final; /** * Checks whether the calibration was successful or not. @@ -125,21 +130,34 @@ class Accelerometer : public IAccelerometer private: /** - * Number of used line sensors. This depends on the Zumo hardware configuration. + * Number of used axis of the accelerometer. */ - static const uint8_t NUMBER_OF_AXIS = 2; + static const uint8_t NUMBER_OF_AXIS = 3; - const SimTime& m_simTime; /**< Simulation time */ - webots::Accelerometer* m_accelerometer; /**< The accelerometer */ - uint8_t m_calibErrorInfo; /**< Indicates which sensor failed the calibration, if the calibration failed. */ - bool m_sensorCalibSuccessfull; /**< Indicates weather the calibration was successfull or not. */ + static const uint8_t INDEX_OF_X_AXIS = 0; /**< the x-Value is stored at the index 0 of m_accelerationValues*/ + static const uint8_t INDEX_OF_Y_AXIS = 1; /**< the y-Value is stored at the index 0 of m_accelerationValues*/ + static const uint8_t INDEX_OF_Z_AXIS = 2; /**< the z-Value is stored at the index 0 of m_accelerationValues*/ + Zumo32U4IMU m_imu; + + uint8_t m_calibErrorInfo; /**< Indicates which sensor failed the calibration, if the calibration failed. */ + bool m_sensorCalibSuccessful; /**< Indicates weather the calibration was successfull or not. */ bool m_sensorCalibStarted; /**< Indicates weather the calibration has started or not. */ - double m_sensorValues[NUMBER_OF_AXIS]; /**< The last value of each sensor */ + double m_accelerationValues[NUMBER_OF_AXIS]; /**< The last value of each sensor */ + double m_gyroValue; int m_samplingPeriod; - /* Default constructor not allowed. */ - Accelerometer(); + /** + * Linear Acceleration Sensitivity Factor according to the LSM6DS33 data sheet. + */ + double ACCELEROMETER_SENSITIVITY_FACTOR = 0.061; + double GYRO_SENSITIVITY_FACTOR = 4.375; + double m_rawAccelerometerOffset_x; + double m_rawAccelerometerOffset_y; + + + double m_rawGyroOffset; + }; /****************************************************************************** diff --git a/webots/protos/Zumo32U4.proto b/webots/protos/Zumo32U4.proto index 647d3c32..786b3249 100644 --- a/webots/protos/Zumo32U4.proto +++ b/webots/protos/Zumo32U4.proto @@ -448,6 +448,7 @@ PROTO Zumo32U4 [ name "accelerometer" lookupTable [ 0 0 0 + 0.1 0.1 0 1 1 0 2 2 0 ]