diff --git a/lib/APPConvoyLeader/App.cpp b/lib/APPConvoyLeader/App.cpp index ca797b2c..0fc3ff42 100644 --- a/lib/APPConvoyLeader/App.cpp +++ b/lib/APPConvoyLeader/App.cpp @@ -56,13 +56,12 @@ * Prototypes *****************************************************************************/ +static void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize); + /****************************************************************************** * Local Variables *****************************************************************************/ -/* Name of Channel to send Position Data to. */ -const char* App::POSITION_CHANNEL = "POSITION"; - /****************************************************************************** * Public Methods *****************************************************************************/ @@ -73,7 +72,17 @@ void App::setup() Board::getInstance().init(); m_systemStateMachine.setState(&StartupState::getInstance()); m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); - m_smpServer.createChannel(POSITION_CHANNEL, POSITION_CHANNEL_DLC); + + /* Setup SerialMuxProt Channels. */ + m_serialMuxProtChannelIdOdometry = m_smpServer.createChannel(ODOMETRY_CHANNEL_NAME, ODOMETRY_CHANNEL_DLC); + m_serialMuxProtChannelIdSpeed = m_smpServer.createChannel(SPEED_CHANNEL_NAME, SPEED_CHANNEL_DLC); + m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedSetpointsChannelCallback); + + /* Channel sucesfully created? */ + if ((0U != m_serialMuxProtChannelIdOdometry) && (0U != m_serialMuxProtChannelIdSpeed)) + { + m_reportTimer.start(REPORTING_PERIOD); + } } void App::loop() @@ -95,12 +104,18 @@ void App::loop() */ Odometry::getInstance().process(); - /* Send Position to SerialMuxProt Client */ - reportPosition(); - m_controlInterval.restart(); } + if (true == m_reportTimer.isTimeout()) + { + /* Send current data to SerialMuxProt Client */ + reportOdometry(); + reportSpeed(); + + m_reportTimer.restart(); + } + m_systemStateMachine.process(); } @@ -112,14 +127,31 @@ void App::loop() * Private Methods *****************************************************************************/ -void App::reportPosition() +void App::reportOdometry() { - ; /* Do nothing. */ + Odometry& odometry = Odometry::getInstance(); + OdometryData payload; + int32_t xPos = 0; + int32_t yPos = 0; + + odometry.getPosition(xPos, yPos); + payload.xPos = xPos; + payload.yPos = yPos; + payload.orientation = odometry.getOrientation(); + + /* Ignoring return value, as error handling is not available. */ + (void)m_smpServer.sendData(m_serialMuxProtChannelIdOdometry, reinterpret_cast(&payload), sizeof(payload)); } -void App::positionCallback(const uint8_t* payload, const uint8_t payloadSize) +void App::reportSpeed() { - ; /* Do nothing. */ + Speedometer& speedometer = Speedometer::getInstance(); + SpeedData payload; + payload.left = speedometer.getLinearSpeedLeft(); + payload.right = speedometer.getLinearSpeedRight(); + + /* Ignoring return value, as error handling is not available. */ + (void)m_smpServer.sendData(m_serialMuxProtChannelIdSpeed, reinterpret_cast(&payload), sizeof(payload)); } /****************************************************************************** @@ -128,4 +160,19 @@ void App::positionCallback(const uint8_t* payload, const uint8_t payloadSize) /****************************************************************************** * Local Functions - *****************************************************************************/ \ No newline at end of file + *****************************************************************************/ + +/** + * Receives motor speed setpoints over SerialMuxProt channel. + * + * @param[in] payload Motor speed left/right + * @param[in] payloadSize Size of twice motor speeds + */ +void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize) +{ + if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize)) + { + const SpeedData* motorSpeedData = reinterpret_cast(payload); + DifferentialDrive::getInstance().setLinearSpeed(motorSpeedData->left, motorSpeedData->right); + } +} \ No newline at end of file diff --git a/lib/APPConvoyLeader/App.h b/lib/APPConvoyLeader/App.h index b5184114..6c63928d 100644 --- a/lib/APPConvoyLeader/App.h +++ b/lib/APPConvoyLeader/App.h @@ -46,6 +46,7 @@ #include #include #include +#include "SerialMuxChannels.h" #include /****************************************************************************** @@ -63,7 +64,13 @@ class App /** * Construct the convoy leader application. */ - App() : m_systemStateMachine(), m_controlInterval(), m_smpServer(Serial) + App() : + m_serialMuxProtChannelIdOdometry(0U), + m_serialMuxProtChannelIdSpeed(0U), + m_systemStateMachine(), + m_controlInterval(), + m_reportTimer(), + m_smpServer(Serial) { } @@ -88,41 +95,45 @@ class App /** Differential drive control period in ms. */ static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; - /** Name of Channel to send Position Data to. */ - static const char* POSITION_CHANNEL; - - /** DLC of Position Channel */ - static const uint8_t POSITION_CHANNEL_DLC = 8U; + /** Current data reporting period in ms. */ + static const uint32_t REPORTING_PERIOD = 50U; /** Baudrate for Serial Communication */ static const uint32_t SERIAL_BAUDRATE = 115200U; + /** SerialMuxProt Channel id for sending the current odometry. */ + uint8_t m_serialMuxProtChannelIdOdometry; + + /** SerialMuxProt Channel id for sending the current speed. */ + uint8_t m_serialMuxProtChannelIdSpeed; + /** The system state machine. */ StateMachine m_systemStateMachine; /** Timer used for differential drive control processing. */ SimpleTimer m_controlInterval; + /** Timer for reporting current data through SerialMuxProt. */ + SimpleTimer m_reportTimer; + /** * SerialMuxProt Server Instance * - * @tparam tMaxChannels set to 10, as App does not require - * more channels for external communication. + * @tparam tMaxChannels set to MAX_CHANNELS, defined in SerialMuxChannels.h. */ - SerialMuxProtServer<10U> m_smpServer; + SerialMuxProtServer m_smpServer; /** - * Report the current position of the robot using the Odometry data. + * Report the current position and heading of the robot using the Odometry data. * Sends data through the SerialMuxProtServer. */ - void reportPosition(); + void reportOdometry(); /** - * Callback for incoming data from the Position Channel. - * @param[in] payload Byte buffer containing incomming data. - * @param[in] payloadSize Number of bytes received. + * Report the current motor speeds of the robot using the Speedometer data. + * Sends data through the SerialMuxProtServer. */ - static void positionCallback(const uint8_t* payload, const uint8_t payloadSize); + void reportSpeed(); /* Not allowed. */ App(const App& app); /**< Copy construction of an instance. */ diff --git a/lib/APPConvoyLeader/MotorSpeedCalibrationState.cpp b/lib/APPConvoyLeader/MotorSpeedCalibrationState.cpp index 77ddb642..71230760 100644 --- a/lib/APPConvoyLeader/MotorSpeedCalibrationState.cpp +++ b/lib/APPConvoyLeader/MotorSpeedCalibrationState.cpp @@ -39,7 +39,7 @@ #include #include #include -#include "LineSensorsCalibrationState.h" +#include "ReadyState.h" #include "ErrorState.h" /****************************************************************************** @@ -218,7 +218,7 @@ void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm) LOG_DEBUG_VAL(TAG, "Calibrated max. speed (steps/s): ", maxSpeed); LOG_DEBUG_VAL(TAG, "Calibrated max. speed (mm/s): ", maxSpeed / RobotConstants::ENCODER_STEPS_PER_MM); - sm.setState(&LineSensorsCalibrationState::getInstance()); + sm.setState(&ReadyState::getInstance()); } } diff --git a/lib/APPConvoyLeader/ReadyState.cpp b/lib/APPConvoyLeader/ReadyState.cpp index e5322dec..7581530f 100644 --- a/lib/APPConvoyLeader/ReadyState.cpp +++ b/lib/APPConvoyLeader/ReadyState.cpp @@ -38,6 +38,7 @@ #include "ReleaseTrackState.h" #include #include +#include /****************************************************************************** * Compiler Switches @@ -59,11 +60,6 @@ * Local Variables *****************************************************************************/ -/** - * Logging source. - */ -static const char* TAG = "RState"; - /****************************************************************************** * Public Methods *****************************************************************************/ @@ -84,51 +80,12 @@ void ReadyState::entry() /* The line sensor value shall be output on console cyclic. */ m_timer.start(SENSOR_VALUE_OUT_PERIOD); + DifferentialDrive::getInstance().enable(); } void ReadyState::process(StateMachine& sm) { - IButton& buttonA = Board::getInstance().getButtonA(); - - /* Shall track be released? */ - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&ReleaseTrackState::getInstance()); - } - /* Shall the line sensor values be printed out on console? */ - else if (true == m_timer.isTimeout()) - { - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - uint8_t index = 0; - int16_t position = lineSensors.readLine(); - const uint16_t* sensorValues = lineSensors.getSensorValues(); - char valueStr[10]; - - LOG_DEBUG_HEAD(TAG); - - /* Print line sensor value on console for debug purposes. */ - for (index = 0; index < lineSensors.getNumLineSensors(); ++index) - { - if (0 < index) - { - LOG_DEBUG_MSG(" / "); - } - - Util::uintToStr(valueStr, sizeof(valueStr), sensorValues[index]); - - LOG_DEBUG_MSG(valueStr); - } - - LOG_DEBUG_MSG(" -> "); - - Util::intToStr(valueStr, sizeof(valueStr), position); - LOG_DEBUG_MSG(valueStr); - - LOG_DEBUG_TAIL(); - - m_timer.restart(); - } + /* Do nothing. */ } void ReadyState::exit() diff --git a/lib/APPConvoyLeader/SerialMuxChannels.h b/lib/APPConvoyLeader/SerialMuxChannels.h new file mode 100644 index 00000000..5bd64310 --- /dev/null +++ b/lib/APPConvoyLeader/SerialMuxChannels.h @@ -0,0 +1,97 @@ +/* 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 Channel structure definition for the SerialMuxProt. + * @author Gabryel Reyes + * + * @addtogroup Application + * + * @{ + */ +#ifndef SERIAL_MUX_CHANNELS_H +#define SERIAL_MUX_CHANNELS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/** Maximum number of SerialMuxProt Channels. */ +#define MAX_CHANNELS (10U) + +/** Name of Channel to send Odometry Data to. */ +#define ODOMETRY_CHANNEL_NAME "ODOMETRY" + +/** DLC of Odometry Channel */ +#define ODOMETRY_CHANNEL_DLC (sizeof(OdometryData)) + +/** Name of Channel to send Speedometer Data to. */ +#define SPEED_CHANNEL_NAME "SPEED" + +/** DLC of Speedometer Channel */ +#define SPEED_CHANNEL_DLC (sizeof(SpeedData)) + +/** Name of Channel to send Motor Speed Setpoints to. */ +#define SPEED_SETPOINT_CHANNEL_NAME "SPEED_SET" + +/** DLC of Speedometer Channel */ +#define SPEED_SETPOINT_CHANNEL_DLC (sizeof(SpeedData)) + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Struct of the "Odometry" channel payload. */ +typedef struct _OdometryData +{ + int32_t xPos; /**< X position [mm]. */ + int32_t yPos; /**< Y position [mm]. */ + int32_t orientation; /**< Orientation [mrad]. */ +} __attribute__((packed)) OdometryData; + +/** Struct of the "Speed" channel payload. */ +typedef struct _SpeedData +{ + int16_t left; /**< Left motor speed [steps/s]. */ + int16_t right; /**< Right motor speed [steps/s]. */ +} __attribute__((packed)) SpeedData; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SERIAL_MUX_CHANNELS_H */ +/** @} */ diff --git a/lib/APPConvoyLeader/StartupState.cpp b/lib/APPConvoyLeader/StartupState.cpp index 733374bc..c6e4161a 100644 --- a/lib/APPConvoyLeader/StartupState.cpp +++ b/lib/APPConvoyLeader/StartupState.cpp @@ -88,13 +88,7 @@ void StartupState::entry() void StartupState::process(StateMachine& sm) { - IButton& buttonA = Board::getInstance().getButtonA(); - - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&MotorSpeedCalibrationState::getInstance()); - } + sm.setState(&MotorSpeedCalibrationState::getInstance()); } void StartupState::exit() diff --git a/webots/protos/HeadingCalculationTrack.proto b/webots/protos/HeadingCalculationTrack.proto new file mode 100644 index 00000000..101bc1c7 --- /dev/null +++ b/webots/protos/HeadingCalculationTrack.proto @@ -0,0 +1,39 @@ +#VRML_SIM R2023a utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/floors/protos/RectangleArena.proto" + +PROTO HeadingCalculationTrack [ + +] +{ + RectangleArena { + floorSize 2 2 + floorTileSize 2 2 + floorAppearance PBRAppearance { + baseColorMap ImageTexture { + url [ + "./compass.jpg" + ] + } + roughness 1 + roughnessMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/parquetry/chequered_parquetry_roughness.jpg" + ] + } + metalness 0 + normalMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/parquetry/chequered_parquetry_normal.jpg" + ] + } + occlusionMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/parquetry/chequered_parquetry_occlusion.jpg" + ] + } + } + wallThickness 0.001 + wallHeight 0.05 + } +} \ No newline at end of file diff --git a/webots/protos/compass.jpg b/webots/protos/compass.jpg new file mode 100644 index 00000000..331d2d92 Binary files /dev/null and b/webots/protos/compass.jpg differ diff --git a/webots/worlds/HeadingCalculation.wbt b/webots/worlds/HeadingCalculation.wbt new file mode 100644 index 00000000..a9a85516 --- /dev/null +++ b/webots/worlds/HeadingCalculation.wbt @@ -0,0 +1,38 @@ +#VRML_SIM R2023b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "../protos/Zumo32U4.proto" +EXTERNPROTO "../protos/HeadingCalculationTrack.proto" + +WorldInfo { + info [ + "Test track for heading calculation." + ] + title "Heading Calculation Track" + basicTimeStep 8 + contactProperties [ + ContactProperties { + material1 "track material" + coulombFriction [ + 500 + ] + softCFM 0.0001 + } + ] +} +Viewpoint { + orientation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.0944 + position 0.10671498723053877 -0.025801636297686177 4.366758398391875 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +HeadingCalculationTrack { +} +Zumo32U4 { + translation 0 0 0.0139993 + rotation 6.60216e-09 2.69589e-07 -1 -1.5286253071795866 + name "zumo_0" +}