Dynamic Traction Demonstration State Machine

CMakeLists.txt

@@ -5,7 +5,7 @@ cmake_minimum_required(VERSION 3.16.0 FATAL_ERROR)
 # -------------------------------------------------- #
 
 # List of all libraries for linking purposes
-set(ALL_LIBS "data;brakes;navigation;propulsion;propulsion_can;sensors;state_machine;telemetry;utils;utils_concurrent;utils_io;utils_math")
+set(ALL_LIBS "data;brakes;navigation;propulsion;propulsion_can;sensors;state_machine;demo_state_machine;telemetry;utils;utils_concurrent;utils_io;utils_math")
 function(make_lib target, include_path)
     file(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp")
     file(GLOB code "${CMAKE_CURRENT_SOURCE_DIR}/*.cpp")
@@ -121,6 +121,7 @@ add_subdirectory(${CMAKE_SOURCE_DIR}/src/navigation)
 add_subdirectory(${CMAKE_SOURCE_DIR}/src/propulsion)
 add_subdirectory(${CMAKE_SOURCE_DIR}/src/sensors)
 add_subdirectory(${CMAKE_SOURCE_DIR}/src/state_machine)
+add_subdirectory(${CMAKE_SOURCE_DIR}/src/demo_state_machine)
 add_subdirectory(${CMAKE_SOURCE_DIR}/src/telemetry)
 add_subdirectory(${CMAKE_SOURCE_DIR}/src/utils)
 
@@ -148,3 +149,20 @@ target_link_libraries(${target}
     utils_io
     utils_math
 )
+
+set(target "hyped_demo")
+add_executable(${target} ${CMAKE_SOURCE_DIR}/run/demo.cpp)
+target_link_libraries(${target}
+    data
+    brakes
+    navigation
+    propulsion
+    propulsion_can
+    sensors
+    demo_state_machine
+    telemetry
+    utils
+    utils_concurrent
+    utils_io
+    utils_math
+)

run/demo.cpp

@@ -0,0 +1,49 @@
+#include <fstream>
+#include <iostream>
+#include <memory>
+
+#include <brakes/main.hpp>
+#include <navigation/main.hpp>
+#include <propulsion/main.hpp>
+#include <sensors/main.hpp>
+#include <demo_state_machine/main.hpp>
+#include <telemetry/main.hpp>
+#include <utils/system.hpp>
+
+int main(int argc, char *argv[])
+{
+  hyped::utils::System::parseArgs(argc, argv);
+  // print HYPED logo at system startup
+  std::ifstream file("main_logo.txt");
+  if (file.is_open()) {
+    std::string buffer;
+    file >> buffer;
+    std::cout << buffer;
+  }
+
+  // Initalise the threads here
+  hyped::brakes::Main brakes;
+  hyped::navigation::Main navigation;
+  hyped::propulsion::Main propulsion;
+  hyped::sensors::Main sensors;
+  hyped::demo_state_machine::Main demo_state_machine;
+  hyped::telemetry::Main telemetry;
+
+  // Start the threads here
+  brakes.start();
+  navigation.start();
+  propulsion.start();
+  sensors.start();
+  demo_state_machine.start();
+  telemetry.start();
+
+  // Join the threads here
+  brakes.join();
+  navigation.join();
+  propulsion.join();
+  sensors.join();
+  demo_state_machine.join();
+  telemetry.join();
+
+  return 0;
+}

src/CMakeLists.txt

@@ -4,5 +4,6 @@ add_subdirectory(navigation)
 add_subdirectory(propulsion)
 add_subdirectory(sensors)
 add_subdirectory(state_machine)
+add_subdirectory(demo_state_machine)
 add_subdirectory(telemetry)
 add_subdirectory(utils)

src/brakes/main.cpp

@@ -121,6 +121,7 @@ void Main::run()
           f_brake_->checkAccFailure();
         }
         break;
+      case data::State::kPreReady:
       case data::State::kReady:
       case data::State::kFailureStopped:
         break;

src/data/data.hpp

@@ -163,6 +163,8 @@ std::optional<State> stateFromString(const std::string &state_name);
 struct StateMachine {
   bool critical_failure;
   State current_state;
+  static constexpr double kAccelerationTime = 0.1;  // seconds
+  std::chrono::steady_clock::time_point acceleration_start;
 };
 
 // -------------------------------------------------------------------------------------------------

src/demo_state_machine/CMakeLists.txt

@@ -0,0 +1,2 @@
+set(target "demo_state_machine")
+make_lib(${target} "${CMAKE_CURRENT_SOURCE_DIR}/..")

src/demo_state_machine/main.cpp

@@ -0,0 +1,40 @@
+#include "main.hpp"
+
+#include <cstdint>
+
+namespace hyped::demo_state_machine {
+
+Main::Main()
+    : utils::concurrent::Thread(utils::Logger(
+      "DEMO-STATE-MACHINE", utils::System::getSystem().config_.log_level_demo_state_machine))
+{
+  current_state_ = Idle::getInstance();
+}
+
+void Main::run()
+{
+  utils::System &sys = utils::System::getSystem();
+  data::Data &data   = data::Data::getInstance();
+
+  current_state_->enter(log_);
+
+  State *new_state;
+  while (sys.isRunning()) {
+    // checkTransition returns a new state or nullptr
+    if ((new_state = current_state_->checkTransition(log_))) {
+      current_state_->exit(log_);
+      current_state_ = new_state;
+      current_state_->enter(log_);
+    }
+
+    // Yielding because running the loop twice without any other thread being active
+    // will result in identical behaviour and thus waste resources.
+    yield();
+  }
+
+  data::StateMachine sm_data = data.getStateMachineData();
+  const auto state_string    = ::hyped::data::stateToString(sm_data.current_state);
+  log_.info("exiting. current state: %s", state_string->c_str());
+}
+
+}  // namespace hyped::demo_state_machine

src/demo_state_machine/main.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "state.hpp"
+
+#include <cstdint>
+
+#include <data/data.hpp>
+#include <demo_state_machine/state.hpp>
+#include <utils/concurrent/thread.hpp>
+#include <utils/system.hpp>
+
+namespace hyped::demo_state_machine {
+
+class Main : public utils::concurrent::Thread {
+ public:
+  Main();
+
+  /**
+   *  @brief  Runs state machine thread.
+   */
+  void run() override;
+
+  /*
+   * @brief  Current state of the pod
+   */
+  State *current_state_;
+};
+
+}  // namespace hyped::demo_state_machine