From 520834367ae1d1bd113cf7e18615e999b24ad98e Mon Sep 17 00:00:00 2001 From: jkerpe Date: Wed, 4 Oct 2023 11:32:27 +0200 Subject: [PATCH] added IMU and send data via serialmuxprot --- .vscode/settings.json | 11 +- lib/HALInterfaces/IAccelerometer.h | 8 ++ lib/HALInterfaces/IBoard.h | 9 ++ lib/HALInterfaces/IIMU.h | 143 ++++++++++++++++++++++++ lib/HALSim/Accelerometer.cpp | 21 +++- lib/HALSim/Accelerometer.h | 13 ++- lib/HALSim/Board.cpp | 6 ++ lib/HALSim/Board.h | 23 +++- lib/HALSim/IMU.cpp | 137 +++++++++++++++++++++++ lib/HALSim/IMU.h | 167 +++++++++++++++++++++++++++++ lib/HALTarget/Accelerometer.cpp | 0 lib/HALTarget/Accelerometer.h | 158 +++++++++++++++++++++++++++ lib/LineFollower/App.cpp | 2 + lib/RemoteControl/App.cpp | 46 +++++++- lib/RemoteControl/App.h | 16 ++- lib/Service/Util.cpp | 34 ++++++ lib/Service/Util.h | 18 ++++ webots/protos/Zumo32U4.proto | 11 +- 18 files changed, 806 insertions(+), 17 deletions(-) create mode 100644 lib/HALInterfaces/IIMU.h create mode 100644 lib/HALSim/IMU.cpp create mode 100644 lib/HALSim/IMU.h create mode 100644 lib/HALTarget/Accelerometer.cpp create mode 100644 lib/HALTarget/Accelerometer.h diff --git a/.vscode/settings.json b/.vscode/settings.json index 859aafd0..e70bdf93 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -47,6 +47,15 @@ "ostream": "cpp", "stdexcept": "cpp", "streambuf": "cpp", - "typeinfo": "cpp" + "typeinfo": "cpp", + "complex": "cpp", + "ctime": "cpp", + "set": "cpp", + "ratio": "cpp", + "semaphore": "cpp", + "sstream": "cpp", + "stop_token": "cpp", + "thread": "cpp", + "cinttypes": "cpp" } } \ No newline at end of file diff --git a/lib/HALInterfaces/IAccelerometer.h b/lib/HALInterfaces/IAccelerometer.h index 38791219..8311b77f 100644 --- a/lib/HALInterfaces/IAccelerometer.h +++ b/lib/HALInterfaces/IAccelerometer.h @@ -81,6 +81,14 @@ class IAccelerometer */ virtual const double* getSensorValues() = 0; + /** + * Get number of axis used. + * + * @return number of axis which are evaluated + */ + virtual const uint8_t getNumberOfAxis() = 0; + + /** * Checks whether the calibration was successful or not. * diff --git a/lib/HALInterfaces/IBoard.h b/lib/HALInterfaces/IBoard.h index 0c2d476d..ba81de82 100644 --- a/lib/HALInterfaces/IBoard.h +++ b/lib/HALInterfaces/IBoard.h @@ -52,6 +52,7 @@ #include #include #include +#include /****************************************************************************** * Macros @@ -164,6 +165,14 @@ class IBoard */ virtual IProximitySensors& getProximitySensors() = 0; + /** + * Get IMU driver. + * + * @return IMU driver + */ + virtual IIMU& getIMU() = 0; + + protected: /** diff --git a/lib/HALInterfaces/IIMU.h b/lib/HALInterfaces/IIMU.h new file mode 100644 index 00000000..43f7037d --- /dev/null +++ b/lib/HALInterfaces/IIMU.h @@ -0,0 +1,143 @@ +/* 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 Abstract IMU interface + * @author Juliane Kerpe + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef IIMU_H +#define IIMU_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The abstract IMU interface. */ +class IIMU +{ +public: + /** + * Destroys the interface. + */ + virtual ~IIMU() + { + } + + /** + * Initializes the Accelerometer. + */ + virtual void init() = 0; + + /** + * Reads the sensors for calibration. + * The calibration factors are stored internally. + */ + virtual void calibrate() = 0; + + + /** + * Get last Accelerometer values. + * + * @return Accelerometer values + */ + virtual const double* getAccelerometerValues() = 0; + + + /** + * Get last Gyroscope values. + * + * @return Gyroscope values + */ + virtual const double getGyroValue() = 0; + + /** + * Get number of axis used. + * + * @return number of axis which are evaluated + */ + virtual const uint8_t getNumberOfAccelerometerAxis() = 0; + + + /** + * Checks whether the calibration was successful or not. + * + * @return If successful, it will return true otherwise false. + */ + virtual bool isCalibrationSuccessful() = 0; + + /** + * 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 xxx + */ + virtual uint8_t getCalibErrorInfo() const = 0; + + + /** + * Calibration error information: Calibration successful. + */ + static const uint8_t CALIB_ERROR_OK = 0xFF; + + /** + * Calibration error information: Calibration not done yet. + */ + static const uint8_t CALIB_ERROR_NOT_CALIBRATED = 0xFE; + +protected: + /** + * Constructs the interface. + */ + IIMU() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IIMU */ +/** @} */ diff --git a/lib/HALSim/Accelerometer.cpp b/lib/HALSim/Accelerometer.cpp index 8c7a759d..26b8849d 100644 --- a/lib/HALSim/Accelerometer.cpp +++ b/lib/HALSim/Accelerometer.cpp @@ -36,6 +36,7 @@ * Includes *****************************************************************************/ #include "Accelerometer.h" +#include /****************************************************************************** * Compiler Switches @@ -65,10 +66,12 @@ void Accelerometer::init() { for (uint8_t axisIndex = 0; axisIndex < NUMBER_OF_AXIS; ++axisIndex) { + m_sensorValues[axisIndex] = 0; } m_accelerometer->enable(m_samplingPeriod); + std::cout << "accel an"<< std::endl; } @@ -80,15 +83,25 @@ void Accelerometer::calibrate() const double* Accelerometer::getSensorValues() { + const double * values = m_accelerometer->getValues(); + + 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 = "< #include #include +#include #include #include @@ -213,6 +214,16 @@ class Board : public IBoard return m_proximitySensors; } + /** + * Get Accelerometer. + * + * @return Accelerometer driver + */ + IIMU& getIMU() final + { + return m_imu; + } + protected: private: /** Name of the speaker in the robot simulation. */ @@ -278,6 +289,12 @@ class Board : public IBoard /** Name of the front right proximity sensor in the robot simulation. */ static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME; + /** Name of the accelerometer in the robot simulation. */ + static const char* ACCELEROMETER_NAME; + + /** Name of the gyro in the robot simulation. */ + static const char* GYRO_NAME; + /** Simulated roboter instance. */ webots::Robot m_robot; @@ -322,6 +339,9 @@ class Board : public IBoard /** Proximity sensors */ ProximitySensors m_proximitySensors; + + /** Accelerometer */ + IMU m_imu; /** * Constructs the concrete board. @@ -348,7 +368,8 @@ class Board : public IBoard m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), m_ledGreen(m_robot.getLED(LED_GREEN_NAME)), m_proximitySensors(m_simTime, m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME), - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)) + m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), + m_imu(m_simTime, m_robot.getAccelerometer(ACCELEROMETER_NAME), m_robot.getGyro(GYRO_NAME)) { } diff --git a/lib/HALSim/IMU.cpp b/lib/HALSim/IMU.cpp new file mode 100644 index 00000000..d17b9534 --- /dev/null +++ b/lib/HALSim/IMU.cpp @@ -0,0 +1,137 @@ +/* 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 IMU implementation + * @author Juliane Kerpe + * + * @addtogroup HALInterfaces + * + * @{ + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IMU.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void IMU::init() +{ + for (uint8_t axisIndex = 0; axisIndex < NUMBER_OF_AXIS; ++axisIndex) + { + + m_accelerationValues[axisIndex] = 0.0; + m_gyroValue = 0.0; + + } + m_accelerometer->enable(m_samplingPeriod); + m_gyro->enable(m_samplingPeriod); + std::cout << "accel an"<< std::endl; + +} + +void IMU::calibrate() +{ + m_sensorCalibStarted = true; +} + + +const double* IMU::getAccelerometerValues() +{ + const double * accelerationValues = m_accelerometer->getValues(); + + if (accelerationValues != nullptr) { + for (uint8_t axisIndex = 0; axisIndex < NUMBER_OF_AXIS; ++axisIndex) + { + 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 = "<getValues(); + if (gyroValues != nullptr) { + m_gyroValue= gyroValues[INDEX_OF_Z_AXIS]; + } + return m_gyroValue; +} + +const uint8_t IMU::getNumberOfAccelerometerAxis() +{ + return NUMBER_OF_AXIS; +} + +bool IMU::isCalibrationSuccessful() +{ + // to do: implement + + return true; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/IMU.h b/lib/HALSim/IMU.h new file mode 100644 index 00000000..4e7ce94f --- /dev/null +++ b/lib/HALSim/IMU.h @@ -0,0 +1,167 @@ +/* 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 IMU realization + * @author Juliane Kerpe + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef IMU_H +#define IMU_H + + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IIMU.h" +#include "SimTime.h" +#include + +#include +#include + +class IMU : public IIMU +{ +public: + /** + * Constructs the IMU adapter. + * + * @param[in] simTime Simulation time + * @param[in] accelerometer The IMU + */ + IMU(const SimTime& simTime, webots::Accelerometer* accelerometer, webots::Gyro* gyro) : + IIMU(), + m_simTime(simTime), + m_accelerometer(accelerometer), + m_gyro(gyro), + + m_sensorCalibSuccessfull(false), + m_sensorCalibStarted(false), + m_samplingPeriod(10) + { + } + + /** + * Destroys the IMU adapter. + */ + ~IMU() + { + } + + /** + * Initializes the IMU. + */ + void init(); + + /** + * Reads the IMU for calibration. + * + * The calibration factors are stored internally. + */ + void calibrate() final; + + + /** + * Get last IMU values. + * + * @return IMU values + */ + // + const double* getAccelerometerValues() final; + + /** + * Get last IMU values. + * + * @return IMU values + */ + // + const double getGyroValue() final; + + + /** + * Get number of axis used. + * + * @return number of axis which are evaluated + */ + const uint8_t getNumberOfAccelerometerAxis() 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 axis of the accelerometer. + */ + static const uint8_t NUMBER_OF_AXIS = 2; + + + static const uint8_t INDEX_OF_Z_AXIS = 2; + + + const SimTime& m_simTime; /**< Simulation time */ + webots::Accelerometer* m_accelerometer; /**< The accelerometer */ + webots::Gyro* m_gyro; /**< The gyro */ + 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_accelerationValues[NUMBER_OF_AXIS]; /**< The last value of each sensor */ + double m_gyroValue; + int m_samplingPeriod; + + /* Default constructor not allowed. */ + IMU(); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LINESENSORS_H */ +/** @} */ diff --git a/lib/HALTarget/Accelerometer.cpp b/lib/HALTarget/Accelerometer.cpp new file mode 100644 index 00000000..e69de29b diff --git a/lib/HALTarget/Accelerometer.h b/lib/HALTarget/Accelerometer.h new file mode 100644 index 00000000..96c3e2ed --- /dev/null +++ b/lib/HALTarget/Accelerometer.h @@ -0,0 +1,158 @@ +/* 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 Accelerometer implementation + * @author Juliane Kerpe + * + * @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/LineFollower/App.cpp b/lib/LineFollower/App.cpp index 47e2fc0e..e872541d 100644 --- a/lib/LineFollower/App.cpp +++ b/lib/LineFollower/App.cpp @@ -94,6 +94,8 @@ void App::loop() } m_systemStateMachine.process(); + IIMU& imu = Board::getInstance().getIMU(); + (void)imu.getAccelerometerValues(); } /****************************************************************************** diff --git a/lib/RemoteControl/App.cpp b/lib/RemoteControl/App.cpp index 11bbc951..ab7a03d2 100644 --- a/lib/RemoteControl/App.cpp +++ b/lib/RemoteControl/App.cpp @@ -41,6 +41,7 @@ #include #include #include +#include // to do: löschen /****************************************************************************** * Compiler Switches @@ -77,6 +78,9 @@ const char* App::CH_NAME_MOTOR_SPEEDS = "MOT_SPEEDS"; /* Initialize channel name for sending line sensors data. */ const char* App::CH_NAME_LINE_SENSORS = "LINE_SENS"; +/* Initialize channel name for sending accelerometer data. */ +const char* App::CH_NAME_ACCELEROMETER = "ACCEL_SENS"; + /** Only in remote control state its possible to control the robot. */ static bool gIsRemoteCtrlActive = false; @@ -91,9 +95,15 @@ void App::setup() uint8_t maxLineSensors = lineSensors.getNumLineSensors(); uint8_t lineSensorChannelDlc = maxLineSensors * sizeof(uint16_t); + IIMU& imu = Board::getInstance().getIMU(); + uint8_t numberOfAxis = imu.getNumberOfAccelerometerAxis(); + // to do: check + uint8_t accelChannelDlc = numberOfAxis * sizeof(double); + + m_systemStateMachine.setState(&StartupState::getInstance()); m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); - m_sendLineSensorsDataInterval.start(SEND_LINE_SENSORS_DATA_PERIOD); + m_sendSensorsDataInterval.start(SEND_LINE_SENSORS_DATA_PERIOD); /* Remote control commands/responses */ m_smpServer.subscribeToChannel(CH_NAME_CMD, App_cmdChannelCallback); @@ -104,6 +114,9 @@ void App::setup() /* Providing line sensor data */ m_smpChannelIdLineSensors = m_smpServer.createChannel(CH_NAME_LINE_SENSORS, lineSensorChannelDlc); + + /* Providing accelerometer data */ + m_smpChannelIdAccelerometer = m_smpServer.createChannel(CH_NAME_ACCELEROMETER, accelChannelDlc); } void App::loop() @@ -137,13 +150,16 @@ void App::loop() } /* Send periodically line sensor data. */ - if (true == m_sendLineSensorsDataInterval.isTimeout()) + if (true == m_sendSensorsDataInterval.isTimeout()) { sendLineSensorsData(); + sendAccelerometerData(); - m_sendLineSensorsDataInterval.restart(); + m_sendSensorsDataInterval.restart(); } + + /* Send remote control command responses. */ sendRemoteControlResponses(); } @@ -181,8 +197,9 @@ void App::sendLineSensorsData() const while (maxLineSensors > lineSensorIdx) { + //Util::uint16ToByteArray(&payload[lineSensorIdx * sizeof(uint16_t)], sizeof(uint16_t), lineSensorValues[lineSensorIdx]); Util::uint16ToByteArray(&payload[lineSensorIdx * sizeof(uint16_t)], sizeof(uint16_t), - lineSensorValues[lineSensorIdx]); + 69); ++lineSensorIdx; } @@ -190,6 +207,27 @@ void App::sendLineSensorsData() const (void)m_smpServer.sendData(m_smpChannelIdLineSensors, payload, sizeof(payload)); } + +void App::sendAccelerometerData() const +{ + IIMU& imu = Board::getInstance().getIMU(); + const double* accelerationValues = imu.getAccelerometerValues(); + uint8_t numberOfAxis = imu.getNumberOfAccelerometerAxis(); + + uint8_t payload[numberOfAxis * sizeof(double)]; + uint8_t axisIdx = 0U; + + while (numberOfAxis > axisIdx) + { + Util::doubleToByteArray(&payload[axisIdx * sizeof(double)], sizeof(double), + accelerationValues[axisIdx]); + + ++axisIdx; + } + + (void)m_smpServer.sendData(m_smpChannelIdAccelerometer, payload, sizeof(payload)); +} + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/RemoteControl/App.h b/lib/RemoteControl/App.h index 7e3c7813..9f839420 100644 --- a/lib/RemoteControl/App.h +++ b/lib/RemoteControl/App.h @@ -67,10 +67,11 @@ class App App() : m_systemStateMachine(), m_controlInterval(), - m_sendLineSensorsDataInterval(), + m_sendSensorsDataInterval(), m_smpServer(Serial), m_smpChannelIdRemoteCtrlRsp(0U), m_smpChannelIdLineSensors(0U), + m_smpChannelIdAccelerometer(1U), m_lastRemoteControlRspId(RemoteCtrlState::RSP_ID_OK) { } @@ -110,6 +111,9 @@ class App /** SerialMuxProt channel name for sending line sensors data. */ static const char* CH_NAME_LINE_SENSORS; + + /** SerialMuxProt channel name for sending line sensors data. */ + static const char* CH_NAME_ACCELEROMETER; /** The system state machine. */ StateMachine m_systemStateMachine; @@ -118,7 +122,7 @@ class App SimpleTimer m_controlInterval; /** Timer used for sending data periodically. */ - SimpleTimer m_sendLineSensorsDataInterval; + SimpleTimer m_sendSensorsDataInterval; /** * SerialMuxProt Server Instance @@ -133,6 +137,9 @@ class App /** Channel id sending line sensors data. */ uint8_t m_smpChannelIdLineSensors; + + /** Channel id sending acceleration data. */ + uint8_t m_smpChannelIdAccelerometer; /** Last remote control response id */ RemoteCtrlState::RspId m_lastRemoteControlRspId; @@ -149,6 +156,11 @@ class App * Send line sensors data via SerialMuxProt. */ void sendLineSensorsData() const; + + /** + * Send acceleration data via SerialMuxProt. + */ + void sendAccelerometerData() const; }; /****************************************************************************** diff --git a/lib/Service/Util.cpp b/lib/Service/Util.cpp index 4ff4d4f1..5432ac7d 100644 --- a/lib/Service/Util.cpp +++ b/lib/Service/Util.cpp @@ -196,6 +196,19 @@ void Util::uint32ToByteArray(uint8_t* buffer, size_t size, uint32_t value) } } +void Util::doubleToByteArray(uint8_t* buffer, size_t size, double value) +{ + if ((nullptr != buffer) && (sizeof(double) <= size)) + { + uint64_t intValue = *reinterpret_cast(&value); + + for (size_t byteIndex = 0; byteIndex < sizeof(double); ++byteIndex) + { + buffer[byteIndex] = static_cast((intValue >> (byteIndex * 8)) & 0xFF); + } + } +} + bool Util::byteArrayToInt16(const uint8_t* buffer, size_t size, int16_t& value) { bool isSuccess = false; @@ -243,6 +256,27 @@ bool Util::byteArrayToUint32(const uint8_t* buffer, size_t size, uint32_t& value return isSuccess; } +bool Util::byteArrayToDouble(const uint8_t* buffer, size_t size, double& value) +{ + bool isSuccess = false; + + if ((nullptr != buffer) && (sizeof(double) <= size)) + { + uint64_t intValue = 0; + + for (size_t byteIndex = 0; byteIndex < sizeof(double); ++byteIndex ) + { + intValue |= static_cast(buffer[byteIndex]) << (byteIndex * 8); + } + + value = *reinterpret_cast(&intValue); + + isSuccess = true; + } + return isSuccess; +} + + /****************************************************************************** * Local Functions *****************************************************************************/ diff --git a/lib/Service/Util.h b/lib/Service/Util.h index 8e39b72b..7d349535 100644 --- a/lib/Service/Util.h +++ b/lib/Service/Util.h @@ -118,6 +118,14 @@ void int32ToByteArray(uint8_t* buffer, size_t size, int32_t value); */ void uint32ToByteArray(uint8_t* buffer, size_t size, uint32_t value); +/** + * Convert a double precision floating-point value to a byte array. + * @param[out] buffer Destination array. + * @param[in] size Size of the destination buffer in bytes. + * @param[in] value The double precision floating-point value to convert. + */ +void doubleToByteArray(uint8_t* buffer, size_t size, double value); + /** * Big endian byte array to int16_t. * @param[in] buffer Source Array. @@ -145,6 +153,16 @@ bool byteArrayToUint16(const uint8_t* buffer, size_t size, uint16_t& value); */ bool byteArrayToUint32(const uint8_t* buffer, size_t size, uint32_t& value); + +/** + * Big endian byte array to double. + * @param[in] buffer Source Array. + * @param[in] size Size of source array. + * @param[out] value Destination double. + * @returns true if succesfully parsed. Otherwise, false. + */ +bool byteArrayToDouble(const uint8_t* buffer, size_t size, double& value); + } // namespace Util #endif /* UTIL_H */ diff --git a/webots/protos/Zumo32U4.proto b/webots/protos/Zumo32U4.proto index d62b7814..647d3c32 100644 --- a/webots/protos/Zumo32U4.proto +++ b/webots/protos/Zumo32U4.proto @@ -446,15 +446,20 @@ PROTO Zumo32U4 [ children [ Accelerometer { name "accelerometer" + lookupTable [ + 0 0 0 + 1 1 0 + 2 2 0 + ] xAxis TRUE yAxis TRUE - zAxis FALSE + zAxis TRUE resolution -1 } Gyro { name "gyro" - xAxis TRUE - yAxis TRUE + xAxis FALSE + yAxis FALSE zAxis TRUE resolution -1 }