Implemented HeadingFinder in ConvoyLeader Application

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<uint8_t*>(&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<uint8_t*>(&payload), sizeof(payload));
 }
 
 /******************************************************************************
@@ -128,4 +160,19 @@ void App::positionCallback(const uint8_t* payload, const uint8_t payloadSize)
 
 /******************************************************************************
  * Local Functions
- *****************************************************************************/
+ *****************************************************************************/
+
+/**
+ * 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<const SpeedData*>(payload);
+        DifferentialDrive::getInstance().setLinearSpeed(motorSpeedData->left, motorSpeedData->right);
+    }
+}

lib/APPConvoyLeader/App.h

@@ -46,6 +46,7 @@
 #include <StateMachine.h>
 #include <SimpleTimer.h>
 #include <SerialMuxProtServer.hpp>
+#include "SerialMuxChannels.h"
 #include <Arduino.h>
 
 /******************************************************************************
@@ -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<MAX_CHANNELS> 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. */

lib/APPConvoyLeader/MotorSpeedCalibrationState.cpp

@@ -39,7 +39,7 @@
 #include <StateMachine.h>
 #include <Logging.h>
 #include <Util.h>
-#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());
     }
 }
 

lib/APPConvoyLeader/ReadyState.cpp

@@ -38,6 +38,7 @@
 #include "ReleaseTrackState.h"
 #include <Logging.h>
 #include <Util.h>
+#include <DifferentialDrive.h>
 
 /******************************************************************************
  * 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()

lib/APPConvoyLeader/SerialMuxChannels.h

@@ -0,0 +1,97 @@
+/* MIT License
+ *
+ * Copyright (c) 2023 Andreas Merkle <[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  Channel structure definition for the SerialMuxProt.
+ * @author Gabryel Reyes <[email protected]>
+ *
+ * @addtogroup Application
+ *
+ * @{
+ */
+#ifndef SERIAL_MUX_CHANNELS_H
+#define SERIAL_MUX_CHANNELS_H
+
+/******************************************************************************
+ * Compile Switches
+ *****************************************************************************/
+
+/******************************************************************************
+ * Includes
+ *****************************************************************************/
+
+#include <Arduino.h>
+
+/******************************************************************************
+ * 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 */
+/** @} */