Commit 145546a2 authored by Niels Post's avatar Niels Post
Browse files

- Implemented all Action commands through their boundary

- Implemented a stack-allocated queue to prevent Heap Allocation issues later
- Added a command to rotate a certain amount of steps
parent 27d53695
......@@ -4,7 +4,9 @@
#include <communication/handler/GeneralCommandHandler.hpp>
#include <communication/handler/ActionCommandHandler.hpp>
#include <communication/handler/MeasurementCommandHandler.hpp>
#include <queue>
#include <util/StackQueue.hpp>
#include <communication/command/Command.hpp>
#include <communication/command/ReturnCommand.hpp>
......@@ -19,6 +21,7 @@ class ReturnCommand;
* This controller continuously checks for received RF24 Commands, parses them, and invokes a \refitem{CommandHandler} for them.
* The controller then transmits the ReturnCode from the handler to the sender of the command
*/
class CommunicationController {
private:
/// The radio used to communicate with the other party
......@@ -30,7 +33,9 @@ private:
/// A handler for commands in the category MEASUREMENT
MeasurementCommandHandler measurementCommandHandler;
/// Queue of commands that are still to be handled
std::queue<Command> commandQueue;
StackQueue<Command, 20> commandQueue;
///
protected:
......
......@@ -29,7 +29,8 @@ enum class ActionCommand : uint8_t {
START_MOVE_STEPS = 1,
START_MOVE_CM = 2,
START_ROTATE_DEGREES = 3,
SET_SPEED = 4
START_ROTATE_STEPS = 4,
SET_SPEED = 5
};
/**
......@@ -41,9 +42,7 @@ enum class MeasurementCommand : uint8_t {
};
static const size_t COMMAND_MAX_PARAM_LENGTH = 5;
/**
* A struct containing a command received by the robot.
......@@ -57,10 +56,12 @@ struct Command {
uint8_t message_id;
/// All parameter bytes given for this command
uint8_t parameters[5];
uint8_t parameters[COMMAND_MAX_PARAM_LENGTH];
/// The byte count of the parameters stored in "parameters"
size_t parameter_length;
Command();
/**
* Create an empty message
* @param categoryId The category (see CommandCategory)
......@@ -95,7 +96,7 @@ struct Command {
* @param cmd The command ID
* @return A pair of sizes, where first is the minimum size, and second is the maximum size
*/
static std::pair<uint8_t, uint8_t> getParameterLimits(CommandCategory cat, uint8_t cmd);
static std::pair <uint8_t, uint8_t> getParameterLimits(CommandCategory cat, uint8_t cmd);
/**
* Get a 16 bit unsigned integer from the parameters
......
......@@ -10,6 +10,7 @@
class MotorBoundary {
/// Stepper motor object that is used to move the motor
AccelStepper stepper;
bool direction = true;
public:
/**
* Create a MotorBoundary
......@@ -41,4 +42,13 @@ public:
* Usually steps the motor at least once
*/
void update();
/**
* Change the direction this motor will rotate.
*
* Note: this does not change directions for movements that are already started
* @param dir The new directin (true for forward, false for backward.
*/
void setDirection(bool dir);
};
\ No newline at end of file
......@@ -4,24 +4,90 @@
#include <communication/command/ReturnCommand.hpp>
class PMSVSettings;
class MovementController {
MotorBoundary leftMotor;
/// Boundary for the motor on the right side of the robot
MotorBoundary leftMotor;
/// Boundary for the motor on the right side of the robot
MotorBoundary rightMotor;
/// Reference to the settings object to use to convert values
const PMSVSettings &settings;
/**
* Try to move both motors a given amount of steps. (Internal use only)
*
* @param steps The amount of steps to move the motors
* @return a ReturnCode determining if the start was successful
*/
ReturnCode tryMoveSteps(long steps);
public:
/**
* Change the speed of movement.
*
* Affects movements currently being made
* @param speed New speed of movement
* @return
*/
ReturnCode setSpeed(uint16_t speed);
/**
* Change the direction of movement
*
* Does not affect direction of movements currently being made
* @param dir New direction, true being forward, and false backward
* @return
*/
ReturnCode setDirection(bool dir);
/**
* Start a movement of a specified distance
*
* @param cm Distance in CM
* @return
*/
ReturnCode startMoveCM(uint16_t cm);
/**
* Start a movement by moving motors a specified amount of steps
* @param steps The amount of steps to move
* @return
*/
ReturnCode startMoveSteps(uint16_t steps);
ReturnCode startRotateDegrees(uint16_t degrees, bool direction);
/**
* Start a rotation of a specified angle
* @param degrees The angle to rotate
* @param rotate_direction The rotation direction (true for right, false for left)
* @return
*/
ReturnCode startRotateDegrees(uint16_t degrees, bool rotate_direction);
/**
* Start a rotation by moving motors a specified amount of steps in opposite direction
* @param steps The amount of steps to move each motor
* @param rotate_direction The rotation direction (true for right, false for left)
* @return
*/
ReturnCode startRotateSteps(uint16_t steps, bool rotate_direction);
/**
* Stop all movements currently being made
* @return
*/
ReturnCode halt();
/**
* Update function for the movementcontroller.
* Should be called often to be able to step motors properly
*/
void update();
MovementController(const PMSVSettings &pmsvSettings );
/**
* Create a MovementController
*
* Initializes both Motors using the settings object
* @param pmsvSettings
*/
MovementController(const PMSVSettings &pmsvSettings);
};
\ No newline at end of file
#pragma once
template<typename T, size_t max_size>
class StackQueue {
size_t _front = 0;
size_t _back = 0;
T data[max_size];
public:
StackQueue() {}
bool empty() {
return _front == _back;
}
bool push(const T& el) {
if(_back == (_front - 1) || (_back == (max_size - 1) && _front == 0)){
return false;
}
data[_back] = el;
_back = (_back + 1) % max_size;
return true;
}
T pop() {
T el = data[_front];
_front = (_front + 1) % max_size;
return std::move(el);
}
T front() {
return data[_front];
}
};
\ No newline at end of file
......@@ -11,7 +11,7 @@
CommunicationController::CommunicationController(const PMSVSettings &settings, MovementController &movementController)
: nrfRadio(settings.nrf_pin_ce,
settings.nrf_pin_csn), generalCommandHandler(), actionCommandHandler(movementController),
measurementCommandHandler() {
measurementCommandHandler(), commandQueue() {
nrfRadio.begin();
nrfRadio.setPALevel(settings.nrf_palevel);
......@@ -39,7 +39,6 @@ CommunicationController::CommunicationController(const PMSVSettings &settings, M
void CommunicationController::update() {
nrfRadio.startListening();
parseNextCommand();
handleQueuedCommand();
}
......@@ -51,7 +50,8 @@ void CommunicationController::parseNextCommand() {
nrfRadio.read(data, size);
if (Command::validate(data, size)) {
commandQueue.push(Command::parse(data, size));
const Command &el = Command::parse(data, size);
commandQueue.push(el);
nrfRadio.stopListening();
} else {
Serial.println("Invalid command received");
......@@ -62,6 +62,7 @@ void CommunicationController::parseNextCommand() {
void CommunicationController::handleQueuedCommand() {
if (!commandQueue.empty()) {
const auto &command = commandQueue.front();
Serial.println(command.message_id);
switch (static_cast<CommandCategory>(command.category_id)) {
case CommandCategory::GENERAL:
sendReturnCommand(generalCommandHandler.handle(command));
......
......@@ -7,7 +7,7 @@
Command Command::parse(uint8_t *data, const uint8_t &len) {
Command c(data[0] >> 5u, data[0] & 0x1Fu, data[2]);
Command c(data[0] >> 5u, data[0] & 0x1Fu, data[1]);
c.parameter_length = len - 2;
......@@ -15,7 +15,7 @@ Command Command::parse(uint8_t *data, const uint8_t &len) {
c.parameters[i] = data[i + 2];
}
for (size_t i = c.parameter_length; i < 30; i++) {
for (size_t i = c.parameter_length; i < COMMAND_MAX_PARAM_LENGTH; i++) {
c.parameters[i] = 0;
}
......@@ -24,7 +24,6 @@ Command Command::parse(uint8_t *data, const uint8_t &len) {
bool Command::validate(const uint8_t *data, const uint8_t &len) {
if (len < 2) {
Serial.println("toto short");
return false;
}
......@@ -36,7 +35,6 @@ bool Command::validate(const uint8_t *data, const uint8_t &len) {
if (param_length >= sizes.first && param_length <= sizes.second) {
Serial.println("Good boy command");
return true;
}
......@@ -85,6 +83,8 @@ std::pair<uint8_t, uint8_t> Command::getParameterLimits(CommandCategory cat, uin
return {2, 3};
case ActionCommand::START_ROTATE_DEGREES:
return {3, 3};
case ActionCommand::START_ROTATE_STEPS:
return {3, 3};
case ActionCommand::SET_SPEED:
return {1, 1};
}
......@@ -101,3 +101,5 @@ std::pair<uint8_t, uint8_t> Command::getParameterLimits(CommandCategory cat, uin
return {255, 255};
}
Command::Command(): category_id(255), command_id(255), message_id(255), parameters{255}, parameter_length(255) {}
......@@ -32,6 +32,10 @@ ReturnCommand ActionCommandHandler::handle(const Command &cmd) {
case ActionCommand::START_ROTATE_DEGREES:
movementController.startRotateDegrees(cmd.get_param16(0), cmd.parameters[2]);
break;
case ActionCommand::START_ROTATE_STEPS:
movementController.startRotateSteps(cmd.get_param16(0), cmd.parameters[2]);
break;
case ActionCommand::SET_SPEED:
movementController.setSpeed(cmd.parameters[0]);
......
......@@ -59,17 +59,17 @@ static const PMSVSettings default_settings(
false,
0,
0,
RF24_PA_HIGH,
RF24_PA_LOW,
50,
0xE0E0F1F1E0LL,
0xE0E0F1F1E4LL,
true,
32,
false,
9,
10,
{12, 5, 4, 80},
{11, 31, 30, 80});
5,
4,
{13, 10, 9, 80},
{8, 3, 2, 80});
void setup() {
......
......@@ -5,7 +5,12 @@
#include <movement/MotorBoundary.hpp>
bool MotorBoundary::startMoveSteps(uint16_t steps) {
stepper.move(steps);
long tomove = steps;
if(!direction) {
tomove *= -1;
}
Serial.println(tomove);
stepper.move(tomove);
stepper.enableOutputs();
return false;
}
......@@ -16,7 +21,7 @@ void MotorBoundary::halt() {
}
void MotorBoundary::update() {
if (stepper.distanceToGo() > 0) {
if (stepper.distanceToGo() != 0) {
stepper.run();
} else {
stepper.disableOutputs();
......@@ -30,6 +35,10 @@ MotorBoundary::MotorBoundary(uint8_t enable_pin, uint8_t step_pin, uint8_t dir_p
stepper.setAcceleration(1000 * steps_per_mm);
stepper.setEnablePin(enable_pin);
stepper.setPinsInverted(false, false, true);
stepper.enableOutputs();
// stepper.enableOutputs();
}
void MotorBoundary::setDirection(bool dir) {
direction = dir;
}
......@@ -5,6 +5,15 @@
#include <Arduino.h>
#include <pmsv/PMSVSettings.hpp>
ReturnCode MovementController::tryMoveSteps(long steps) {
if (!leftMotor.startMoveSteps(steps) ||
!rightMotor.startMoveSteps(steps)) {
return ReturnCode::HARDWARE_ERROR;
}
return ReturnCode::SUCCESS;
}
ReturnCode MovementController::setSpeed(uint16_t speed) {
Serial.print("Speed set to: ");
Serial.println(speed);
......@@ -14,32 +23,40 @@ ReturnCode MovementController::setSpeed(uint16_t speed) {
ReturnCode MovementController::setDirection(bool dir) {
Serial.print("Direction set to: ");
Serial.println(dir);
leftMotor.setDirection(dir);
rightMotor.setDirection(dir);
return ReturnCode::BAD_PARAMETERS;
}
ReturnCode MovementController::startMoveCM(uint16_t cm) {
Serial.print("Moving in cm: ");
Serial.println(cm);
return ReturnCode::BAD_PARAMETERS;
const long &stepsToMove = settings.steps_per_cm * cm;
return tryMoveSteps(stepsToMove);
}
ReturnCode MovementController::startMoveSteps(uint16_t steps) {
Serial.print("Moving in steps: ");
Serial.println(steps);
return ReturnCode::BAD_PARAMETERS;
return tryMoveSteps(steps);
}
ReturnCode MovementController::startRotateDegrees(uint16_t degrees, bool direction) {
Serial.print("Rotating: degrees: ");
Serial.print(degrees);
Serial.print(", direction: ");
Serial.println(direction);
return ReturnCode::BAD_PARAMETERS;
ReturnCode MovementController::startRotateDegrees(uint16_t degrees, bool rotate_direction) {
leftMotor.setDirection(rotate_direction);
rightMotor.setDirection(!rotate_direction);
const long &stepsToMove = settings.steps_per_degree * degrees;
return tryMoveSteps(stepsToMove);
}
ReturnCode MovementController::startRotateSteps(uint16_t steps, bool rotate_direction) {
leftMotor.setDirection(rotate_direction);
rightMotor.setDirection(!rotate_direction);
return tryMoveSteps(steps);
}
ReturnCode MovementController::halt() {
Serial.println("Halting! ");
return ReturnCode::BAD_PARAMETERS;
leftMotor.halt();
rightMotor.halt();
return ReturnCode::SUCCESS;
}
void MovementController::update() {
......@@ -51,7 +68,12 @@ MovementController::MovementController(const PMSVSettings &pmsvSettings) : leftM
pmsvSettings.leftMotor.pin_step,
pmsvSettings.leftMotor.pin_dir,
pmsvSettings.leftMotor.steps_per_mm),
rightMotor(pmsvSettings.rightMotor.pin_enable,
pmsvSettings.rightMotor.pin_step,
pmsvSettings.rightMotor.pin_dir,
pmsvSettings.rightMotor.steps_per_mm){}
rightMotor(
pmsvSettings.rightMotor.pin_enable,
pmsvSettings.rightMotor.pin_step,
pmsvSettings.rightMotor.pin_dir,
pmsvSettings.rightMotor.steps_per_mm),
settings(pmsvSettings) {}
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