Feature/solving graph

.vscode/settings.json

@@ -15,10 +15,5 @@
 
     "doxdocgen.file.fileOrder": [
         "file",
-    ],
-
-    "clangd.arguments": [
-        "--pretty",
-        "--compile-commands-dir=${workspaceFolder}/build/",
     ]
 }

config/constants.hpp

@@ -24,10 +24,11 @@ constexpr float    cell_size{0.18};
 constexpr uint32_t loop_time_us{1042};
 constexpr float    wall_thickness{0.0126F};
 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 float    max_linear_acceleration{9.0F};
+constexpr float    max_linear_deceleration{9.0F};
+constexpr float    max_angular_acceleration{300.0F};
+constexpr float    crash_acceleration{35.0F};
+constexpr float    fan_speed{100.0F};
 
 constexpr core::WallSensorsIndex wall_sensors_index{
     .left_front = 0,
@@ -51,24 +52,23 @@ using Maze = TMaze<maze_width, maze_height>;
 const nav::ActionQueuer::Config action_queuer_config{
     .cell_size = cell_size,
     .start_offset = start_offset,
+    .curve_safety_margin = 0.0375F + 0.015F,
     .exploring =
         {
-            .max_linear_speed = exploration_speed,
+            .max_linear_speed = 0.4F,
             .max_linear_acceleration = max_linear_acceleration,
-            .max_linear_deceleration = max_linear_acceleration,
-            .curve_radius = cell_size / 2.0F,
+            .max_linear_deceleration = max_linear_deceleration,
             .max_centrifugal_acceleration = 2.78F,
             .max_angular_acceleration = max_angular_acceleration,
         },
     .solving =
         {
-            .max_linear_speed = exploration_speed,
+            .max_linear_speed = 3.0F,
             .max_linear_acceleration = max_linear_acceleration,
-            .max_linear_deceleration = max_linear_acceleration,
-            .curve_radius = cell_size / 2.0F,
-            .max_centrifugal_acceleration = 1.0F,
+            .max_linear_deceleration = max_linear_deceleration,
+            .max_centrifugal_acceleration = 5.0F,
             .max_angular_acceleration = max_angular_acceleration,
-        },
+        }
 };
 
 const nav::FollowWall::Config follow_wall_config{
@@ -81,10 +81,11 @@ const nav::FollowWall::Config follow_wall_config{
             .saturation = 1.0F,
             .max_integral = -1.0F,
         },
-    .max_linear_speed = 0.1F,
-    .post_threshold = 16.5F,
+    .max_angular_acceleration = max_angular_acceleration,
     .cell_size = cell_size,
-    .post_clearance = 0.2F * cell_size,
+    .post_threshold = 16.5F,
+    .post_reference = 0.44F * cell_size,
+    .post_clearance = 0.025F,
 };
 
 const nav::Maze::Config maze_config{
@@ -96,17 +97,16 @@ const nav::Maze::Config maze_config{
         {(maze_width - 1) / 2, (maze_height - 1) / 2},
     }},
     .cost_margin = 1.2F,
+    .action_queuer_config = action_queuer_config,
 };
 
 const nav::Odometry::Config odometry_config{
     .linear_cutoff_frequency = 5.0F,
     .wheel_radius = 0.0112F,
-    .initial_pose = {{0.0F, 0.0F}, 0.0F},
+    .initial_pose = {{cell_size / 2.0F, start_offset}, std::numbers::pi / 2.0F},
 };
 
 const nav::SpeedController::Config speed_controller_config{
-    .max_linear_acceleration = max_linear_acceleration,
-    .max_angular_acceleration = max_angular_acceleration,
     .linear_pid =
         {
             .kp = 10.0F,

include/micras/interface.hpp

@@ -27,7 +27,15 @@ class Interface {
         SOLVE = 1,
         CALIBRATE = 2,
         ERROR = 3,
-        NUMBER_OF_EVENTS = 4,
+        TURN_ON_FAN = 4,
+        TURN_OFF_FAN = 5,
+        TURN_ON_DIAGONAL = 6,
+        TURN_OFF_DIAGONAL = 7,
+        TURN_ON_BOOST = 8,
+        TURN_OFF_BOOST = 9,
+        TURN_ON_RISKY = 10,
+        TURN_OFF_RISKY = 11,
+        NUMBER_OF_EVENTS = 12,
     };
 
     /**
@@ -74,6 +82,16 @@ class Interface {
     bool peek_event(Event event) const;
 
 private:
+    /**
+     * @brief Enum for what each dip switch pin does.
+     */
+    enum DipSwitchPins : uint8_t {
+        FAN = 0,
+        DIAGONAL = 1,
+        BOOST = 2,
+        RISKY = 3,
+    };
+
     /**
      * @brief Addressable RGB LED object.
      */
@@ -103,6 +121,11 @@ class Interface {
      * @brief Array of the listed events.
      */
     std::array<bool, Event::NUMBER_OF_EVENTS> events{};
+
+    /**
+     * @brief Array to store the last dip switch states.
+     */
+    std::array<bool, 4> dip_switch_states{};
 };
 }  // namespace micras
 

include/micras/micras.hpp

@@ -134,11 +134,16 @@ class Micras {
      */
     void load_best_route();
 
+    /**
+     * @brief Handle events from the interface.
+     */
+    void handle_events();
+
 private:
     /**
      * @brief Enum for the type of calibration being performed.
      */
-    enum CalibrationType : uint8_t {
+    enum class CalibrationType : uint8_t {
         SIDE_WALLS = 0,  // Calibrate side walls and front free space detection.
         FRONT_WALL = 1,  // Calibrate front wall detection.
     };
@@ -190,7 +195,7 @@ class Micras {
     /**
      * @brief Finite state machine for the robot.
      */
-    core::FSM fsm{State::INIT};
+    core::Fsm fsm{State::INIT};
 
     /**
      * @brief Class for controlling the interface with the external world.

include/micras/states/base.hpp

@@ -9,7 +9,7 @@
 #include "micras/micras.hpp"
 
 namespace micras {
-class BaseState : public core::FSM::State {
+class BaseState : public core::Fsm::State {
 public:
     /**
      * @brief Construct a new BaseState object.

include/micras/states/idle.hpp

@@ -26,6 +26,8 @@ class IdleState : public BaseState {
      * @return The id of the next state.
      */
     uint8_t execute() override {
+        this->micras.handle_events();
+
         if (this->micras.acknowledge_event(Interface::Event::EXPLORE)) {
             this->micras.set_objective(core::Objective::EXPLORE);
 

micras_core/include/micras/core/fsm.hpp

@@ -10,7 +10,7 @@
 #include <unordered_map>
 
 namespace micras::core {
-class FSM {
+class Fsm {
 public:
     class State {
     public:
@@ -67,7 +67,7 @@ class FSM {
      *
      * @param initial_state_id The id of the initial state.
      */
-    explicit FSM(uint8_t initial_state_id);
+    explicit Fsm(uint8_t initial_state_id);
 
     /**
      * @brief Add a state to the FSM.

micras_core/include/micras/core/pid_controller.hpp

@@ -94,6 +94,11 @@ class PidController {
      */
     float max_integral;
 
+    /**
+     * @brief Flag indicating whether this is the first run of the controller.
+     */
+    bool first_run = true;
+
     /**
      * @brief Accumulated error for integrative term.
      */

micras_core/include/micras/core/types.hpp

@@ -20,7 +20,7 @@ struct Observation {
 /**
  * @brief Possible objectives of the robot.
  */
-enum Objective : uint8_t {
+enum class Objective : uint8_t {
     EXPLORE = 0,
     RETURN = 1,
     SOLVE = 2

micras_core/include/micras/core/utils.hpp

@@ -38,7 +38,7 @@ constexpr T remap(T value, T in_min, T in_max, T out_min, T out_max) {
  */
 template <typename T>
 constexpr T move_towards(T value, T target, T step) {
-    return std::max(std::min(value + step, target), value - step);
+    return std::clamp(target, value - step, value + step);
 }
 
 /**

micras_core/src/fsm.cpp

@@ -5,19 +5,19 @@
 #include "micras/core/fsm.hpp"
 
 namespace micras::core {
-FSM::State::State(uint8_t id) : id{id} { }
+Fsm::State::State(uint8_t id) : id{id} { }
 
-uint8_t FSM::State::get_id() const {
+uint8_t Fsm::State::get_id() const {
     return this->id;
 }
 
-FSM::FSM(uint8_t initial_state_id) : current_state_id{initial_state_id} { }
+Fsm::Fsm(uint8_t initial_state_id) : current_state_id{initial_state_id} { }
 
-void FSM::add_state(std::unique_ptr<State> state) {
+void Fsm::add_state(std::unique_ptr<State> state) {
     this->states.emplace(state->get_id(), std::move(state));
 }
 
-void FSM::update() {
+void Fsm::update() {
     if (this->current_state_id != this->previous_state_id) {
         this->states.at(this->current_state_id)->on_entry();
     }

micras_core/src/pid_controller.cpp

@@ -33,9 +33,15 @@ void PidController::reset() {
     this->error_acc = 0;
     this->prev_state = 0;
     this->last_response = 0;
+    this->first_run = true;
 }
 
 float PidController::compute_response(float state, float elapsed_time, bool save) {
+    if (first_run) {
+        this->prev_state = state;
+        this->first_run = false;
+    }
+
     const float state_change = (state - this->prev_state) / elapsed_time;
     return this->compute_response(state, elapsed_time, state_change, save);
 }