Merge pull request #44 from Team-Micras/feature/maze-algorithms

Feature/maze algorithms

Author: PedroDeSanti

.gitignore

@@ -59,3 +59,6 @@ cube/*
 .ionide
 
 # End of https://www.toptal.com/developers/gitignore/api/visualstudiocode,c++
+
+### Clangd ###
+.clangd

.vscode/settings.json

@@ -19,9 +19,6 @@
 
     "clangd.arguments": [
         "--pretty",
-        "--clang-tidy",
         "--compile-commands-dir=${workspaceFolder}/build/",
-        "--enable-config",
-        "--query-driver=/usr/bin/arm-none-eabi-g++"
     ]
 }

config/constants.hpp

@@ -6,7 +6,6 @@
 #define MICRAS_CONSTANTS_HPP
 
 #include <cstdint>
-#include <numbers>
 
 #include "micras/nav/action_queuer.hpp"
 #include "micras/nav/follow_wall.hpp"
@@ -28,6 +27,7 @@ constexpr float    start_offset{0.04F + wall_thickness / 2.0F};
 constexpr float    exploration_speed{0.5F};
 constexpr float    max_linear_acceleration{1.0F};
 constexpr float    max_angular_acceleration{200.0F};
+constexpr float    crash_acceleration{20.0F};
 
 constexpr core::WallSensorsIndex wall_sensors_index{
     .left_front = 0,
@@ -95,6 +95,7 @@ const nav::Maze::Config maze_config{
         {maze_width / 2, (maze_height - 1) / 2},
         {(maze_width - 1) / 2, (maze_height - 1) / 2},
     }},
+    .cost_margin = 1.2F,
 };
 
 const nav::Odometry::Config odometry_config{

include/micras/micras.hpp

@@ -73,6 +73,13 @@ class Micras {
      */
     void reset();
 
+    /**
+     * @brief Use the imu to check if the robot crashed.
+     *
+     * @return True if the robot crashed, false otherwise.
+     */
+    bool check_crash() const;
+
     /**
      * @brief Get the current objective of the robot.
      *

include/micras/states/error.hpp

@@ -12,16 +12,20 @@ class ErrorState : public BaseState {
 public:
     using BaseState::BaseState;
 
+    /**
+     * @brief Execute the entry function of this state.
+     */
+    void on_entry() override {
+        this->micras.stop();
+        this->micras.send_event(Interface::Event::ERROR);
+    }
+
     /**
      * @brief Execute this state.
      *
      * @return The id of the next state.
      */
-    uint8_t execute() override {
-        this->micras.send_event(Interface::Event::ERROR);
-
-        return this->get_id();
-    }
+    uint8_t execute() override { return this->get_id(); }
 };
 }  // namespace micras
 

include/micras/states/run.hpp

@@ -26,13 +26,18 @@ class RunState : public BaseState {
      * @return The id of the next state.
      */
     uint8_t execute() override {
+        if (this->micras.check_crash()) {
+            return Micras::State::ERROR;
+        }
+
         if (not this->micras.run()) {
             return this->get_id();
         }
 
         switch (this->micras.get_objective()) {
             case core::Objective::EXPLORE:
                 this->micras.set_objective(core::Objective::RETURN);
+                this->micras.save_best_route();
                 return Micras::State::WAIT_FOR_RUN;
 
             case core::Objective::RETURN:

micras_core/include/micras/core/vector.hpp

@@ -0,0 +1,89 @@
+/**
+ * @file
+ */
+
+#ifndef MICRAS_CORE_VECTOR_HPP
+#define MICRAS_CORE_VECTOR_HPP
+
+namespace micras::core {
+/**
+ * @brief Type to store a point in 2D space.
+ */
+struct Vector {
+    /**
+     * @brief Calculate the distance to another point.
+     *
+     * @param other The other point.
+     * @return The distance to the other point.
+     */
+    float distance(const Vector& other) const;
+
+    /**
+     * @brief Calculate the distance of the point to the origin.
+     *
+     * @return The distance of the point to the origin.
+     */
+    float magnitude() const;
+
+    /**
+     * @brief Calculate the angle between two points.
+     *
+     * @param other The other point.
+     * @return The angle between the two points.
+     */
+    float angle_between(const Vector& other) const;
+
+    /**
+     * @brief Move the point towards another point by a given distance.
+     *
+     * @param other The point to move towards.
+     * @param distance The distance to move.
+     * @return The point after moving towards the other point.
+     */
+    Vector move_towards(const Vector& other, float distance) const;
+
+    /**
+     * @brief Add a point to another.
+     *
+     * @param other The point to add.
+     * @return The result of the addition.
+     */
+    Vector operator+(const Vector& other) const;
+
+    /**
+     * @brief Subtract a point from another.
+     *
+     * @param other The point to subtract.
+     * @return The result of the subtraction.
+     */
+    Vector operator-(const Vector& other) const;
+
+    /**
+     * @brief Compare two points for equality.
+     *
+     * @param other The other point to compare.
+     * @return True if the points are equal, false otherwise.
+     */
+    bool operator==(const Vector& other) const;
+
+    /**
+     * @brief Calculate the remainder of the division of the point by a value.
+     *
+     * @param value The value to divide the point by.
+     * @return The remainder of the division.
+     */
+    Vector operator%(float value) const;
+
+    /**
+     * @brief The x coordinate of the point in 2D space.
+     */
+    float x;
+
+    /**
+     * @brief The y coordinate of the point in 2D space.
+     */
+    float y;
+};
+}  // namespace micras::core
+
+#endif  // MICRAS_CORE_VECTOR_HPP

micras_core/src/vector.cpp

@@ -0,0 +1,42 @@
+/**
+ * @file
+ */
+
+#include <cmath>
+
+#include "micras/core/vector.hpp"
+
+namespace micras::core {
+float Vector::distance(const Vector& other) const {
+    return std::hypot(other.x - this->x, other.y - this->y);
+}
+
+float Vector::magnitude() const {
+    return std::hypot(this->x, this->y);
+}
+
+float Vector::angle_between(const Vector& other) const {
+    return std::atan2(other.y - this->y, other.x - this->x);
+}
+
+Vector Vector::move_towards(const Vector& other, float distance) const {
+    const float angle = this->angle_between(other);
+    return {this->x + distance * std::cos(angle), this->y + distance * std::sin(angle)};
+}
+
+Vector Vector::operator+(const Vector& other) const {
+    return {this->x + other.x, this->y + other.y};
+}
+
+Vector Vector::operator-(const Vector& other) const {
+    return {this->x - other.x, this->y - other.y};
+}
+
+bool Vector::operator==(const Vector& other) const {
+    return this->x == other.x and this->y == other.y;
+}
+
+Vector Vector::operator%(float value) const {
+    return {std::fmod(this->x, value), std::fmod(this->y, value)};
+}
+}  // namespace micras::core

micras_hal/include/micras/hal/mcu.hpp

@@ -5,8 +5,6 @@
 #ifndef MICRAS_HAL_MCU_HPP
 #define MICRAS_HAL_MCU_HPP
 
-#include <cstdint>
-
 namespace micras::hal {
 /**
  * @brief Microcontroller unit class.

micras_nav/include/micras/nav/action_queuer.hpp

@@ -5,7 +5,7 @@
 #ifndef MICRAS_NAV_ACTION_QUEUER_HPP
 #define MICRAS_NAV_ACTION_QUEUER_HPP
 
-#include <map>
+#include <list>
 #include <memory>
 #include <queue>
 
@@ -82,7 +82,7 @@ class ActionQueuer {
     /**
      * @brief Fill the action queue with a sequence of actions to the end.
      */
-    void recompute(const std::map<uint16_t, GridPose, std::greater<>>& best_route);
+    void recompute(const std::list<GridPose>& best_route);
 
 private:
     /**