Program Listing for File diff_drive_fsm.hpp

↰ Return to documentation for file (cosm/kin2D/diff_drive_fsm.hpp)

#pragma once

/*******************************************************************************
 * Includes
 ******************************************************************************/
#include <string>
#include <utility>

#include "rcppsw/math/radians.hpp"
#include "rcppsw/math/vector2.hpp"
#include "rcppsw/patterns/fsm/simple_fsm.hpp"
#include "cosm/kin2D/config/diff_drive_config.hpp"

#include "cosm/cosm.hpp"
#include "cosm/kin/twist.hpp"

/*******************************************************************************
 * Namespaces/Decls
 ******************************************************************************/
namespace cosm::kin2D {

/*******************************************************************************
 * Class Definitions
 ******************************************************************************/
class diff_drive_fsm final : public rpfsm::simple_fsm {
 public:
  diff_drive_fsm(const ckin2D::config::diff_drive_config* config);

  /* move only constructible/assignable to work with the saa subsystem */
  diff_drive_fsm(diff_drive_fsm&&);
  diff_drive_fsm& operator=(diff_drive_fsm&&);

  /*
   * \brief Gets a direction vector as input and transforms it into wheel
   * speeds internally.
   *
   * \param delta_vel The difference from the robot's CURRENT heading
   *                  (i.e."change this much from the direction you are
   *                  currently going in") is computed according to \p old_vel.
   */
  void change_velocity(const ckin::twist& delta);

  const ckin::twist& configured_twist(void) const { return m_twist; }

 private:
  void configure_wheel_speeds(double speed1,
                              double speed2,
                              const rmath::radians& new_heading);
  void configure_twist(double speed, const rmath::radians& heading);

  /*
   * @enum The robot can be in three different turning states.
   */
  enum fsm_states {
    ekST_SOFT_TURN,
    ekST_HARD_TURN,
    ekST_MAX_STATES
  };

  struct turn_data final : public rpfsm::event_data {
    turn_data(double speed_in, const rmath::radians& angle_in)
        : speed(speed_in), angle(angle_in) {}

    double speed;
    rmath::radians angle;
  };

  RCPPSW_FSM_STATE_DECLARE(diff_drive_fsm, soft_turn, turn_data);

  RCPPSW_FSM_STATE_DECLARE(diff_drive_fsm, hard_turn, turn_data);

  RCPPSW_FSM_DEFINE_STATE_MAP_ACCESSOR(state_map, index) override {
    return &mc_state_map[index];
  }
  /* clang-format off */
  const config::diff_drive_config mc_config;

  ckin::twist                     m_twist{};

  RCPPSW_FSM_DECLARE_STATE_MAP(state_map,
                        mc_state_map,
                        ekST_MAX_STATES);
  /* clang-format on */
};

} /* namespace cosm::kin2D */