Added motor speed setpoints

BlueAndi · Oct 27, 2023 · 9bf5fb9 · 9bf5fb9
1 parent 45a588e
commit 9bf5fb9
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 1 deletion.
diff --git a/lib/ConvoyLeader/App.cpp b/lib/ConvoyLeader/App.cpp
@@ -57,6 +57,8 @@
  * Prototypes
  *****************************************************************************/
 
+static void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize);
+
 /******************************************************************************
  * Local Variables
  *****************************************************************************/
@@ -71,8 +73,11 @@ void App::setup()
     Board::getInstance().init();
     m_systemStateMachine.setState(&StartupState::getInstance());
     m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD);
+
+    /* 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))
@@ -154,4 +159,19 @@ void App::reportSpeed()
 
 /******************************************************************************
  * 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);
+    }
+}
diff --git a/lib/ConvoyLeader/SerialMuxChannels.h b/lib/ConvoyLeader/SerialMuxChannels.h
@@ -55,6 +55,12 @@ SOFTWARE.
 /** 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
  *****************************************************************************/