added IMU and send data via serialmuxprot

.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"
     }
 }

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.
      * 

lib/HALInterfaces/IBoard.h

@@ -52,6 +52,7 @@
 #include <IMotors.h>
 #include <ILed.h>
 #include <IProximitySensors.h>
+#include <IIMU.h>
 
 /******************************************************************************
  * Macros
@@ -164,6 +165,14 @@ class IBoard
      */
     virtual IProximitySensors& getProximitySensors() = 0;
 
+    /**
+     * Get IMU driver.
+     * 
+     * @return IMU driver
+     */
+    virtual IIMU& getIMU() = 0;
+
+
 protected:
 
     /**

lib/HALInterfaces/IIMU.h

@@ -0,0 +1,143 @@
+/* MIT License
+ *
+ * Copyright (c) 2023 Juliane Kerpe <[email protected]>
+ * 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 <[email protected]>
+ * 
+ * @addtogroup HALInterfaces
+ *
+ * @{
+ */
+#ifndef IIMU_H
+#define IIMU_H
+
+/******************************************************************************
+ * Compile Switches
+ *****************************************************************************/
+
+/******************************************************************************
+ * Includes
+ *****************************************************************************/
+#include <stdint.h>
+
+/******************************************************************************
+ * 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 */
+/** @} */

lib/HALSim/Accelerometer.cpp

@@ -36,6 +36,7 @@
  * Includes
  *****************************************************************************/
 #include "Accelerometer.h"
+#include <iostream>
 
 /******************************************************************************
  * 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 = "<<m_sensorValues[1]<< std::endl;
     }
-
     return m_sensorValues;
 }
 
+const uint8_t Accelerometer::getNumberOfAxis()
+{
+    return NUMBER_OF_AXIS;
+}
+
 bool Accelerometer::isCalibrationSuccessful()
 {
 

lib/HALSim/Accelerometer.h

@@ -92,9 +92,18 @@ class Accelerometer : public IAccelerometer
      * Get last accelerometer values.
      *
      * @return accelerometer values
-     */
+     */ 
+    //
     const double* getSensorValues() final;
 
+
+    /**
+     * Get number of axis used.
+     *
+     * @return number of axis which are evaluated
+     */
+    const uint8_t getNumberOfAxis() final;
+
     /**
      * Checks whether the calibration was successful or not.
      *
@@ -118,7 +127,7 @@ class Accelerometer : public IAccelerometer
     /**
      * Number of used line sensors. This depends on the Zumo hardware configuration.
      */
-    static const uint8_t NUMBER_OF_AXIS = 3;
+    static const uint8_t NUMBER_OF_AXIS = 2;
 
 
     const SimTime&   m_simTime;                      /**< Simulation time */

lib/HALSim/Board.cpp

@@ -117,6 +117,11 @@ const char* Board::PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl";
 /* Name of the front right proximity sensor in the robot simulation. */
 const char* Board::PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr";
 
+/* Name of the accelerometer in the robot simulation. */
+const char* Board::ACCELEROMETER_NAME = "accelerometer";
+
+/* Name of the gyro in the robot simulation. */
+const char* Board::GYRO_NAME = "gyro";
 /******************************************************************************
  * Protected Methods
  *****************************************************************************/
@@ -136,6 +141,7 @@ void Board::init()
     m_lineSensors.init();
     m_motors.init();
     m_proximitySensors.initFrontSensor();
+    m_imu.init();
 }
 
 /******************************************************************************

lib/HALSim/Board.h

@@ -56,6 +56,7 @@
 #include <LedYellow.h>
 #include <LedGreen.h>
 #include <ProximitySensors.h>
+#include <IMU.h>
 
 #include <math.h>
 #include <webots/Robot.hpp>
@@ -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))
     {
     }