Dynamic Traction Demonstration State Machine

CMakeLists.txt

@@ -5,7 +5,7 @@ cmake_minimum_required(VERSION 3.12.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")
@@ -112,6 +112,7 @@ include_directories(
 # building all the libraries and tests
 add_subdirectory(${CMAKE_SOURCE_DIR}/src)
 
+
 if (CROSS)
 else()
   add_subdirectory(${CMAKE_SOURCE_DIR}/test)
@@ -137,6 +138,23 @@ target_link_libraries(${target}
     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
+)
+
 set(target "debugger")
 add_executable(${target} ${CMAKE_SOURCE_DIR}/run/debugger.cpp)
 target_link_libraries(${target}

configurations/demo/config.json

@@ -0,0 +1,61 @@
+{
+  "system": {
+    "log_level": 2,
+    "use_fake_trajectory": true,
+    "use_fake_batteries": true,
+    "use_fake_batteries_fail": false,
+    "use_fake_temperature": true,
+    "use_fake_temperature_fail": false,
+    "use_fake_pressure": true,
+    "use_fake_pressure_fail": false,
+    "use_fake_brakes": true,
+    "use_fake_controller": true,
+    "use_fake_high_power": true,
+    "use_fake_ambient_pressure" : true,
+    "use_fake_brake_pressure" : true,
+    "axis": 0
+  },
+  "brakes": {
+    "pins": [
+      {
+        "command_pin": 87,
+        "button_pin": 66
+      },
+      {
+        "command_pin": 10,
+        "button_pin": 69
+      }
+    ]
+  },
+  "sensors": {
+    "bms_startup_time_micros": 5000000,
+    "imu_pins": [22, 27, 47, 86],
+    "temperature_pins": [3, 4, 5, 6],
+    "ambient_pressure_pins": {
+      "pressure_pin": 10,
+      "temperature_pin": 11
+    }
+  },
+  "telemetry": {
+    "server_ip": "127.0.01",
+    "server_port": "9090"
+  },
+  "fake_trajectory": {
+    "maximum_acceleration": 1000.0,
+    "braking_deceleration": 2000.0,
+    "cruising_deceleration": 0.01
+  },
+  "fake_imu": [
+    {
+      "noise": 0.01
+    },
+    {
+      "noise": 0.02
+    },
+    {
+      "noise": 0.005
+    },
+    {
+      "noise": 0.001
+    } ]
+}

run/demo.cpp

@@ -0,0 +1,47 @@
+#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 <demo_state_machine/UI.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::propulsion::Main propulsion;
+  hyped::sensors::Main sensors;
+  hyped::demo_state_machine::Main demo_state_machine;
+
+  // Start the threads here
+  brakes.start();
+  propulsion.start();
+  sensors.start();
+  demo_state_machine.start();
+
+  hyped::demo_state_machine::Ui Ui; 
+  Ui.run(); 
+
+  // Join the threads here
+  brakes.join();
+  propulsion.join();
+  sensors.join();
+  demo_state_machine.join();
+
+  return 0;
+}

src/CMakeLists.txt

@@ -5,5 +5,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/data/data.hpp

@@ -174,6 +174,7 @@ std::optional<State> stateFromString(const std::string &state_name);
 struct StateMachine {
   bool critical_failure;
   State current_state;
+  static constexpr int64_t kAccelerationTime = 0.12e+06;  // microseconds
 };
 
 // -------------------------------------------------------------------------------------------------

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/UI.cpp

@@ -0,0 +1,138 @@
+#include "UI.hpp"
+
+#include <fstream>
+#include <iostream>
+#include <sstream>
+
+namespace hyped::demo_state_machine {
+
+Ui::Ui()
+    : system_(utils::System::getSystem()),
+      log_("UI", utils::System::getSystem().config_.log_level),
+      data_(data::Data::getInstance())
+{
+  addCalibrationCommand();
+  addLaunchCommand();
+  addBrakingCommand();
+  addHighPowerOffCommand();
+  addShutDownCommand();
+  addHelpCommand();
+  addQuitCommand();
+}
+
+void Ui::run()
+{
+  printAvailableCommands();
+  while (system_.isRunning()) {
+    readAndHandleCommand();
+  }
+}
+
+void Ui::printAvailableCommands()
+{
+  std::cout << "------------------------------" << std::endl;
+  std::cout << "Available options:" << std::endl;
+  for (auto &option : commands_) {
+    std::cout << " * `" << option.identifier;
+    std::cout << "' - " << option.description;
+    std::cout << std::endl;
+  }
+  std::cout << "------------------------------" << std::endl;
+}
+
+void Ui::readAndHandleCommand()
+{
+  const auto current_state = data_.getStateMachineData().current_state;
+  if (current_state == data::State::kPreCalibrating || current_state == data::State::kReady
+      || current_state == data::State::kAccelerating || current_state == data::State::kCruising
+      || current_state == data::State::kFinished) {
+    std::cout << "> ";
+    std::string command;
+    std::getline(std::cin, command);
+    auto identifier_and_handler = handlers_by_identifier_.find(command);
+    if (identifier_and_handler == handlers_by_identifier_.end()) {
+      std::cout << "Unknown command: " << command << std::endl;
+      return;
+    }
+    identifier_and_handler->second();
+  }
+}
+
+void Ui::addCommand(const Command &command)
+{
+  commands_.push_back(command);
+  handlers_by_identifier_.emplace(command.identifier, command.handler);
+}
+
+void Ui::addQuitCommand()
+{
+  addCommand({"quit", "Turn the system off", [this]() { system_.stop(); }});
+}
+
+void Ui::addHelpCommand()
+{
+  addCommand({"help", "Print all options", [this]() { printAvailableCommands(); }});
+}
+
+void Ui::giveCalibrationCommand()
+{
+  telemetry_data_                   = data_.getTelemetryData();
+  telemetry_data_.calibrate_command = true;
+  data_.setTelemetryData(telemetry_data_);
+}
+
+void Ui::addCalibrationCommand()
+{
+  addCommand(
+    {"calibrate", "Allows pod to enter Calibrating", [this]() { giveCalibrationCommand(); }});
+}
+
+void Ui::giveLaunchCommand()
+{
+  telemetry_data_                = data_.getTelemetryData();
+  telemetry_data_.launch_command = true;
+  data_.setTelemetryData(telemetry_data_);
+}
+
+void Ui::addLaunchCommand()
+{
+  addCommand({"launch", "Allows pod to enter Accelerating", [this]() { giveLaunchCommand(); }});
+}
+
+void Ui::giveBrakingCommand()
+{
+  telemetry_data_                        = data_.getTelemetryData();
+  telemetry_data_.emergency_stop_command = true;
+  data_.setTelemetryData(telemetry_data_);
+}
+
+void Ui::addBrakingCommand()
+{
+  addCommand({"brake", "Allows pod to enter Pre-Braking", [this]() { giveBrakingCommand(); }});
+}
+
+void Ui::giveHighPowerOffCommand()
+{
+  sensors_data_                = data_.getSensorsData();
+  sensors_data_.high_power_off = true;
+  data_.setSensorsData(sensors_data_);
+}
+
+void Ui::addHighPowerOffCommand()
+{
+  addCommand({"hpoff", "Allows pod to enter Nominal Braking", [this]() { giveHighPowerOffCommand(); }});
+}
+
+void Ui::giveShutDownCommand()
+{
+  telemetry_data_                  = data_.getTelemetryData();
+  telemetry_data_.shutdown_command = true;
+  data_.setTelemetryData(telemetry_data_);
+}
+
+void Ui::addShutDownCommand()
+{
+  addCommand({"shut", "Allows pod to turn off", [this]() { giveShutDownCommand(); }});
+}
+
+}  // namespace hyped::demo_state_machine

src/demo_state_machine/UI.hpp

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <atomic>
+#include <functional>
+#include <optional>
+#include <string>
+
+#include <data/data.hpp>
+#include <utils/logger.hpp>
+#include <utils/system.hpp>
+
+namespace hyped::demo_state_machine {
+
+class Ui {
+ public:
+  using Handler = std::function<void(void)>;
+  void printAvailableCommands();
+  void readAndHandleCommand();
+
+  struct Command {
+    std::string identifier;
+    std::string description;
+    Handler handler;
+  };
+
+  Ui();
+
+  void run();
+
+ private:
+  utils::System &system_;
+  utils::Logger log_;
+  data::Data &data_;
+  std::vector<Command> commands_;
+  std::unordered_map<std::string, Handler> handlers_by_identifier_;
+
+  data::Telemetry telemetry_data_;
+  data::Sensors sensors_data_;
+
+  void addCommand(const Command &command);
+  void addQuitCommand();
+  void addHelpCommand();
+  void addCalibrationCommand();
+  void addLaunchCommand();
+  void addBrakingCommand();
+  void addShutDownCommand();
+  void addHighPowerOffCommand();
+
+  void giveCalibrationCommand();
+  void giveLaunchCommand();
+  void giveBrakingCommand();
+  void giveHighPowerOffCommand(); 
+  void giveShutDownCommand();
+};
+
+}  // namespace hyped::demo_state_machine