Commit ae34bb4f authored by Niels Post's avatar Niels Post
Browse files

- Implemented callback based response

parent 980c03ad
Pipeline #29 failed with stage
......@@ -103,6 +103,8 @@ The result might look as follows:
};
While editing this file, it is recommended to add an enum for the commands in your custom category.
This enum can later be used to easily check which command was sent.
For the example, our category only accepts two commands, so the enum may look like:
.. code-block:: c++
......
#pragma once
#include <cstdint>
#include <entity/RobotMessage.hpp>
class MotorBoundary{
......@@ -12,4 +13,6 @@ public:
virtual void setDirection(bool dir) = 0;
virtual bool startMoveMM(uint16_t mm) = 0;
virtual bool isMoving() = 0;
};
\ No newline at end of file
......@@ -55,4 +55,10 @@ public:
*/
void setDirection(bool dir) override;
/**
* Check if this motor is currently moving
* @return True if the robot is still moving
*/
bool isMoving() override;
};
\ No newline at end of file
......@@ -2,6 +2,7 @@
#include <entity/RobotMessage.hpp>
#include <entity/ControllerMessage.hpp>
#include <functional>
/**
* Interface to handle commands in a specific category
......@@ -19,7 +20,7 @@ public:
* @param cmd Message to handle
* @return The result of the command. This should be sent back to the sender of the command
*/
virtual RobotMessage handle(const ControllerMessage &cmd) = 0;
virtual RobotMessage handle(const ControllerMessage &cmd, std::function<void(RobotMessage &)> callback ) = 0;
/**
* Get the minimum and maximum size of each command in this category
......
......@@ -19,7 +19,7 @@ public:
* @param cmd Command to handle
* @return The result of the action. This should be sent back to the sender of the command
*/
RobotMessage handle(const ControllerMessage &cmd) override;
RobotMessage handle(const ControllerMessage &cmd, std::function<void(RobotMessage &)> callback) override;
/**
* Construct the ActionCommandHandler
......
......@@ -12,7 +12,7 @@ public:
* @param cmd Command to handle
* @return The result of the command. This should be sent back to the sender of the command
*/
RobotMessage handle(const ControllerMessage &cmd) override;
RobotMessage handle(const ControllerMessage &cmd, std::function<void(RobotMessage &)> callback) override;
uint8_t getCategoryID() override;
......
......@@ -12,7 +12,7 @@ public:
* @param cmd Command to handle
* @return The result of the command, usually containing a measurement. This should be sent back to the sender of the command
*/
RobotMessage handle(const ControllerMessage &cmd) override;
RobotMessage handle(const ControllerMessage &cmd, std::function<void(RobotMessage &)> callback) override;
uint8_t getCategoryID() override;
......
......@@ -12,9 +12,8 @@ class PMSVSettings;
class RobotMessage;
template <class T, std::size_t N>
constexpr std::size_t array_size(const T (&array)[N]) noexcept
{
template<class T, std::size_t N>
constexpr std::size_t array_size(const T (&array)[N]) noexcept {
return N;
}
......@@ -28,11 +27,11 @@ constexpr std::size_t array_size(const T (&array)[N]) noexcept
template<size_t commandQueueMaxLength, size_t handlerCount>
class CommunicationController {
private:
MessageBoundary &commandBoundary;
MessageBoundary &commandBoundary;
/// Queue of commands that are still to be handled
StackQueue <ControllerMessage, commandQueueMaxLength> commandQueue;
StackQueue<ControllerMessage, commandQueueMaxLength> commandQueue;
///
CommandCategoryController *commandCategoryHandlers[handlerCount];
CommandCategoryController *commandCategoryHandlers[handlerCount];
protected:
......@@ -41,10 +40,17 @@ protected:
*/
void handleQueuedCommand() {
if (!commandQueue.empty()) {
const auto &command = commandQueue.front();
const auto &command = commandQueue.front();
for (CommandCategoryController *handler: commandCategoryHandlers) {
if (command.category_id == handler->getCategoryID()) {
commandBoundary.sendMessage(handler->handle(command));
RobotMessage firstResponse = handler->handle(command, [this, command](RobotMessage &msg) {
msg.message_id = command.message_id;
commandBoundary.sendMessage(msg);
});
commandBoundary.sendMessage(firstResponse);
commandQueue.pop();
return;
}
......@@ -52,6 +58,7 @@ protected:
Serial.println("Command category not recognized");
}
}
/**
......@@ -63,6 +70,7 @@ protected:
void parseNextCommand() {
if (commandBoundary.isMessageAvailable()) {
ControllerMessage cmd = commandBoundary.getNextMessage();
if (isKnownAndValidMessage(cmd)) {
commandQueue.push(cmd);
}
......@@ -71,18 +79,22 @@ protected:
bool isKnownAndValidMessage(const ControllerMessage &msg) {
if(msg.isValid()) {
if (!msg.isValid()) {
msg.print();
return false;
}
for (CommandCategoryController *handler: commandCategoryHandlers) {
if (msg.category_id == handler->getCategoryID()) {
auto limits = handler->getParameterLimits(msg.command_id);
if(msg.parameter_length >= limits.first && msg.parameter_length <= limits.second) {
if (msg.parameter_length >= limits.first && msg.parameter_length <= limits.second) {
return true;
}
}
}
Serial.println("No Handler");
return false;
}
......
......@@ -3,6 +3,10 @@
#include <entity/RobotMessage.hpp>
#include <boundary/movement/MotorBoundary.hpp>
#undef min
#undef max
#include <functional>
class PMSVSettings;
class MovementController {
......@@ -13,7 +17,14 @@ class MovementController {
/// Reference to the settings object to use to convert values
const PMSVSettings &settings;
SuccessCode tryMoveMM(uint16_t mm) const;
///
bool isCallbackEnabled = false;
///
std::function<void(RobotMessage &)> current_callback;
SuccessCode tryMoveMM(uint16_t mm, std::function<void(RobotMessage &)> callback);
public:
/**
* Change the speed of movement.
......@@ -39,7 +50,7 @@ public:
* @param mm Distance in CM
* @return
*/
SuccessCode startMoveMM(uint16_t mm);
SuccessCode startMoveMM(uint16_t mm, std::function<void(RobotMessage &)> callback);
/**
* Start a rotation of a specified angle
......@@ -47,7 +58,8 @@ public:
* @param rotate_direction The rotation direction (true for right, false for left)
* @return
*/
SuccessCode startRotateDegrees(uint16_t degrees, bool rotate_direction);
SuccessCode
startRotateDegrees(uint16_t degrees, bool rotate_direction, std::function<void(RobotMessage &)> callback);
/**
* Stop all movements currently being made
......
......@@ -4,7 +4,9 @@
#include <cstdio>
#include <utility>
#include <entity/CommandData.hpp>
#include <Arduino.h>
#undef min
#undef max
static const size_t COMMAND_MAX_PARAM_LENGTH = 5;
......@@ -64,4 +66,16 @@ struct ControllerMessage {
return COMMAND_MAX_PARAM_LENGTH + 3;
}
void print() const {
Serial.println("---message---");
Serial.print("Category: ");
Serial.println(category_id);
Serial.print("Command: ");
Serial.println(command_id);
Serial.print("Message: ");
Serial.println(message_id);
Serial.println("----------------");
}
};
......@@ -19,8 +19,8 @@ class PMSVSettings {
public:
// Settings related to the Movement Controller
uint16_t steps_per_cm;
uint16_t degrees_per_mm_distance;
uint16_t mm_distance_per_degree;
bool left_motor_fwd;
bool right_motor_fwd;
uint16_t min_speed;
......@@ -46,13 +46,12 @@ public:
StepperSettings rightMotor;
PMSVSettings(uint16_t stepsPerCm, uint16_t stepsPerDegree, bool leftMotorFwd, bool rightMotorFwd, uint16_t minSpeed,
PMSVSettings(uint16_t mm_per_degree, bool leftMotorFwd, bool rightMotorFwd, uint16_t minSpeed,
uint16_t maxSpeed, rf24_pa_dbm_e nrfPalevel, uint8_t nrfChannel, uint64_t nrfReadingpipe,
uint64_t nrfWritingpipe, bool nrfEnableDynamicpayloads, size_t nrfPacketSize, bool nrfEnableAutoack,
uint8_t nrfPinCe, uint8_t nrfPinCsn, StepperSettings leftMotor, StepperSettings rightMotor)
: steps_per_cm(stepsPerCm),
degrees_per_mm_distance(
stepsPerDegree),
: mm_distance_per_degree(
mm_per_degree),
left_motor_fwd(
leftMotorFwd),
right_motor_fwd(
......
......@@ -22,7 +22,9 @@ enum class SuccessCode : uint8_t {
/// A reading was requested for a sensor that doesn't exist
NO_SUCH_SENSOR = 5,
/// The command requested is unknown
UNKNOWN_COMMAND = 6
UNKNOWN_COMMAND = 6,
/// Special case: Action started is used when the robot cannot immediately send a response. The robot does not send this code
ACTION_STARTED = 7
};
/**
......@@ -36,18 +38,23 @@ struct RobotMessage {
/// (Optional) the data returned from the Command
uint8_t data[4];
/// Size of the data returned
size_t data_size;
uint8_t data_size;
/**
* Create a ReturnCommand
*/
RobotMessage(uint8_t messageId, SuccessCode returnCode, uint8_t data_length = 0, const uint8_t *dat = nullptr);
/**
* Create a ReturnCommand
*/
RobotMessage(SuccessCode returnCode, uint8_t data_length = 0, const uint8_t *dat = nullptr);
/**
* Get the byte size of this command
* @return Total size of the command in bytes
*/
size_t size() const {
uint8_t size() const {
return 2 + data_size;
}
};
......@@ -29,6 +29,7 @@ NRFBoundary::NRFBoundary(uint16_t pin_ce, uint16_t pin_csn, uint8_t channel, uin
nrfRadio.openWritingPipe(writingPipe);
nrfRadio.powerUp();
nrfRadio.startListening();
nrfRadio.printDetails();
}
......@@ -43,6 +44,7 @@ bool NRFBoundary::sendMessage(const RobotMessage &robotMessage) {
}
bool NRFBoundary::isMessageAvailable() {
return nrfRadio.available();
}
......@@ -50,7 +52,7 @@ ControllerMessage NRFBoundary::getNextMessage() {
auto size = nrfRadio.getDynamicPayloadSize();
uint8_t data[size];
nrfRadio.read(data, size);
ControllerMessage cmd;
ControllerMessage cmd = ControllerMessage::parse(data, size);
return cmd;
}
......
......@@ -9,10 +9,9 @@ bool AccelStepperBoundary::startMoveSteps(uint16_t steps) {
if (!direction) {
tomove *= -1;
}
Serial.println(tomove);
stepper.move(tomove);
stepper.enableOutputs();
return false;
return true;
}
void AccelStepperBoundary::halt() {
......@@ -31,7 +30,7 @@ void AccelStepperBoundary::update() {
AccelStepperBoundary::AccelStepperBoundary(uint8_t enable_pin, uint8_t step_pin, uint8_t dir_pin, uint8_t steps_per_mm)
: stepper(stepper.DRIVER,
step_pin, dir_pin), stepsPerMM(steps_per_mm) {
stepper.setMaxSpeed(50 * steps_per_mm);
stepper.setMaxSpeed(100 * steps_per_mm);
stepper.setAcceleration(1000 * steps_per_mm);
stepper.setEnablePin(enable_pin);
stepper.setPinsInverted(false, false, true);
......@@ -47,3 +46,7 @@ bool AccelStepperBoundary::startMoveMM(uint16_t mm) {
}
bool AccelStepperBoundary::isMoving() {
return stepper.distanceToGo() != 0;
}
#include <controller/CommandCategoryController/ActionCommandController.hpp>
#include <controller/MovementController.hpp>
RobotMessage ActionCommandController::handle(const ControllerMessage &cmd) {
#include <Arduino.h>
RobotMessage ActionCommandController::handle(const ControllerMessage &cmd, std::function<void(RobotMessage &)> callback) {
SuccessCode code = SuccessCode::BAD_PARAMETERS;
switch (cmd.command_id) {
case CANCEL_MOVEMENT:
......@@ -14,11 +13,12 @@ RobotMessage ActionCommandController::handle(const ControllerMessage &cmd) {
movementController.setDirection(cmd.parameters[2]);
}
code = movementController.startMoveMM(cmd.get_param16(0));
code = movementController.startMoveMM(cmd.get_param16(0), callback);
break;
}
case START_ROTATE_DEGREES:
code = movementController.startRotateDegrees(cmd.get_param16(0), cmd.parameters[2]);
code = movementController.startRotateDegrees(cmd.get_param16(0), cmd.parameters[2],
callback);
break;
case SET_SPEED:
......
......@@ -2,7 +2,7 @@
RobotMessage GeneralCommandController::handle(const ControllerMessage &cmd) {
RobotMessage GeneralCommandController::handle(const ControllerMessage &cmd, std::function<void(RobotMessage &)> callback) {
return {0, SuccessCode::UNKNOWN_COMMAND};
}
......
......@@ -2,7 +2,7 @@
#include <controller/CommandCategoryController/MeasurementCommandController.hpp>
RobotMessage MeasurementCommandController::handle(const ControllerMessage &cmd) {
RobotMessage MeasurementCommandController::handle(const ControllerMessage &cmd, std::function<void(RobotMessage &)> callback) {
return {0, SuccessCode::UNKNOWN_COMMAND};
}
......
......@@ -4,39 +4,53 @@
#include <entity/PMSVSettings.hpp>
SuccessCode MovementController::setSpeed(uint16_t speed) {
if(leftMotor.isMoving() || rightMotor.isMoving()) {
return SuccessCode::ROBOT_BUSY;
}
Serial.print("Speed set to: ");
Serial.println(speed);
return SuccessCode::BAD_PARAMETERS;
return SuccessCode::SUCCESS;
}
SuccessCode MovementController::setDirection(bool dir) {
Serial.print("Direction set to: ");
Serial.println(dir);
if(leftMotor.isMoving() || rightMotor.isMoving()) {
return SuccessCode::ROBOT_BUSY;
}
leftMotor.setDirection(dir);
rightMotor.setDirection(dir);
return SuccessCode::BAD_PARAMETERS;
return SuccessCode::SUCCESS;
}
SuccessCode MovementController::startMoveMM(uint16_t mm) {
return tryMoveMM(mm);
SuccessCode MovementController::startMoveMM(uint16_t mm, std::function<void(RobotMessage &)> callback) {
return tryMoveMM(mm, std::move(callback));
}
SuccessCode MovementController::startRotateDegrees(uint16_t degrees, bool rotate_direction) {
SuccessCode MovementController::startRotateDegrees(uint16_t degrees, bool rotate_direction,
std::function<void(RobotMessage &)> callback) {
if (leftMotor.isMoving() || rightMotor.isMoving()) {
return SuccessCode::ROBOT_BUSY;
}
leftMotor.setDirection(rotate_direction);
rightMotor.setDirection(!rotate_direction);
return tryMoveMM(degrees * settings.degrees_per_mm_distance);
return tryMoveMM(degrees * settings.mm_distance_per_degree, std::move(callback));
}
SuccessCode MovementController::tryMoveMM(uint16_t mm) const {
if(leftMotor.startMoveMM(mm) &&
rightMotor.startMoveMM(mm)) {
return SuccessCode::SUCCESS;
} else {
SuccessCode MovementController::tryMoveMM(uint16_t mm, std::function<void(RobotMessage &)> callback) {
if (leftMotor.isMoving() || rightMotor.isMoving()) {
return SuccessCode::ROBOT_BUSY;
}
current_callback = std::move(callback);
isCallbackEnabled = true;
if (!leftMotor.startMoveMM(mm) || !rightMotor.startMoveMM(mm)) {
return SuccessCode::HARDWARE_ERROR;
}
return SuccessCode::ACTION_STARTED;
}
SuccessCode MovementController::halt() {
......@@ -49,6 +63,12 @@ SuccessCode MovementController::halt() {
void MovementController::update() {
leftMotor.update();
rightMotor.update();
if (isCallbackEnabled && !leftMotor.isMoving() && !rightMotor.isMoving()) {
RobotMessage message = RobotMessage{SuccessCode::SUCCESS, 0, nullptr};
current_callback(message);
isCallbackEnabled = false;
}
}
MovementController::MovementController(MotorBoundary &leftMotor, MotorBoundary &rightMotor,
......
......@@ -14,3 +14,7 @@ RobotMessage::RobotMessage(uint8_t messageId, SuccessCode returnCode, uint8_t da
data[i] = dat[i];
}
}
RobotMessage::RobotMessage(SuccessCode returnCode, uint8_t data_length, const uint8_t *dat): RobotMessage(0,returnCode,data_length,dat) {
}
......@@ -15,23 +15,22 @@ constexpr uint32_t steps_per_mm = 80;
static const PMSVSettings default_settings(
0,
0,
8,
false,
false,
0,
0,
RF24_PA_LOW,
RF24_PA_HIGH,
50,
0xE0E0F1F1E0LL,
0xE0E0F1F1E4LL,
0xE0E0F1F1E4LL,
true,
32,
false,
true,
5,
4,
{13, 10, 9, 80},
{8, 3, 2, 80});
{13, 10, 9, 8},
{8, 3, 2, 8});
void setup() {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment