From bbbb67f88e4340c875aae56d0d6d1bf70a626d81 Mon Sep 17 00:00:00 2001
From: jkerpe <juliane-98@web.de>
Date: Fri, 6 Oct 2023 10:25:15 +0200
Subject: [PATCH] corrected proto file

---
 lib/HALSim/IMU.cpp           | 22 +++++++++----------
 lib/HALSim/IMU.h             |  6 +++---
 lib/RemoteControl/App.cpp    | 41 ++++++++++++++++++------------------
 lib/RemoteControl/App.h      | 11 ++++++----
 webots/protos/Zumo32U4.proto |  8 +------
 5 files changed, 43 insertions(+), 45 deletions(-)

diff --git a/lib/HALSim/IMU.cpp b/lib/HALSim/IMU.cpp
index 6f467e2c..730fb9f8 100644
--- a/lib/HALSim/IMU.cpp
+++ b/lib/HALSim/IMU.cpp
@@ -27,7 +27,7 @@
  * @brief  IMU implementation
  * @author Juliane Kerpe <juliane.kerpe@web.de>
  * 
- * @addtogroup HALInterfaces
+ * @addtogroup HALSim
  *
  * @{
  */
@@ -66,20 +66,20 @@ void IMU::init()
 {
     for (uint8_t axisIndex = 0; axisIndex < NUMBER_OF_AXIS; ++axisIndex)
     {
-
-        m_accelerationValues[axisIndex] = 0.0;
-        m_gyroValue = 0.0;
-        
+        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;
+    //std::cout << "IMU.cpp: Turned on IMU."<< std::endl;
     
 }
 
 void IMU::calibrate()
 {
+    // to do: implement
     m_sensorCalibStarted = true;
+    m_sensorCalibSuccessful = true;
 }
 
 
@@ -93,9 +93,8 @@ 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 = "<<m_accelerationValues[1]<< "; z = "<<m_accelerationValues[2]<< std::endl;
-    //}
+    std::cout<<"IMU.cpp: ACCEL: x = " << m_accelerationValues[0]<< "; y = "<<m_accelerationValues[1]<< "; z = "<<accelerationValues[2]<< std::endl;
+
     return m_accelerationValues;
 }
 
@@ -103,8 +102,9 @@ const double IMU::getGyroValue()
 {
     const double * gyroValues = m_gyro->getValues();
     if (gyroValues != nullptr) {
-        m_gyroValue= gyroValues[INDEX_OF_Z_AXIS];
+        m_gyroValue = gyroValues[INDEX_OF_Z_AXIS];
     }
+    //std::cout<<"IMU.cpp: GYRO: z = " << m_gyroValue<< std::endl;
     return m_gyroValue;
 }
 
@@ -117,7 +117,7 @@ bool IMU::isCalibrationSuccessful()
 {
     // to do: implement
 
-    return true;
+    return m_sensorCalibSuccessful;
 }
 
 /******************************************************************************
diff --git a/lib/HALSim/IMU.h b/lib/HALSim/IMU.h
index 95aa1a35..b0f112e0 100644
--- a/lib/HALSim/IMU.h
+++ b/lib/HALSim/IMU.h
@@ -66,7 +66,7 @@ class IMU : public IIMU
         m_accelerometer(accelerometer),
         m_gyro(gyro),
 
-        m_sensorCalibSuccessfull(false),
+        m_sensorCalibSuccessful(false),
         m_sensorCalibStarted(false),
         m_samplingPeriod(10)
     {
@@ -139,7 +139,7 @@ class IMU : public IIMU
     /**
      * Number of used axis of the accelerometer.
      */
-    static const uint8_t NUMBER_OF_AXIS = 3;
+    static const uint8_t NUMBER_OF_AXIS = 2;
 
 
     static const uint8_t INDEX_OF_Z_AXIS = 2;
@@ -149,7 +149,7 @@ class IMU : public IIMU
     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_sensorCalibSuccessful; /**< 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;
diff --git a/lib/RemoteControl/App.cpp b/lib/RemoteControl/App.cpp
index ab7a03d2..06a4b59c 100644
--- a/lib/RemoteControl/App.cpp
+++ b/lib/RemoteControl/App.cpp
@@ -78,8 +78,8 @@ 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";
+/* Initialize channel name for sending IMU data. */
+const char* App::CH_NAME_IMU = "IMU_DATA";
 
 /** Only in remote control state its possible to control the robot. */
 static bool gIsRemoteCtrlActive = false;
@@ -96,9 +96,9 @@ void App::setup()
     uint8_t       lineSensorChannelDlc = maxLineSensors * sizeof(uint16_t);
 
     IIMU& imu      = Board::getInstance().getIMU();
-    uint8_t         numberOfAxis       = imu.getNumberOfAccelerometerAxis(); 
+    uint8_t         numberOfIMUValues      = imu.getNumberOfAccelerometerAxis()+1; 
     // to do: check
-    uint8_t         accelChannelDlc    = numberOfAxis * sizeof(double);
+    uint8_t         imuChannelDlc          = numberOfIMUValues * sizeof(double);
     
 
     m_systemStateMachine.setState(&StartupState::getInstance());
@@ -115,8 +115,8 @@ 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);
+    /* Providing IMU data */
+    m_smpChannelIdAccelerometer = m_smpServer.createChannel(CH_NAME_IMU, imuChannelDlc);
 }
 
 void App::loop()
@@ -153,7 +153,7 @@ void App::loop()
     if (true == m_sendSensorsDataInterval.isTimeout())
     {
         sendLineSensorsData();
-        sendAccelerometerData();
+        sendIMUData();
 
         m_sendSensorsDataInterval.restart();
     }
@@ -208,22 +208,23 @@ void App::sendLineSensorsData() const
 }
 
 
-void App::sendAccelerometerData() const
+void App::sendIMUData() const
 {
-    IIMU& imu                        = Board::getInstance().getIMU();
-    const double* accelerationValues = imu.getAccelerometerValues();
-    uint8_t         numberOfAxis     = imu.getNumberOfAccelerometerAxis();
+    IIMU& imu                          = Board::getInstance().getIMU();
+    uint8_t         numberOfValues     = imu.getNumberOfAccelerometerAxis()+1;
     
-    uint8_t         payload[numberOfAxis * sizeof(double)];
-    uint8_t         axisIdx    = 0U;
-
-    while (numberOfAxis > axisIdx)
-    {
-        Util::doubleToByteArray(&payload[axisIdx * sizeof(double)], sizeof(double),
-                                accelerationValues[axisIdx]);
+    uint8_t         payload[numberOfValues * sizeof(double)];
 
-        ++axisIdx;
-    }
+    const double* accelerationValues = imu.getAccelerometerValues();
+    const double  gyrovalue          = imu.getGyroValue();
+
+    /** 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
+     */
+    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_smpChannelIdAccelerometer, payload, sizeof(payload));
 }
diff --git a/lib/RemoteControl/App.h b/lib/RemoteControl/App.h
index 9f839420..f25b38ba 100644
--- a/lib/RemoteControl/App.h
+++ b/lib/RemoteControl/App.h
@@ -112,8 +112,8 @@ 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;
+    /** SerialMuxProt channel name for sending IMU data. */
+    static const char* CH_NAME_IMU;
 
     /** The system state machine. */
     StateMachine m_systemStateMachine;
@@ -158,9 +158,12 @@ class App
     void sendLineSensorsData() const;
 
     /**
-     * Send acceleration data via SerialMuxProt.
+     * 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 sendAccelerometerData() const;
+    void sendIMUData() const;
 };
 
 /******************************************************************************
diff --git a/webots/protos/Zumo32U4.proto b/webots/protos/Zumo32U4.proto
index 786b3249..1548fedd 100644
--- a/webots/protos/Zumo32U4.proto
+++ b/webots/protos/Zumo32U4.proto
@@ -446,16 +446,10 @@ PROTO Zumo32U4 [
         children [
           Accelerometer {
             name "accelerometer"
-            lookupTable [
-              0 0 0
-              0.1 0.1 0
-              1 1 0
-              2 2 0
-            ]
             xAxis TRUE
             yAxis TRUE
             zAxis TRUE
-            resolution  -1
+            resolution  0.01
           }
           Gyro {
             name "gyro"