Update 16-10

- Class Structure mostly implemented
- Communication implemented
- Commands are checked to be valid before execution
- Movement controller prints executed commands

.gitignore

+.pio
+CMakeListsPrivate.txt
+cmake-build-*/
+CMakeLists.txt

include/README

+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

include/communication/CommunicationController.hpp

+#pragma once
+
+#include <RF24.h>
+#include <communication/handler/GeneralCommandHandler.hpp>
+#include <communication/handler/ActionCommandHandler.hpp>
+#include <communication/handler/MeasurementCommandHandler.hpp>
+#include <queue>
+
+class PMSVSettings;
+class Command;
+class ReturnCommand;
+
+class CommunicationController {
+private:
+    RF24                      nrfRadio;
+    GeneralCommandHandler     generalCommandHandler;
+    ActionCommandHandler      actionCommandHandler;
+    MeasurementCommandHandler measurementCommandHandler;
+
+    std::queue<Command> commandQueue;
+
+
+protected:
+    void handleCommand(const Command &command);
+    void sendReturnCommand(const ReturnCommand &command);
+    void parseNextCommand();
+public:
+    CommunicationController(const PMSVSettings &settings, MovementController &movementController);
+
+    void update();
+
+
+};
+

include/communication/command/Command.hpp

+#pragma once
+
+#include <cstdint>
+#include <cstdio>
+
+enum class CommandCategory: uint8_t {
+    GENERAL,
+    ACTION,
+    MOVEMENT
+};
+
+enum class GeneralCommand: uint8_t {
+    SET_COMMUNICATION_ACTIVEMODE
+};
+
+enum class ActionCommand: uint8_t {
+    CANCEL_MOVEMENT,
+    START_MOVE_STEPS,
+    START_MOVE_CM,
+    START_ROTATE_DEGREES,
+    SET_SPEED
+};
+
+enum class MeasurementCommand: uint8_t {
+    GET_MEASUREMENT_RAW,
+    GET_DISTANCE_SENSOR
+};
+
+struct CommandData {
+    uint8_t category_id;
+    uint8_t command_id;
+    uint8_t min_param_length;
+    uint8_t max_param_length;
+};
+
+const CommandData commandDetails[] = {
+        {0,0, 1,1},
+        {1,0,0,0},
+        {1,1,2,3},
+        {1,2,2,3},
+        {1,3,3,3},
+        {1,4,1,1},
+        {2,0,1,2},
+        {2,1,1,2}
+};
+
+struct Command {
+
+    uint8_t category_id;
+    uint8_t command_id;
+    uint8_t message_id;
+    uint8_t parameters[30]; // TODO this could be handled better
+    size_t parameter_length;
+
+    /**
+     * Create an empty message
+     * @param categoryId The category (see CommandCategory)
+     * @param commandId The command ID (see GeneralCommand, ActionCommand, MeasurementCommand)
+     * @param messageId
+     */
+    Command(uint8_t categoryId, uint8_t commandId, uint8_t messageId);
+
+    /**
+     * Parse an array of bytes into a command
+     * @param data Pointer to the start of the array
+     * @param len Length of the array
+     * @return
+     */
+    static Command parse(uint8_t *data, const uint8_t &len);
+
+    /**
+     * Check if an array of data is a properly formatted Command
+     *
+     * \details Checks if the size is correct for the given command.
+     * Checks if the command given is known.
+     * @param data
+     * @param len
+     * @return
+     */
+    static bool validate(const uint8_t * data, const uint8_t & len);
+
+    uint16_t get_param16(const size_t &startIndex, bool msb_first = true) const;
+
+    uint32_t get_param32(const size_t &startIndex, bool msb_first = true) const;
+};

include/communication/command/ReturnCommand.hpp

+#pragma once
+
+#include <cstdint>
+#include <cstdio>
+
+enum class ReturnCode : uint8_t {
+    SUCCESS        = 0,
+    ROBOT_BUSY     = 1,
+    HARDWARE_ERROR = 2,
+    MOTOR_STALL    = 3,
+    BAD_PARAMETERS = 4,
+    NO_SUCH_SENSOR = 5
+};
+
+struct ReturnCommand {
+    uint8_t message_id;
+    ReturnCode returnCode;
+    uint8_t data[4];
+
+    ReturnCommand(uint8_t messageId, ReturnCode returnCode, const uint8_t *dat);
+};

include/communication/handler/ActionCommandHandler.hpp

+#pragma once
+
+#include <communication/handler/CommandHandler.hpp>
+
+class MovementController;
+
+class ActionCommandHandler: public CommandHandler {
+    MovementController &movementController;
+public:
+    ReturnCommand handle(const Command &cmd) override;
+
+    ActionCommandHandler(MovementController &movementController);
+};

include/communication/handler/CommandHandler.hpp

+#pragma once
+
+#include <communication/command/Command.hpp>
+#include <communication/command/ReturnCommand.hpp>
+
+class CommandHandler {
+public:
+    virtual ReturnCommand handle(const Command &cmd) = 0;
+};

include/communication/handler/GeneralCommandHandler.hpp

+#pragma once
+#include <communication/handler/CommandHandler.hpp>
+
+class GeneralCommandHandler: public CommandHandler {
+public:
+    ReturnCommand handle(const Command &cmd) override;
+
+};

include/communication/handler/MeasurementCommandHandler.hpp

+#pragma once
+#include <communication/handler/CommandHandler.hpp>
+
+class MeasurementCommandHandler: public CommandHandler {
+public:
+    ReturnCommand handle(const Command &cmd) override;
+};
+

include/movement/MotorController.hpp

+#pragma once
+
+#include <cstdint>
+
+
+class MotorController {
+public:
+    bool startMoveSteps(uint16_t steps) {
+        return false;
+    }
+
+    void halt() {
+
+    }
+};
\ No newline at end of file

include/movement/MovementController.hpp

+#pragma once
+
+#include <movement/MotorController.hpp>
+#include <communication/command/ReturnCommand.hpp>
+
+class MovementController {
+    MotorController leftMotor;
+    MotorController rightMotor;
+
+public:
+    ReturnCode setSpeed(uint16_t speed);
+    ReturnCode setDirection(bool dir);
+    ReturnCode startMoveCM(uint16_t cm);
+    ReturnCode startMoveSteps(uint16_t steps);
+    ReturnCode startRotateDegrees(uint16_t degrees, bool direction);
+    ReturnCode halt();
+    void update();
+};
\ No newline at end of file

include/pmsv/PMSVRobot.hpp

+#pragma once
+
+#include <communication/CommunicationController.hpp>
+#include <movement/MovementController.hpp>
+#include <pmsv/PMSVSettings.hpp>
+
+class PMSVRobot {
+    MovementController      movementController;
+    CommunicationController communicationController;
+    PMSVSettings            settings;
+
+
+public:
+    PMSVRobot(PMSVSettings &settings);
+
+    void update();
+};
+

include/pmsv/PMSVSettings.hpp

+#pragma once
+
+#include <RF24.h>
+
+class PMSVSettings {
+
+public:
+    // Settings related to the Movement Controller
+    uint16_t steps_per_cm;
+    uint16_t steps_per_degree;
+    bool     left_motor_fwd;
+    bool     right_motor_fwd;
+    uint16_t min_speed;
+    uint16_t max_speed;
+
+    // Curve start_curve
+    // Curve stop_curve
+
+
+    // Settings related to NRF
+    rf24_pa_dbm_e nrf_palevel;
+    uint8_t       nrf_channel;
+    uint64_t      nrf_readingpipe = 0xE0E0F1F1E0LL;
+    uint64_t      nrf_writingpipe = 0xE0E0F1F1E4LL;
+    bool          nrf_enable_dynamicpayloads;
+    size_t        nrf_payload_size;
+    bool          nrf_enable_autoack;
+
+    uint8_t nrf_pin_ce;
+    uint8_t nrf_pin_csn;
+
+
+    PMSVSettings(uint16_t stepsPerCm, uint16_t stepsPerDegree, 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)
+            : steps_per_cm(stepsPerCm),
+              steps_per_degree(
+                      stepsPerDegree),
+              left_motor_fwd(
+                      leftMotorFwd),
+              right_motor_fwd(
+                      rightMotorFwd),
+              min_speed(minSpeed),
+              max_speed(maxSpeed),
+              nrf_palevel(nrfPalevel),
+              nrf_channel(nrfChannel),
+              nrf_readingpipe(
+                      nrfReadingpipe), nrf_writingpipe(nrfWritingpipe),
+              nrf_enable_dynamicpayloads(
+                      nrfEnableDynamicpayloads),
+              nrf_payload_size(
+                      nrfPacketSize),
+              nrf_enable_autoack(
+                      nrfEnableAutoack),
+              nrf_pin_ce(nrfPinCe), nrf_pin_csn(nrfPinCsn) {}
+
+};
+

platformio.ini

+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:due]
+platform = atmelsam
+board = due
+framework = arduino
+lib_deps = tmrh20/RF24@^1.3.9

src/communication/CommunicationController.cpp

+#include <queue>
+
+#include <communication/CommunicationController.hpp>
+#include <movement/MovementController.hpp>
+#include <pmsv/PMSVSettings.hpp>
+
+#include <communication/command/ReturnCommand.hpp>
+#include <communication/command/Command.hpp>
+
+
+CommunicationController::CommunicationController(const PMSVSettings &settings, MovementController &movementController)
+        : nrfRadio(settings.nrf_pin_ce,
+                   settings.nrf_pin_csn), generalCommandHandler(), actionCommandHandler(movementController), measurementCommandHandler(){
+    nrfRadio.begin();
+    nrfRadio.setPALevel(settings.nrf_palevel);
+
+    if (settings.nrf_enable_dynamicpayloads) {
+        nrfRadio.enableDynamicPayloads();
+    } else {
+        nrfRadio.disableDynamicPayloads();
+        nrfRadio.setPayloadSize(settings.nrf_payload_size);
+    }
+
+    nrfRadio.setAutoAck(settings.nrf_enable_autoack);
+
+    nrfRadio.setChannel(settings.nrf_channel);
+
+    nrfRadio.openReadingPipe(1, settings.nrf_readingpipe);
+
+    nrfRadio.openWritingPipe(settings.nrf_writingpipe);
+
+    nrfRadio.powerUp();
+
+    nrfRadio.printDetails();
+    delay(2000);
+}
+
+void CommunicationController::update() {
+    nrfRadio.startListening();
+    if (nrfRadio.available()) {
+        parseNextCommand();
+    }
+
+    if (!commandQueue.empty()) {
+        handleCommand(commandQueue.front());
+        commandQueue.pop();
+    }
+
+}
+
+void CommunicationController::parseNextCommand() {
+    auto size = nrfRadio.getDynamicPayloadSize();
+    uint8_t data[size];
+    nrfRadio.read(data, size);
+
+    if (Command::validate(data, size)) {
+        commandQueue.push(Command::parse(data, size));
+        nrfRadio.stopListening();
+    } else {
+        Serial.println("Invalid command received");
+    }
+}
+
+void CommunicationController::handleCommand(const Command &command) {
+    switch (static_cast<CommandCategory>(command.category_id)) {
+        case CommandCategory::GENERAL:
+            sendReturnCommand(generalCommandHandler.handle(command));
+            break;
+        case CommandCategory::ACTION:
+            sendReturnCommand(actionCommandHandler.handle(command));
+            break;
+        case CommandCategory::MOVEMENT:
+            sendReturnCommand(measurementCommandHandler.handle(command));
+            break;
+    }
+}
+
+void CommunicationController::sendReturnCommand(const ReturnCommand &command) {
+    nrfRadio.stopListening();
+    if(!nrfRadio.write(&command, 6) )
+    {
+        Serial.println("Sending return failed");
+    }
+}

src/communication/command/Command.cpp

+//
+// Created by Niels on 16/10/2020.
+//
+
+#include <communication/command/Command.hpp>
+#include <Arduino.h>
+
+
+Command Command::parse(uint8_t *data, const uint8_t &len) {
+    Command c(data[0] >> 5u, data[0] & 0x1Fu, data[2]);
+
+    c.parameter_length = len - 2;
+
+    for(size_t i = 0; i < c.parameter_length; i++) {
+        c.parameters[i] = data[i+2];
+    }
+
+    for(size_t i = c.parameter_length; i < 30; i++) {
+        c.parameters[i] = 0;
+    }
+
+    return c;
+}
+
+bool Command::validate(const uint8_t *data, const uint8_t &len) {
+    if(len < 2) {
+        Serial.println("toto short");
+        return false;
+    }
+
+    uint8_t cat = data[0] >> 5u;
+    uint8_t cmd = data[0] & 0x1Fu;
+    uint8_t param_length = len - 2;
+
+    for(const CommandData & details : commandDetails) {
+        if(details.category_id == cat &&
+           details.command_id == cmd) {
+            Serial.println("Command found");
+            if(param_length >= details.min_param_length && param_length <= details.max_param_length) {
+                Serial.println("Good boy command");
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+Command::Command(uint8_t categoryId, uint8_t commandId, uint8_t messageId) : category_id(categoryId),
+                                                                             command_id(commandId),
+                                                                             message_id(messageId) {}
+
+uint16_t Command::get_param16(const size_t &startIndex, bool msb_first) const {
+    if(msb_first) {
+        return (parameters[0] << 8) | parameters[1];
+    } else {
+        return (parameters[1] << 8) | parameters[0];
+    }
+}
+
+uint32_t Command::get_param32(const size_t &startIndex, bool msb_first) const {
+    if(msb_first) {
+        return (parameters[0] << 24) | (parameters[1] << 24) |  (parameters[2] << 24) | parameters[3];
+    } else {
+        return (parameters[3] << 24) | (parameters[2] << 24) |  (parameters[1] << 24) | parameters[0];
+    }
+}

src/communication/command/ReturnCommand.cpp

+//
+// Created by Niels on 16/10/2020.
+//
+
+
+
+#include <communication/command/ReturnCommand.hpp>
+
+
+
+ReturnCommand::ReturnCommand(uint8_t messageId, ReturnCode returnCode, const uint8_t *dat) : message_id(messageId),
+                                                                                       returnCode(returnCode){
+    for( size_t i = 0; i < 4; i++) {
+        data[i] = dat[i];
+    }
+}

src/communication/handler/ActionCommandHandler.cpp

+//
+// Created by Niels on 16/10/2020.
+//
+
+#include <communication/handler/ActionCommandHandler.hpp>
+
+#include <movement/MovementController.hpp>
+#include <communication/command/ReturnCommand.hpp>
+#include <communication/command/Command.hpp>
+
+ReturnCommand ActionCommandHandler::handle(const Command &cmd) {
+    switch(static_cast<ActionCommand>(cmd.command_id)){
+        case ActionCommand::CANCEL_MOVEMENT:
+            movementController.halt();
+            break;
+        case ActionCommand::START_MOVE_STEPS: {
+            if(cmd.parameter_length > 2) {
+                movementController.setDirection(cmd.parameters[2]);
+            }
+
+            movementController.startMoveSteps(cmd.get_param16(0));
+            break;
+        }
+        case ActionCommand::START_MOVE_CM: {
+            if(cmd.parameter_length > 2) {
+                movementController.setDirection(cmd.parameters[2]);
+            }
+
+            movementController.startMoveSteps(cmd.get_param16(0));
+            break;
+        }
+        case ActionCommand::START_ROTATE_DEGREES:
+            movementController.startRotateDegrees(cmd.get_param16(0), cmd.parameters[2]);
+
+            break;
+        case ActionCommand::SET_SPEED:
+            movementController.setSpeed(cmd.parameters[0]);
+            break;
+    }
+
+    return {0, ReturnCode::SUCCESS, nullptr};
+}
+
+ActionCommandHandler::ActionCommandHandler(MovementController &movementController) : movementController(
+        movementController) {}

src/communication/handler/GeneralCommandHandler.cpp

+//
+// Created by Niels on 16/10/2020.
+//
+
+#include <communication/handler/GeneralCommandHandler.hpp>
+
+#include <communication/command/ReturnCommand.hpp>
+#include <communication/command/Command.hpp>
+
+ReturnCommand GeneralCommandHandler::handle(const Command &cmd) {
+    return {0,ReturnCode::BAD_PARAMETERS, nullptr};
+}

src/communication/handler/MeasurementCommandHandler.cpp

+//
+// Created by Niels on 16/10/2020.
+//
+
+#include <communication/handler/MeasurementCommandHandler.hpp>
+
+#include <communication/command/ReturnCommand.hpp>
+#include <communication/command/Command.hpp>
+