Skip to content

Commit

Permalink
Application implemented.
Browse files Browse the repository at this point in the history 
Reports odometry and speed over SerialMuxProt.
Receives motor speeds over SerialMuxProt
  • Loading branch information
@gabryelreyes
gabryelreyes committed Nov 7, 2023
1 parent 5c56679 commit 5d72e11
Show file tree
Hide file tree
Showing 2 changed files with 88 additions and 29 deletions.
75 changes: 63 additions & 12 deletions lib/APPConvoyLeader/App.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
*****************************************************************************/
Expand All @@ -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()
Expand All @@ -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();
}

Expand All @@ -112,14 +127,35 @@ void App::loop()
* Private Methods
*****************************************************************************/

void App::reportPosition()
void App::reportOdometry()
{
; /* Do nothing. */
Odometry& odometry = Odometry::getInstance();
IDisplay& display = Board::getInstance().getDisplay();
OdometryData payload;
int32_t xPos = 0;
int32_t yPos = 0;

odometry.getPosition(xPos, yPos);
payload.xPos = xPos;
payload.yPos = yPos;
payload.orientation = odometry.getOrientation();

m_smpServer.sendData(m_serialMuxProtChannelIdOdometry, reinterpret_cast<uint8_t*>(&payload), sizeof(payload));

display.clear();
display.print("Heading:");
display.gotoXY(0U, 1U);
display.print(payload.orientation);
}

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();

m_smpServer.sendData(m_serialMuxProtChannelIdSpeed, reinterpret_cast<uint8_t*>(&payload), sizeof(payload));
}

/******************************************************************************
Expand All @@ -128,4 +164,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);
}
}
42 changes: 25 additions & 17 deletions lib/APPConvoyLeader/App.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
/**
* @brief ConvoyLeader application
* @author Andreas Merkle <[email protected]>
*
*
* @addtogroup Application
*
* @{
Expand All @@ -46,6 +46,7 @@
#include <StateMachine.h>
#include <SimpleTimer.h>
#include <SerialMuxProtServer.hpp>
#include "SerialMuxChannels.h"
#include <Arduino.h>

/******************************************************************************
Expand All @@ -60,13 +61,15 @@
class App
{
public:

/**
* Construct the convoy leader application.
*/
App() :
m_serialMuxProtChannelIdOdometry(0U),
m_serialMuxProtChannelIdSpeed(0U),
m_systemStateMachine(),
m_controlInterval(),
m_reportTimer(),
m_smpServer(Serial)
{
}
Expand All @@ -89,46 +92,51 @@ class App
void loop();

private:

/** 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();

private:
/* An instance shall not be copied or assigned. */
App(const App& app);
App& operator=(const App& app);
};
Expand Down

0 comments on commit 5d72e11

Please sign in to comment.