Skip to content

Commit

Permalink
Merge pull request #34 from BlueAndi/HeadingFinder
Browse files Browse the repository at this point in the history
Implemented HeadingFinder in ConvoyLeader Application
  • Loading branch information
BlueAndi authored Nov 9, 2023
2 parents 4452228 + 5901863 commit 9034d5b
Show file tree
Hide file tree
Showing 9 changed files with 265 additions and 82 deletions.
71 changes: 59 additions & 12 deletions lib/APPConvoyLeader/App.cpp
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,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));
}

/******************************************************************************
Expand All @@ -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);
}
}
41 changes: 26 additions & 15 deletions lib/APPConvoyLeader/App.h
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@
#include <StateMachine.h>
#include <SimpleTimer.h>
#include <SerialMuxProtServer.hpp>
#include "SerialMuxChannels.h"
#include <Arduino.h>

/******************************************************************************
Expand All @@ -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)
{
}

Expand All @@ -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. */
Expand Down
4 changes: 2 additions & 2 deletions lib/APPConvoyLeader/MotorSpeedCalibrationState.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@
#include <StateMachine.h>
#include <Logging.h>
#include <Util.h>
#include "LineSensorsCalibrationState.h"
#include "ReadyState.h"
#include "ErrorState.h"

/******************************************************************************
Expand Down Expand Up @@ -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());
}
}

Expand Down
49 changes: 3 additions & 46 deletions lib/APPConvoyLeader/ReadyState.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include "ReleaseTrackState.h"
#include <Logging.h>
#include <Util.h>
#include <DifferentialDrive.h>

/******************************************************************************
* Compiler Switches
Expand All @@ -59,11 +60,6 @@
* Local Variables
*****************************************************************************/

/**
* Logging source.
*/
static const char* TAG = "RState";

/******************************************************************************
* Public Methods
*****************************************************************************/
Expand All @@ -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()
Expand Down
97 changes: 97 additions & 0 deletions lib/APPConvoyLeader/SerialMuxChannels.h
Original file line number Diff line number Diff line change
@@ -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 */
/** @} */
Loading

0 comments on commit 9034d5b

Please sign in to comment.