diff --git a/lib/HALInterfaces/IBoard.h b/lib/HALInterfaces/IBoard.h index ba81de82..022f6795 100644 --- a/lib/HALInterfaces/IBoard.h +++ b/lib/HALInterfaces/IBoard.h @@ -45,13 +45,11 @@ *****************************************************************************/ #include #include -#include #include #include #include #include #include -#include #include /****************************************************************************** @@ -102,13 +100,6 @@ class IBoard */ virtual IButton& getButtonC() = 0; - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - virtual IBuzzer& getBuzzer() = 0; - /** * Get LCD driver. * @@ -158,13 +149,6 @@ class IBoard */ virtual ILed& getGreenLed() = 0; - /** - * Get proximity sensors driver. - * - * @return Proximity sensors driver - */ - virtual IProximitySensors& getProximitySensors() = 0; - /** * Get IMU driver. * diff --git a/lib/HALInterfaces/IIMU.h b/lib/HALInterfaces/IIMU.h index 994a46ae..f8e6576b 100644 --- a/lib/HALInterfaces/IIMU.h +++ b/lib/HALInterfaces/IIMU.h @@ -97,14 +97,6 @@ class IIMU */ virtual const double getMagnetometerValue() = 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. * diff --git a/lib/HALSim/Board.cpp b/lib/HALSim/Board.cpp index a29ffe67..5d48c5e1 100644 --- a/lib/HALSim/Board.cpp +++ b/lib/HALSim/Board.cpp @@ -111,12 +111,6 @@ const char* Board::LED_YELLOW_NAME = "led_yellow"; /* Name of the green LED in the robot simulation. */ const char* Board::LED_GREEN_NAME = "led_green"; -/* Name of the front left proximity sensor in the robot simulation. */ -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"; @@ -143,7 +137,6 @@ void Board::init() m_keyboard.init(); m_lineSensors.init(); m_motors.init(); - m_proximitySensors.initFrontSensor(); m_imu.init(); } diff --git a/lib/HALSim/Board.h b/lib/HALSim/Board.h index 9b913d6b..00310f83 100644 --- a/lib/HALSim/Board.h +++ b/lib/HALSim/Board.h @@ -47,7 +47,6 @@ #include #include #include -#include #include #include #include @@ -55,7 +54,6 @@ #include #include #include -#include #include #include @@ -124,16 +122,6 @@ class Board : public IBoard return m_buttonC; } - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() final - { - return m_buzzer; - } - /** * Get LCD driver. * @@ -204,16 +192,6 @@ class Board : public IBoard return m_ledGreen; } - /** - * Get proximity sensors driver. - * - * @return Proximity sensors driver - */ - IProximitySensors& getProximitySensors() final - { - return m_proximitySensors; - } - /** * Get Accelerometer. * @@ -283,12 +261,6 @@ class Board : public IBoard /** Name of the green LED in the robot simulation. */ static const char* LED_GREEN_NAME; - /** Name of the front proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME; - - /** 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; @@ -316,9 +288,6 @@ class Board : public IBoard /** Button C driver */ ButtonC m_buttonC; - /** Buzzer driver */ - Buzzer m_buzzer; - /** Display driver */ Display m_display; @@ -339,9 +308,6 @@ class Board : public IBoard /** Red LED driver */ LedGreen m_ledGreen; - - /** Proximity sensors */ - ProximitySensors m_proximitySensors; /** Accelerometer */ IMU m_imu; @@ -357,7 +323,6 @@ class Board : public IBoard m_buttonA(m_keyboard), m_buttonB(m_keyboard), m_buttonC(m_keyboard), - m_buzzer(m_robot.getSpeaker(SPEAKER_NAME)), m_display(m_robot.getDisplay(DISPLAY_NAME)), m_encoders(m_simTime, m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), @@ -370,8 +335,6 @@ class Board : public IBoard m_ledRed(m_robot.getLED(LED_RED_NAME)), 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_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 532d720c..54bc5a4e 100644 --- a/lib/HALSim/IMU.cpp +++ b/lib/HALSim/IMU.cpp @@ -37,7 +37,7 @@ *****************************************************************************/ #include "IMU.h" #include -#include // to do: entfernen +#include # define M_PI 3.14159265358979323846 /****************************************************************************** * Compiler Switches @@ -114,16 +114,10 @@ const double IMU::getMagnetometerValue() if (magnetometerValues != nullptr) { double angleInRadians = atan2(magnetometerValues[INDEX_OF_Y_AXIS], magnetometerValues[INDEX_OF_X_AXIS]); m_gyroValue = angleInRadians; - std::cout << "IMU.cpp: Magnetometer = " << m_gyroValue<< std::endl; } return m_gyroValue; } -const uint8_t IMU::getNumberOfAccelerometerAxis() -{ - return NUMBER_OF_AXIS; -} - bool IMU::isCalibrationSuccessful() { // to do: implement @@ -146,10 +140,3 @@ bool IMU::isCalibrationSuccessful() /****************************************************************************** * Local Functions *****************************************************************************/ -void transformFromUToX(double* result, const double* vectorP, const double* vectorX, const double& angle) { - double cosAngle = cos(angle); - double sinAngle = sin(angle); - - result[0] = cosAngle * vectorP[0] - sinAngle * vectorP[1] + vectorX[0]; - result[1] = sinAngle * vectorP[0] + cosAngle * vectorP[1] + vectorX[1]; -} \ No newline at end of file diff --git a/lib/HALSim/IMU.h b/lib/HALSim/IMU.h index 95dcb237..dce2fcb4 100644 --- a/lib/HALSim/IMU.h +++ b/lib/HALSim/IMU.h @@ -67,7 +67,10 @@ class IMU : public IIMU m_accelerometer(accelerometer), m_gyro(gyro), m_magnetometer(compass), - + m_calibErrorInfo(CALIB_ERROR_NOT_CALIBRATED), + m_accelerationValues(), + m_gyroValue(), + m_magnetometerValue(), m_sensorCalibSuccessful(false), m_sensorCalibStarted(false), m_samplingPeriod(10) // to do: woher kommt wert? @@ -117,15 +120,7 @@ class IMU : public IIMU */ // const double getMagnetometerValue() 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. * @@ -157,17 +152,17 @@ class IMU : public IIMU 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 */ - webots::Compass* m_magnetometer; /**< The magnetometer */ - 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_accelerationValues[NUMBER_OF_AXIS]; /**< The last value of each sensor */ - double m_gyroValue; - double m_magnetometerValue; - int m_samplingPeriod; + const SimTime& m_simTime; /**< Simulation time */ + webots::Accelerometer* m_accelerometer; /**< The accelerometer */ + webots::Gyro* m_gyro; /**< The gyro */ + webots::Compass* m_magnetometer; /**< The magnetometer */ + 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_accelerationValues[NUMBER_OF_AXIS]; /**< The last values of the acceleration sensor */ + double m_gyroValue; /**< The last value of the gyroscope around the z-axis*/ + double m_magnetometerValue; /**< The last value of the magnetometer */ + int m_samplingPeriod; /**< The sampling Period of the IMU Sensors*/ /* Default constructor not allowed. */ IMU(); @@ -177,5 +172,5 @@ class IMU : public IIMU * Functions *****************************************************************************/ -#endif /* LINESENSORS_H */ +#endif /* IMU_H */ /** @} */ diff --git a/lib/HALTarget/Board.cpp b/lib/HALTarget/Board.cpp index 66721be0..392cb90b 100644 --- a/lib/HALTarget/Board.cpp +++ b/lib/HALTarget/Board.cpp @@ -75,7 +75,6 @@ void Board::init() m_encoders.init(); m_lineSensors.init(); m_motors.init(); - m_proximitySensors.initFrontSensor(); } /****************************************************************************** diff --git a/lib/HALTarget/Board.h b/lib/HALTarget/Board.h index 81f8d686..bd32c8c1 100644 --- a/lib/HALTarget/Board.h +++ b/lib/HALTarget/Board.h @@ -47,7 +47,6 @@ #include #include #include -#include #include #include #include @@ -55,7 +54,6 @@ #include #include #include -#include #include /****************************************************************************** @@ -120,16 +118,6 @@ class Board : public IBoard return m_buttonC; } - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() final - { - return m_buzzer; - } - /** * Get LCD driver. * @@ -200,16 +188,6 @@ class Board : public IBoard return m_ledGreen; } - /** - * Get proximity sensors driver. - * - * @return Proximity sensors driver - */ - IProximitySensors& getProximitySensors() final - { - return m_proximitySensors; - } - /** * Get IMU driver. * @@ -233,9 +211,6 @@ class Board : public IBoard /** Button C driver */ ButtonC m_buttonC; - /** Buzzer driver */ - Buzzer m_buzzer; - /** Display driver */ Display m_display; @@ -257,9 +232,6 @@ class Board : public IBoard /** Red LED driver */ LedGreen m_ledGreen; - /** Proximity sensors */ - ProximitySensors m_proximitySensors; - /** IMU */ IMU m_imu; @@ -271,7 +243,6 @@ class Board : public IBoard m_buttonA(), m_buttonB(), m_buttonC(), - m_buzzer(), m_display(), m_encoders(), m_lineSensors(), @@ -279,7 +250,6 @@ class Board : public IBoard m_ledRed(), m_ledYellow(), m_ledGreen(), - m_proximitySensors(), m_imu() { } diff --git a/lib/HALTarget/IMU.cpp b/lib/HALTarget/IMU.cpp index cbccc08a..938a3ee2 100644 --- a/lib/HALTarget/IMU.cpp +++ b/lib/HALTarget/IMU.cpp @@ -36,6 +36,7 @@ * Includes *****************************************************************************/ #include "IMU.h" +#include "RobotConstants.h" /****************************************************************************** * Compiler Switches @@ -73,6 +74,25 @@ void IMU::init() if(m_imu.init()){ ; } + + switch (m_imu.getType()) + { + case Zumo32U4IMUType::LSM303D_L3GD20H : + m_accelerometerSensitivityFactor = RobotConstants::ACCELEROMETER_SENSITIVITY_FACTOR_LSM303D; + m_gyroSensitivityFactor = RobotConstants::GYRO_SENSITIVITY_FACTOR_L3GD20H; + m_magnetometerSensitivityFactor = RobotConstants::MAGNETOMETER_SENSITIVITY_FACTOR_LSM303D; + break; + + case Zumo32U4IMUType::LSM6DS33_LIS3MDL : + m_accelerometerSensitivityFactor = RobotConstants::ACCELEROMETER_SENSITIVITY_FACTOR_LSM6DS33; + m_gyroSensitivityFactor = RobotConstants::GYRO_SENSITIVITY_FACTOR_LSM6DS33; + m_magnetometerSensitivityFactor = RobotConstants::MAGNETOMETER_SENSITIVITY_FACTOR_LIS3MDL; + + break; + default: + break; + } + m_imu.enableDefault(); m_imu.configureForTurnSensing(); } @@ -110,8 +130,8 @@ const double* IMU::getAccelerometerValues() while(!m_imu.accDataReady()){} m_imu.readAcc(); - m_accelerationValues[INDEX_OF_X_AXIS] = (m_imu.a.x - m_rawAccelerometerOffset_x) * ACCELEROMETER_SENSITIVITY_FACTOR; - m_accelerationValues[INDEX_OF_Y_AXIS] = (m_imu.a.y - m_rawAccelerometerOffset_y) * ACCELEROMETER_SENSITIVITY_FACTOR; + m_accelerationValues[INDEX_OF_X_AXIS] = (m_imu.a.x - m_rawAccelerometerOffset_x) * m_accelerometerSensitivityFactor; + m_accelerationValues[INDEX_OF_Y_AXIS] = (m_imu.a.y - m_rawAccelerometerOffset_y) * m_accelerometerSensitivityFactor; return m_accelerationValues; } @@ -121,7 +141,7 @@ const double IMU::getGyroValue() while(!m_imu.gyroDataReady()){} m_imu.readGyro(); - m_gyroValue = (m_imu.g.z - m_rawGyroOffset) * GYRO_SENSITIVITY_FACTOR; + m_gyroValue = (m_imu.g.z - m_rawGyroOffset) * m_gyroSensitivityFactor; return m_gyroValue; } @@ -130,12 +150,6 @@ const double IMU::getMagnetometerValue() return 0.0; } - -const uint8_t IMU::getNumberOfAccelerometerAxis() -{ - return NUMBER_OF_AXIS; -} - bool IMU::isCalibrationSuccessful() { return m_sensorCalibSuccessful; diff --git a/lib/HALTarget/IMU.h b/lib/HALTarget/IMU.h index 8df55710..1f3f606b 100644 --- a/lib/HALTarget/IMU.h +++ b/lib/HALTarget/IMU.h @@ -58,10 +58,18 @@ class IMU : public IIMU */ IMU() : IIMU(), - + m_calibErrorInfo(CALIB_ERROR_NOT_CALIBRATED), m_sensorCalibSuccessful(false), m_sensorCalibStarted(false), - m_samplingPeriod(10) + m_accelerationValues(), + m_gyroValue(), + m_magnetometerValue(), + m_accelerometerSensitivityFactor(), + m_gyroSensitivityFactor(), + m_magnetometerSensitivityFactor(), + m_rawAccelerometerOffset_x(), + m_rawAccelerometerOffset_y(), + m_rawGyroOffset() { } @@ -109,14 +117,6 @@ class IMU : public IIMU // const double getMagnetometerValue() 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. * @@ -140,31 +140,33 @@ 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_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*/ + static const uint8_t INDEX_OF_X_AXIS = 0; /**< the x-Value is stored at the index 0 */ + static const uint8_t INDEX_OF_Y_AXIS = 1; /**< the y-Value is stored at the index 1 */ + static const uint8_t INDEX_OF_Z_AXIS = 2; /**< the z-Value is stored at the index 2 */ 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_accelerationValues[NUMBER_OF_AXIS]; /**< The last value of each sensor */ + 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_accelerationValues[NUMBER_OF_AXIS]; /**< The last value of each sensor */ double m_gyroValue; double m_magnetometerValue; - int m_samplingPeriod; /** - * Linear Acceleration Sensitivity Factor according to the LSM6DS33 data sheet. + * Linear Acceleration Sensitivity Factors specified in RobotConstant file. + */ + double m_accelerometerSensitivityFactor; + double m_gyroSensitivityFactor; + double m_magnetometerSensitivityFactor; + + /** + * Bias values of the sensors determined by the calibration. */ - double ACCELEROMETER_SENSITIVITY_FACTOR = 0.061; - double GYRO_SENSITIVITY_FACTOR = 4.375; double m_rawAccelerometerOffset_x; double m_rawAccelerometerOffset_y; - - double m_rawGyroOffset; }; @@ -173,5 +175,5 @@ class IMU : public IIMU * Functions *****************************************************************************/ -#endif /* LINESENSORS_H */ +#endif /* IMU_H */ /** @} */ diff --git a/lib/HALTarget/RobotConstants.h b/lib/HALTarget/RobotConstants.h index 6bb47803..e9d67afa 100644 --- a/lib/HALTarget/RobotConstants.h +++ b/lib/HALTarget/RobotConstants.h @@ -90,6 +90,19 @@ namespace RobotConstants * Number of encoder steps per mm. */ static const uint32_t ENCODER_STEPS_PER_MM = (ENCODER_RESOLUTION * GEAR_RATIO) / WHEEL_CIRCUMFERENCE; + + /** + * The different versions of the Pololu Zumo use different IMUs (see Pololu Zumo User Guide page 26) + * The Linear Sensitivity Factors are specified in the data sheets of the sensors LSM303D (accelerometer+gyro), L3GD20H (gyro), LSM6DS33 (gyro + accelerometer), LIS3MDL (magnetometer) + */ + static const double ACCELEROMETER_SENSITIVITY_FACTOR_LSM303D = 0.061; /**< Sensitivity Factor of the LSM303D accelerometer in mg/LSB at Range of +/- 2 mg/LSB */ + static const double ACCELEROMETER_SENSITIVITY_FACTOR_LSM6DS33 = 0.061; /**< Sensitivity Factor of the LSM6DS33 accelerometer in mg/LSB at Range of +/- 2 mg/LSB */ + static const double GYRO_SENSITIVITY_FACTOR_L3GD20H = 8.75; /**< Sensitivity Factor of the L3GD20H gyro in mdps/digit at Range of +/- 245 dps */ + static const double GYRO_SENSITIVITY_FACTOR_LSM6DS33 = 4.375; /**< Sensitivity Factor of the LSM6DS33 gyro in mdps/LSB at Range of +/- 125 mdps/LSB */ + static const double MAGNETOMETER_SENSITIVITY_FACTOR_LSM303D = 0.08; /**< Sensitivity Factor of the LSM303D magnetometer in mgauss/LSB at Range of +/- 2 gauss */ + static const double MAGNETOMETER_SENSITIVITY_FACTOR_LIS3MDL = 0.1461; /**< Sensitivity Factor of the LIS3MDL magnetometer in mgauss/LSB at Range of +/- 4 gauss */ + + }; /****************************************************************************** diff --git a/lib/LineFollower/App.cpp b/lib/LineFollower/App.cpp index 322483bd..d383a273 100644 --- a/lib/LineFollower/App.cpp +++ b/lib/LineFollower/App.cpp @@ -60,11 +60,11 @@ * Local Variables *****************************************************************************/ -/* Initialize channel name for sending IMU data. */ -const char* App::CH_NAME_IMU = "IMU_DATA"; +/* Initialize channel name for sending sensor data. */ +const char* App::CH_NAME_SENSORDATA = "SENSOR_DATA"; + +const uint8_t NUMBER_OF_SENSOR_DATA = 7; -/* Initialize channel name for sending Odometry data. */ -const char* App::CH_NAME_ODOMETRY = "ODOMETRY_DATA"; /****************************************************************************** * Public Methods *****************************************************************************/ @@ -76,19 +76,12 @@ void App::setup() m_systemStateMachine.setState(&StartupState::getInstance()); m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); - - IIMU& imu = Board::getInstance().getIMU(); - 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_sendSensorsDataInterval.start(SEND_SENSORS_DATA_PERIOD); - /* Providing IMU data */ - m_smpChannelIdIMU = m_smpServer.createChannel(CH_NAME_IMU, imuChannelDlc); + uint8_t sensorDataChannelDlc = NUMBER_OF_SENSOR_DATA * sizeof(float); + /* Providing Sensor data */ + m_smpChannelIdSensorData = m_smpServer.createChannel(CH_NAME_SENSORDATA, sensorDataChannelDlc); - /* Providing Odometry data */ - m_smpChannelIdOdometry = m_smpServer.createChannel(CH_NAME_ODOMETRY, odometryChannelDlc); } void App::loop() @@ -116,10 +109,7 @@ void App::loop() if (true == m_sendSensorsDataInterval.isTimeout()) { m_sendSensorsDataInterval.restart(); - sendIMUData(); - sendOdometryData(); - - (void)Board::getInstance().getIMU().getMagnetometerValue(); + sendSensorData(); } m_systemStateMachine.process(); @@ -133,41 +123,33 @@ void App::loop() * Private Methods *****************************************************************************/ -void App::sendIMUData() const +void App::sendSensorData() const { + uint8_t payload[NUMBER_OF_SENSOR_DATA * sizeof(float)]; IIMU& imu = Board::getInstance().getIMU(); - uint8_t numberOfValues = imu.getNumberOfAccelerometerAxis()+1; + Odometry& odometry = Odometry::getInstance(); + int32_t positionXOdometry; + int32_t positionYOdometry; + int32_t orientationOdometry; - uint8_t payload[numberOfValues * sizeof(double)]; + odometry.getPosition(positionXOdometry, positionYOdometry); + orientationOdometry = odometry.getOrientation(); const double* accelerationValues = imu.getAccelerometerValues(); const double gyrovalue = imu.getGyroValue(); + const double magnetometerValue = imu.getMagnetometerValue(); - /** 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); + Util::floatToByteArray(&payload[0 * sizeof(float)], sizeof(float), static_cast(accelerationValues[0])); + Util::floatToByteArray(&payload[1 * sizeof(float)], sizeof(float), static_cast(accelerationValues[1])); + Util::floatToByteArray(&payload[2 * sizeof(float)], sizeof(float), static_cast(gyrovalue)); + Util::floatToByteArray(&payload[3 * sizeof(float)], sizeof(float), static_cast(magnetometerValue)); + Util::floatToByteArray(&payload[4 * sizeof(float)], sizeof(float), static_cast(orientationOdometry)); + Util::floatToByteArray(&payload[5 * sizeof(float)], sizeof(float), static_cast(positionXOdometry)); + Util::floatToByteArray(&payload[6 * sizeof(float)], sizeof(float), static_cast(positionYOdometry)); - (void)m_smpServer.sendData(m_smpChannelIdIMU, payload, sizeof(payload)); + (void)m_smpServer.sendData(m_smpChannelIdSensorData, 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 9f34e559..a7d3cc6e 100644 --- a/lib/LineFollower/App.h +++ b/lib/LineFollower/App.h @@ -63,8 +63,7 @@ class App /** * Construct the line follower application. */ - App() : m_smpChannelIdOdometry(1U), - m_smpChannelIdIMU(1U), + App() : m_smpChannelIdSensorData(1U), m_systemStateMachine(), m_controlInterval(), m_sendSensorsDataInterval(), @@ -99,17 +98,11 @@ class App /** 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; + static const char* CH_NAME_SENSORDATA; /** Channel id Odometry data. */ - uint8_t m_smpChannelIdOdometry; - - /** Channel id sending IMU data. */ - uint8_t m_smpChannelIdIMU; + uint8_t m_smpChannelIdSensorData; /** The system state machine. */ StateMachine m_systemStateMachine; @@ -130,19 +123,14 @@ class App App(const App& app); App& operator=(const App& app); - - /** - * Send odometry data via SerialMuxProt. - */ - void sendOdometryData() const; /** - * Send IMU data via SerialMuxProt. + * Send Sensor 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; + void sendSensorData() const; }; /****************************************************************************** diff --git a/lib/LineFollower/DrivingState.cpp b/lib/LineFollower/DrivingState.cpp index ecb6f275..42adff8b 100644 --- a/lib/LineFollower/DrivingState.cpp +++ b/lib/LineFollower/DrivingState.cpp @@ -34,7 +34,6 @@ *****************************************************************************/ #include "DrivingState.h" #include -#include #include #include #include @@ -119,7 +118,6 @@ void DrivingState::process(StateMachine& sm) default: /* Fatal error */ diffDrive.setLinearSpeed(0, 0); - Sound::playAlarm(); sm.setState(&ReadyState::getInstance()); break; } @@ -133,8 +131,6 @@ void DrivingState::process(StateMachine& sm) * as this would extend the driven length. */ diffDrive.setLinearSpeed(0, 0); - - Sound::playAlarm(); } } @@ -181,8 +177,6 @@ void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorVa { m_lineStatus = LINE_STATUS_START_LINE_DETECTED; - Sound::playBeep(); - /* Measure the lap time and use as start point the detected start line. */ m_lapTime.start(0); } @@ -196,7 +190,6 @@ void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorVa */ diffDrive.setLinearSpeed(0, 0); - Sound::playBeep(); m_trackStatus = TRACK_STATUS_FINISHED; /* Calculate lap time and show it*/ @@ -253,7 +246,6 @@ void DrivingState::processTrackLost(int16_t position, const uint16_t* lineSensor */ diffDrive.setLinearSpeed(0, 0); - Sound::playAlarm(); m_trackStatus = TRACK_STATUS_FINISHED; } else diff --git a/lib/LineFollower/StartupState.cpp b/lib/LineFollower/StartupState.cpp index 10ffefaf..3934cdd2 100644 --- a/lib/LineFollower/StartupState.cpp +++ b/lib/LineFollower/StartupState.cpp @@ -36,7 +36,7 @@ #include #include #include "MotorSpeedCalibrationState.h" -#include "Sound.h" +// #include "Sound.h" /****************************************************************************** * Compiler Switches @@ -70,7 +70,7 @@ void StartupState::entry() Board::getInstance().init(); /* Surprise the audience. */ - Sound::playStartup(); + // Sound::playStartup(); /* Show team id / team name */ display.clear(); diff --git a/lib/Service/Sound.cpp b/lib/Service/Sound.cpp index cfd52809..17915208 100644 --- a/lib/Service/Sound.cpp +++ b/lib/Service/Sound.cpp @@ -93,34 +93,34 @@ static const uint8_t VOLUME = 10; void Sound::playAlarm() { - IBuzzer& buzzer = Board::getInstance().getBuzzer(); - - /* Req. 3.4.5-2: - * The Sound shall play two consecutive sounds of 500Hz frequency and 1/3s duration, - * interrupted by 1/3s of silence. - */ - buzzer.playFrequency(ALARM_FREQ, ALARM_DURATION, VOLUME); - while (true == buzzer.isPlaying()) - ; - delay(SILENCE_DURATION); - buzzer.playFrequency(ALARM_FREQ, ALARM_DURATION, VOLUME); + // IBuzzer& buzzer = Board::getInstance().getBuzzer(); + + // /* Req. 3.4.5-2: + // * The Sound shall play two consecutive sounds of 500Hz frequency and 1/3s duration, + // * interrupted by 1/3s of silence. + // */ + // buzzer.playFrequency(ALARM_FREQ, ALARM_DURATION, VOLUME); + // while (true == buzzer.isPlaying()) + // ; + // delay(SILENCE_DURATION); + // buzzer.playFrequency(ALARM_FREQ, ALARM_DURATION, VOLUME); } void Sound::playBeep() { - IBuzzer& buzzer = Board::getInstance().getBuzzer(); + // IBuzzer& buzzer = Board::getInstance().getBuzzer(); - /* Req. 3.4.5-1: - * The Sound shall play a sound of 1000Hz frequency and 1s duration. - */ - buzzer.playFrequency(BEEP_FREQ, BEEP_DURATION, VOLUME); + // /* Req. 3.4.5-1: + // * The Sound shall play a sound of 1000Hz frequency and 1s duration. + // */ + // buzzer.playFrequency(BEEP_FREQ, BEEP_DURATION, VOLUME); } void Sound::playStartup() { - IBuzzer& buzzer = Board::getInstance().getBuzzer(); + // IBuzzer& buzzer = Board::getInstance().getBuzzer(); - buzzer.playMelodyPGM(PSTR("O4 T100 V15 L4 MS g12>c12>e12>G6>E12 ML>G2")); + // buzzer.playMelodyPGM(PSTR("O4 T100 V15 L4 MS g12>c12>e12>G6>E12 ML>G2")); } /****************************************************************************** diff --git a/lib/Service/Util.cpp b/lib/Service/Util.cpp index e4a7bbfa..3b3b3420 100644 --- a/lib/Service/Util.cpp +++ b/lib/Service/Util.cpp @@ -198,26 +198,10 @@ 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)) - { - uint64_t intValue = *reinterpret_cast(&value); - - for (size_t byteIndex = 0; byteIndex < sizeof(double); ++byteIndex) - { - buffer[byteIndex] = static_cast((intValue >> (byteIndex * 8)) & 0xFF); + if ((nullptr != buffer) && (sizeof(float) <= size)) { + uint8_t const *floatBytes = reinterpret_cast(&value); + for (size_t byteIndex = 0; byteIndex != sizeof(float); ++byteIndex) { + buffer[byteIndex] = floatBytes[byteIndex]; } } } @@ -269,41 +253,18 @@ 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; -} - 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); + uint8_t bytes[sizeof(float)]; + for (size_t byteIndex = 0; byteIndex != sizeof(float); ++byteIndex) { + bytes[byteIndex] = buffer[byteIndex]; } - - value = *reinterpret_cast(&intValue); - + float* floatValue = reinterpret_cast(bytes); + value = *floatValue; isSuccess = true; } return isSuccess; diff --git a/lib/Service/Util.h b/lib/Service/Util.h index 3442dac7..22a04b82 100644 --- a/lib/Service/Util.h +++ b/lib/Service/Util.h @@ -118,14 +118,6 @@ 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); - /** * Convert a floating-point value to a byte array. * @param[out] buffer Destination array. @@ -161,23 +153,14 @@ 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); - /** * 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); + */ +bool byteArrayToFloat(const uint8_t * buffer, size_t size, float & value); } // namespace Util #endif /* UTIL_H */