diff --git a/lib/HALInterfaces/IAccelerometer.h b/lib/HALInterfaces/IAccelerometer.h deleted file mode 100644 index 8311b77f..00000000 --- a/lib/HALInterfaces/IAccelerometer.h +++ /dev/null @@ -1,135 +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 Abstract Accelerometer interface - * @author Juliane Kerpe - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef IACCELEROMETER_H -#define IACCELEROMETER_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The abstract Accelerometerinterface. */ -class IAccelerometer -{ -public: - /** - * Destroys the interface. - */ - virtual ~IAccelerometer() - { - } - - /** - * 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* 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. - * - * @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. - */ - IAccelerometer() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IACCELEROMETER_H */ -/** @} */ diff --git a/lib/HALInterfaces/IIMU.h b/lib/HALInterfaces/IIMU.h index 43f7037d..994a46ae 100644 --- a/lib/HALInterfaces/IIMU.h +++ b/lib/HALInterfaces/IIMU.h @@ -89,6 +89,14 @@ class IIMU */ virtual const double getGyroValue() = 0; + + /** + * Get last Magnetometer values. + * + * @return Magnetometer values + */ + virtual const double getMagnetometerValue() = 0; + /** * Get number of axis used. * diff --git a/lib/HALSim/Board.cpp b/lib/HALSim/Board.cpp index 8cc35a05..a29ffe67 100644 --- a/lib/HALSim/Board.cpp +++ b/lib/HALSim/Board.cpp @@ -122,6 +122,9 @@ const char* Board::ACCELEROMETER_NAME = "accelerometer"; /* Name of the gyro in the robot simulation. */ const char* Board::GYRO_NAME = "gyro"; + +/* Name of the magnetometer in the robot simulation. */ +const char* Board::MAGNETOMETER_NAME = "magnetometer"; /****************************************************************************** * Protected Methods *****************************************************************************/ diff --git a/lib/HALSim/Board.h b/lib/HALSim/Board.h index 24e35662..9b913d6b 100644 --- a/lib/HALSim/Board.h +++ b/lib/HALSim/Board.h @@ -295,6 +295,9 @@ class Board : public IBoard /** Name of the gyro in the robot simulation. */ static const char* GYRO_NAME; + /** Name of the Magnetometer in the robot simulation. */ + static const char* MAGNETOMETER_NAME; + /** Simulated roboter instance. */ webots::Robot m_robot; @@ -369,7 +372,7 @@ class Board : public IBoard 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_imu(m_simTime, m_robot.getAccelerometer(ACCELEROMETER_NAME), m_robot.getGyro(GYRO_NAME)) + m_imu(m_simTime, m_robot.getAccelerometer(ACCELEROMETER_NAME), m_robot.getGyro(GYRO_NAME), m_robot.getCompass(MAGNETOMETER_NAME)) { } diff --git a/lib/HALSim/IMU.cpp b/lib/HALSim/IMU.cpp index 730fb9f8..532d720c 100644 --- a/lib/HALSim/IMU.cpp +++ b/lib/HALSim/IMU.cpp @@ -37,7 +37,8 @@ *****************************************************************************/ #include "IMU.h" #include - +#include // to do: entfernen +# define M_PI 3.14159265358979323846 /****************************************************************************** * Compiler Switches *****************************************************************************/ @@ -71,8 +72,7 @@ void IMU::init() m_gyroValue = 0.0; m_accelerometer->enable(m_samplingPeriod); m_gyro->enable(m_samplingPeriod); - //std::cout << "IMU.cpp: Turned on IMU."<< std::endl; - + m_magnetometer->enable(m_samplingPeriod); } void IMU::calibrate() @@ -93,7 +93,6 @@ const double* IMU::getAccelerometerValues() m_accelerationValues[axisIndex] = accelerationValues[axisIndex]; } } - std::cout<<"IMU.cpp: ACCEL: x = " << m_accelerationValues[0]<< "; y = "< index 0 + * Second index: acceleration value in y-direction --> index 1 + * Third index: turning rate around z-axis --> index 2 + */ + Util::doubleToByteArray(&payload[0 * sizeof(double)], sizeof(double), accelerationValues[0]); + Util::doubleToByteArray(&payload[1 * sizeof(double)], sizeof(double), accelerationValues[1]); + Util::doubleToByteArray(&payload[2 * sizeof(double)], sizeof(double), gyrovalue); + + (void)m_smpServer.sendData(m_smpChannelIdIMU, payload, sizeof(payload)); +} + +void App::sendOdometryData() const +{ + int32_t positionX; + int32_t positionY; + Odometry::getInstance().getPosition(positionX, positionY); + //std::cout<<"App.cpp: pos_x = " << positionX<<"; pos_y = " << positionY << std::endl; + uint8_t payload[2 * sizeof(int16_t)]; + + Util::int16ToByteArray(&payload[0 * sizeof(int16_t)], sizeof(int16_t), static_cast(positionX)); + Util::int16ToByteArray(&payload[1 * sizeof(int16_t)], sizeof(int16_t), static_cast(positionY)); + + (void)m_smpServer.sendData(m_smpChannelIdOdometry, payload, sizeof(payload)); + +} /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/LineFollower/App.h b/lib/LineFollower/App.h index 7ff60046..9f34e559 100644 --- a/lib/LineFollower/App.h +++ b/lib/LineFollower/App.h @@ -46,6 +46,7 @@ #include #include #include +#include /****************************************************************************** * Macros @@ -62,7 +63,12 @@ class App /** * Construct the line follower application. */ - App() : m_systemStateMachine(), m_controlInterval() + App() : m_smpChannelIdOdometry(1U), + m_smpChannelIdIMU(1U), + m_systemStateMachine(), + m_controlInterval(), + m_sendSensorsDataInterval(), + m_smpServer(Serial) { } @@ -87,17 +93,56 @@ class App /** Differential drive control period in ms. */ static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; + /** Sending Data period in ms. */ + static const uint32_t SEND_SENSORS_DATA_PERIOD = 20; + /** Baudrate for Serial Communication */ static const uint32_t SERIAL_BAUDRATE = 115200U; + /** SerialMuxProt channel name for sending IMU data. */ + static const char* CH_NAME_IMU; + + /** SerialMuxProt channel name for sending Odometry data. */ + static const char* CH_NAME_ODOMETRY; + + /** Channel id Odometry data. */ + uint8_t m_smpChannelIdOdometry; + + /** Channel id sending IMU data. */ + uint8_t m_smpChannelIdIMU; + /** The system state machine. */ StateMachine m_systemStateMachine; /** Timer used for differential drive control processing. */ SimpleTimer m_controlInterval; + /** Timer used for sending data periodically. */ + SimpleTimer m_sendSensorsDataInterval; + + /** + * SerialMuxProt Server Instance + * + * @tparam tMaxChannels set to 10, as App does not require + * more channels for external communication. + */ + SerialMuxProtServer<10U> m_smpServer; + App(const App& app); App& operator=(const App& app); + + /** + * Send odometry data via SerialMuxProt. + */ + void sendOdometryData() const; + + /** + * Send IMU data via SerialMuxProt. + * First index: acceleration value in x-direction --> index 0 + * Second index: acceleration value in y-direction --> index 1 + * Third index: turning rate around z-axis --> index 2 + */ + void sendIMUData() const; }; /****************************************************************************** diff --git a/lib/RemoteControl/App.cpp b/lib/RemoteControl/App.cpp index 06a4b59c..b03cfdfd 100644 --- a/lib/RemoteControl/App.cpp +++ b/lib/RemoteControl/App.cpp @@ -81,6 +81,9 @@ const char* App::CH_NAME_LINE_SENSORS = "LINE_SENS"; /* Initialize channel name for sending IMU data. */ const char* App::CH_NAME_IMU = "IMU_DATA"; +/* Initialize channel name for sending Odometry data. */ +const char* App::CH_NAME_ODOMETRY = "ODOMETRY_DATA"; + /** Only in remote control state its possible to control the robot. */ static bool gIsRemoteCtrlActive = false; @@ -96,10 +99,12 @@ void App::setup() uint8_t lineSensorChannelDlc = maxLineSensors * sizeof(uint16_t); IIMU& imu = Board::getInstance().getIMU(); - uint8_t numberOfIMUValues = imu.getNumberOfAccelerometerAxis()+1; - // to do: check + uint8_t numberOfIMUValues = imu.getNumberOfAccelerometerAxis()+1; // Number of Accelerometer axis + 1 for the gyro + uint8_t imuChannelDlc = numberOfIMUValues * sizeof(double); + uint8_t odometryChannelDlc = 2 * sizeof(int16_t); // only position in x and y are evaluated + m_systemStateMachine.setState(&StartupState::getInstance()); m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); @@ -117,6 +122,9 @@ void App::setup() /* Providing IMU data */ m_smpChannelIdAccelerometer = m_smpServer.createChannel(CH_NAME_IMU, imuChannelDlc); + + /* Providing Odometry data */ + m_smpChannelIdOdometry = m_smpServer.createChannel(CH_NAME_ODOMETRY, odometryChannelDlc); } void App::loop() @@ -154,6 +162,7 @@ void App::loop() { sendLineSensorsData(); sendIMUData(); + sendOdometryData(); m_sendSensorsDataInterval.restart(); } @@ -197,10 +206,7 @@ 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), - 69); - + Util::uint16ToByteArray(&payload[lineSensorIdx * sizeof(uint16_t)], sizeof(uint16_t), lineSensorValues[lineSensorIdx]); ++lineSensorIdx; } @@ -229,6 +235,21 @@ void App::sendIMUData() const (void)m_smpServer.sendData(m_smpChannelIdAccelerometer, payload, sizeof(payload)); } +void App::sendOdometryData() const +{ + int positionX; + int positionY; + Odometry::getInstance().getPosition(positionX, positionY); + std::cout<<"App.cpp: pos_x = " << positionX<<"; pos_y = " << positionY << std::endl; + uint8_t payload[2 * sizeof(int16_t)]; + + Util::int16ToByteArray(&payload[0 * sizeof(int16_t)], sizeof(int16_t), static_cast(positionX)); + Util::int16ToByteArray(&payload[1 * sizeof(int16_t)], sizeof(int16_t), static_cast(positionY)); + + (void)m_smpServer.sendData(m_smpChannelIdOdometry, payload, sizeof(payload)); + +} + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/RemoteControl/App.h b/lib/RemoteControl/App.h index f25b38ba..9c01dc4d 100644 --- a/lib/RemoteControl/App.h +++ b/lib/RemoteControl/App.h @@ -72,6 +72,7 @@ class App m_smpChannelIdRemoteCtrlRsp(0U), m_smpChannelIdLineSensors(0U), m_smpChannelIdAccelerometer(1U), + m_smpChannelIdOdometry(1U), m_lastRemoteControlRspId(RemoteCtrlState::RSP_ID_OK) { } @@ -115,6 +116,9 @@ class App /** SerialMuxProt channel name for sending IMU data. */ static const char* CH_NAME_IMU; + /** SerialMuxProt channel name for sending Odometry data. */ + static const char* CH_NAME_ODOMETRY; + /** The system state machine. */ StateMachine m_systemStateMachine; @@ -140,6 +144,9 @@ class App /** Channel id sending acceleration data. */ uint8_t m_smpChannelIdAccelerometer; + + /** Channel id Odometry data. */ + uint8_t m_smpChannelIdOdometry; /** Last remote control response id */ RemoteCtrlState::RspId m_lastRemoteControlRspId; @@ -157,6 +164,11 @@ class App */ void sendLineSensorsData() const; + /** + * Send odometry data via SerialMuxProt. + */ + void sendOdometryData() const; + /** * Send IMU data via SerialMuxProt. * First index: acceleration value in x-direction --> index 0 diff --git a/lib/Service/LKF.cpp b/lib/Service/LKF.cpp deleted file mode 100644 index e69de29b..00000000 diff --git a/lib/Service/LKF.h b/lib/Service/LKF.h deleted file mode 100644 index f7470109..00000000 --- a/lib/Service/LKF.h +++ /dev/null @@ -1,89 +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 Implementation of the Linear Kalman Filter - * @author Juliane Kerpe - * - * @addtogroup Service - * - * @{ - */ - -#ifndef LKF_H -#define LKF_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target green LED. */ -class LKF -{ -public: - /** - * Constructs the Linear Kalman Filter - * - * @param[in] led The green LED of the simulated robot. - */ - LKF(); - - /** - * Destroys the green LED adapter. - */ - ~LKF() - { - } - void handleEncoderMeasurement(int encoderPosX, int encoderPosY); - void handleAccelerationMeasurement(int accelerationX, int accelerationY); - void predictionStep(); - void updateStep(); - - -private: - int m_state[3]; - int m_covariance[3][3]; - bool m_isInitialized; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LKF_H */ -/** @} */ diff --git a/lib/Service/Util.cpp b/lib/Service/Util.cpp index 5432ac7d..e4a7bbfa 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::floatToByteArray(uint8_t* buffer, size_t size, float value) +{ + if ((nullptr != buffer) && (sizeof(float) <= size)) + { + uint64_t intValue = *reinterpret_cast(&value); + + for (size_t byteIndex = 0; byteIndex < sizeof(float); ++byteIndex) + { + buffer[byteIndex] = static_cast((intValue >> (byteIndex * 8)) & 0xFF); + } + } +} + void Util::doubleToByteArray(uint8_t* buffer, size_t size, double value) { if ((nullptr != buffer) && (sizeof(double) <= size)) @@ -276,6 +289,25 @@ bool Util::byteArrayToDouble(const uint8_t* buffer, size_t size, double& value) return isSuccess; } +bool Util::byteArrayToFloat(const uint8_t* buffer, size_t size, float& value) +{ + bool isSuccess = false; + + if ((nullptr != buffer) && (sizeof(float) <= size)) + { + uint64_t intValue = 0; + + for (size_t byteIndex = 0; byteIndex < sizeof(float); ++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 7d349535..3442dac7 100644 --- a/lib/Service/Util.h +++ b/lib/Service/Util.h @@ -126,6 +126,14 @@ void uint32ToByteArray(uint8_t* buffer, size_t size, uint32_t value); */ void doubleToByteArray(uint8_t* buffer, size_t size, double value); +/** + * Convert a 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 floating-point value to convert. + */ +void floatToByteArray(uint8_t * buffer, size_t size, float value); + /** * Big endian byte array to int16_t. * @param[in] buffer Source Array. @@ -163,6 +171,13 @@ bool byteArrayToUint32(const uint8_t* buffer, size_t size, uint32_t& value); */ bool byteArrayToDouble(const uint8_t* buffer, size_t size, double& value); +/** + * Big endian byte array to float. + * @param[in] buffer Source Array. + * @param[in] size Size of source array. + * @param[out] value Destination float. + * @returns true if succesfully parsed. Otherwise, false. + */bool byteArrayToFloat(const uint8_t * buffer, size_t size, float & value); } // namespace Util #endif /* UTIL_H */ diff --git a/webots/protos/Zumo32U4.proto b/webots/protos/Zumo32U4.proto index 1548fedd..7266e84a 100644 --- a/webots/protos/Zumo32U4.proto +++ b/webots/protos/Zumo32U4.proto @@ -449,14 +449,21 @@ PROTO Zumo32U4 [ xAxis TRUE yAxis TRUE zAxis TRUE - resolution 0.01 + resolution 0.001 } Gyro { name "gyro" xAxis FALSE yAxis FALSE zAxis TRUE - resolution -1 + resolution 0.001 + } + Compass { + name "magnetometer" + xAxis TRUE + yAxis TRUE + zAxis FALSE + resolution 0.001 } ]